diff --git a/.github/workflows/qurt_build.yml b/.github/workflows/qurt_build.yml index 5d303fa65aedeb..debf75c7f3db1e 100644 --- a/.github/workflows/qurt_build.yml +++ b/.github/workflows/qurt_build.yml @@ -141,8 +141,8 @@ concurrency: jobs: build: + if: github.repository == 'ArduPilot/ardupilot' runs-on: 'ardupilot-qurt' - steps: - uses: actions/checkout@v4 with: diff --git a/AntennaTracker/ReleaseNotes.txt b/AntennaTracker/ReleaseNotes.txt index 558ecfa5b57951..89982976f1caa0 100644 --- a/AntennaTracker/ReleaseNotes.txt +++ b/AntennaTracker/ReleaseNotes.txt @@ -1,5 +1,14 @@ Antenna Tracker Release Notes: ------------------------------- +------------------------------------------------------------------ +Release 4.5.5-beta2 27 July 2024 + +Changes from 4.5.5-beta1 + +1) Board specific enhancements and bug fixes + +- CubeRed's second core disabled at boot to avoid spurious writes to RAM +- CubeRed bootloader's dual endpoint update method fixed +------------------------------------------------------------------ Release 4.5.5-beta1 1st July 2024 Changes from 4.5.4 diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index c711b06f5197ee..a003f23756b8bf 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -341,10 +341,10 @@ bool AP_Arming_Copter::gps_checks(bool display_failure) { // check if fence requires GPS bool fence_requires_gps = false; - #if AP_FENCE_ENABLED +#if AP_FENCE_ENABLED // if circular or polygon fence is enabled we need GPS fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0; - #endif +#endif // check if flight mode requires GPS bool mode_requires_gps = copter.flightmode->requires_GPS() || fence_requires_gps || (copter.simple_mode == Copter::SimpleMode::SUPERSIMPLE); @@ -443,10 +443,10 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure) // check if fence requires GPS bool fence_requires_gps = false; - #if AP_FENCE_ENABLED +#if AP_FENCE_ENABLED // if circular or polygon fence is enabled we need GPS fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0; - #endif +#endif if (mode_requires_gps || copter.option_is_enabled(Copter::FlightOption::REQUIRE_POSITION_FOR_ARMING)) { if (!copter.position_ok()) { diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 7c9a0e91b3402f..bedc575a1f6b86 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -659,7 +659,7 @@ void Copter::three_hz_loop() // check for deadreckoning failsafe failsafe_deadreckon_check(); - // update ch6 in flight tuning + //update transmitter based in flight tuning tuning(); // check if avoidance should be enabled based on alt diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index 3cb3b99ea26080..c62f8b6d2bcd07 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -128,6 +128,7 @@ void RC_Channel_Copter::init_aux_function(const AUX_FUNC ch_option, const AuxSwi case AUX_FUNC::FORCEFLYING: case AUX_FUNC::CUSTOM_CONTROLLER: case AUX_FUNC::WEATHER_VANE_ENABLE: + case AUX_FUNC::TRANSMITTER_TUNING: run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT); break; default: @@ -648,6 +649,9 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc break; } #endif + case AUX_FUNC::TRANSMITTER_TUNING: + // do nothing, used in tuning.cpp for transmitter based tuning + break; default: return RC_Channel::do_aux_function(ch_option, ch_flag); diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 3e1504a0626b57..84d8c9f384f65e 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,14 @@ ArduPilot Copter Release Notes: -------------------------------- +------------------------------------------------------------------ +Release 4.5.5-beta2 27 July 2024 + +Changes from 4.5.5-beta1 + +1) Board specific enhancements and bug fixes + +- CubeRed's second core disabled at boot to avoid spurious writes to RAM +- CubeRed bootloader's dual endpoint update method fixed +------------------------------------------------------------------ Release 4.5.5-beta1 1st July 2024 Changes from 4.5.4 diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 19de3725dbbeba..3a851eee911595 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -439,6 +439,7 @@ void Copter::update_flight_mode() #if AP_RANGEFINDER_ENABLED surface_tracking.invalidate_for_logging(); // invalidate surface tracking alt, flight mode will set to true if used #endif + attitude_control->landed_gain_reduction(copter.ap.land_complete); // Adjust gains when landed to attenuate ground oscillation flightmode->run(); } diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index 8b2578d49b5683..5c1d243ed129e4 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -68,7 +68,7 @@ void ModeCircle::run() } // update the orbicular rate target based on pilot roll stick inputs - // skip if using CH6 tuning knob for circle rate + // skip if using transmitter based tuning knob for circle rate if (g.radio_tuning != TUNING_CIRCLE_RATE) { const float roll_stick = channel_roll->norm_input_dz(); // roll stick normalized -1 to 1 diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index c2cea3c72f2c38..a0bb8f7b829e90 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -1,33 +1,34 @@ #include "Copter.h" /* - * Function to update various parameters in flight using the ch6 tuning knob + * Function to update various parameters in flight using the TRANSMITTER_TUNING channel knob * This should not be confused with the AutoTune feature which can be found in control_autotune.cpp */ -// tuning - updates parameters based on the ch6 tuning knob's position +// tuning - updates parameters based on the ch6 TRANSMITTER_TUNING channel knob's position // should be called at 3.3hz void Copter::tuning() { - const RC_Channel *rc6 = rc().channel(CH_6); + const RC_Channel *rc_tuning = rc().find_channel_for_option(RC_Channel::AUX_FUNC::TRANSMITTER_TUNING); + // exit immediately if tuning channel is not set + if (rc_tuning == nullptr) { + return; + } + // exit immediately if the tuning function is not set or min and max are both zero if ((g.radio_tuning <= 0) || (is_zero(g2.tuning_min.get()) && is_zero(g2.tuning_max.get()))) { return; } // exit immediately when radio failsafe is invoked or transmitter has not been turned on - if (failsafe.radio || failsafe.radio_counter != 0 || rc6->get_radio_in() == 0) { - return; - } - - // exit immediately if a function is assigned to channel 6 - if ((RC_Channel::AUX_FUNC)rc6->option.get() != RC_Channel::AUX_FUNC::DO_NOTHING) { + if (failsafe.radio || failsafe.radio_counter != 0 || rc_tuning->get_radio_in() == 0) { return; } - const uint16_t radio_in = rc6->get_radio_in(); - float tuning_value = linear_interpolate(g2.tuning_min, g2.tuning_max, radio_in, rc6->get_radio_min(), rc6->get_radio_max()); + const uint16_t radio_in = rc_tuning->get_radio_in(); + float tuning_value = linear_interpolate(g2.tuning_min, g2.tuning_max, radio_in, rc_tuning->get_radio_min(), rc_tuning->get_radio_max()); + #if HAL_LOGGING_ENABLED Log_Write_Parameter_Tuning(g.radio_tuning, tuning_value, g2.tuning_min, g2.tuning_max); #endif diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 0996a50b725ab4..788fe97dbada6d 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -448,6 +448,10 @@ void Plane::stabilize() } +/* + * Set the throttle output. + * This is called by TECS-enabled flight modes, e.g. AUTO, GUIDED, etc. +*/ void Plane::calc_throttle() { if (aparm.throttle_cruise <= 1) { @@ -458,6 +462,7 @@ void Plane::calc_throttle() return; } + // Read the TECS throttle output and set it to the throttle channel. float commanded_throttle = TECS_controller.get_throttle_demand(); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, commanded_throttle); } diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 65198d9cb53240..cc6bc6e6ee626b 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -142,12 +142,28 @@ const AP_Param::Info Plane::var_info[] = { // @Param: TKOFF_THR_MAX_T // @DisplayName: Takeoff throttle maximum time - // @Description: This sets the time that maximum throttle will be forced during a fixed wing takeoff without an airspeed sensor. If an airspeed sensor is being used then the throttle is set to maximum until the takeoff airspeed is reached. + // @Description: This sets the time that maximum throttle will be forced during a fixed wing takeoff. // @Units: s // @Range: 0 10 // @Increment: 0.5 // @User: Standard ASCALAR(takeoff_throttle_max_t, "TKOFF_THR_MAX_T", 4), + + // @Param: TKOFF_THR_MIN + // @DisplayName: Takeoff minimum throttle + // @Description: The minimum throttle to use in takeoffs in AUTO and TAKEOFF flight modes, when TKOFF_OPTIONS bit 0 is set. Also, the minimum throttle to use in a quadpane forward transition. This can be useful to ensure faster takeoffs or transitions on aircraft where the normal throttle control leads to a slow takeoff or transition. It is used when it is larger than THR_MIN, otherwise THR_MIN is used instead. + // @Units: % + // @Range: 0 100 + // @Increment: 1 + // @User: Standard + ASCALAR(takeoff_throttle_min, "TKOFF_THR_MIN", 60), + + // @Param: TKOFF_OPTIONS + // @DisplayName: Takeoff options + // @Description: This selects the mode of the takeoff in AUTO and TAKEOFF flight modes. + // @Bitmask: 0: When unset the maximum allowed throttle is always used (THR_MAX or TKOFF_THR_MAX) during takeoff. When set TECS is allowed to operate between a minimum (THR_MIN or TKOFF_THR_MIN) and a maximum (THR_MAX or TKOFF_THR_MAX) limit. Applicable only when using an airspeed sensor. + // @User: Advanced + ASCALAR(takeoff_options, "TKOFF_OPTIONS", 0), // @Param: TKOFF_TDRAG_ELEV // @DisplayName: Takeoff tail dragger elevator @@ -1096,6 +1112,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Bitmask: 11: Disable suppression of fixed wing rate gains in ground mode // @Bitmask: 12: Enable FBWB style loiter altitude control // @Bitmask: 13: Indicate takeoff waiting for neutral rudder with flight control surfaces + // @Bitmask: 14: In AUTO - climb to next waypoint altitude immediately instead of linear climb // @User: Advanced AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0), diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index de514b891532cc..c06fcbafc0a077 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -357,6 +357,8 @@ class Parameters { k_param_acro_yaw_rate, k_param_takeoff_throttle_max_t, k_param_autotune_options, + k_param_takeoff_throttle_min, + k_param_takeoff_options, }; AP_Int16 format_version; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 443f2d56a1fb4b..10bca62c8a7b82 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1117,6 +1117,7 @@ class Plane : public AP_Vehicle { bool auto_takeoff_check(void); void takeoff_calc_roll(void); void takeoff_calc_pitch(void); + void takeoff_calc_throttle(const bool use_max_throttle=false); int8_t takeoff_tail_hold(void); int16_t get_takeoff_pitch_min_cd(void); void landing_gear_update(void); diff --git a/ArduPlane/ReleaseNotes.txt b/ArduPlane/ReleaseNotes.txt index 277136de6841ac..5d2677d74a85a5 100644 --- a/ArduPlane/ReleaseNotes.txt +++ b/ArduPlane/ReleaseNotes.txt @@ -1,5 +1,14 @@ ArduPilot Plane Release Notes: ------------------------------- +------------------------------------------------------------------ +Release 4.5.5-beta2 27 July 2024 + +Changes from 4.5.5-beta1 + +1) Board specific enhancements and bug fixes + +- CubeRed's second core disabled at boot to avoid spurious writes to RAM +- CubeRed bootloader's dual endpoint update method fixed +------------------------------------------------------------------ Release 4.5.5-beta1 1st July 2024 Changes from 4.5.4 diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 8b7970e6315d1a..5b845f38a86646 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -81,6 +81,13 @@ void Plane::setup_glide_slope(void) break; case Mode::Number::AUTO: + + //climb without doing glide slope if option is enabled + if (!above_location_current(next_WP_loc) && plane.flight_option_enabled(FlightOptions::IMMEDIATE_CLIMB_IN_AUTO)) { + reset_offset_altitude(); + break; + } + // we only do glide slide handling in AUTO when above 20m or // when descending. The 20 meter threshold is arbitrary, and // is basically to prevent situations where we try to slowly @@ -513,9 +520,9 @@ int32_t Plane::adjusted_altitude_cm(void) } /* - return home-relative altitude adjusted for ALT_OFFSET This is useful + return home-relative altitude adjusted for ALT_OFFSET. This is useful during long flights to account for barometer changes from the GCS, - or to adjust the flying height of a long mission + or to adjust the flying height of a long mission. */ int32_t Plane::adjusted_relative_altitude_cm(void) { diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 57969f17ce8a4b..a843901b34fb4b 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -376,7 +376,7 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd) next_WP_loc.lat = home.lat + 10; next_WP_loc.lng = home.lng + 10; auto_state.takeoff_speed_time_ms = 0; - auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction + auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction. auto_state.height_below_takeoff_to_level_off_cm = 0; // Flag also used to override "on the ground" throttle disable diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index f4a8f9fc45111e..b2939557adfdf5 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -168,6 +168,7 @@ enum FlightOptions { DISABLE_GROUND_PID_SUPPRESSION = (1<<11), ENABLE_LOITER_ALT_CONTROL = (1<<12), INDICATE_WAITING_FOR_RUDDER_NEUTRAL = (1<<13), + IMMEDIATE_CLIMB_IN_AUTO = (1<<14), }; enum CrowFlapOptions { diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 0854e512450a94..325e8cab2db2dc 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -22,11 +22,11 @@ const AP_Param::GroupInfo ModeTakeoff::var_info[] = { // @Increment: 1 // @Units: m // @User: Standard - AP_GROUPINFO("LVL_ALT", 2, ModeTakeoff, level_alt, 5), + AP_GROUPINFO("LVL_ALT", 2, ModeTakeoff, level_alt, 10), // @Param: LVL_PITCH // @DisplayName: Takeoff mode altitude initial pitch - // @Description: This is the target pitch for the initial climb to TKOFF_LVL_ALT + // @Description: This is the target pitch during the takeoff. // @Range: 0 30 // @Increment: 1 // @Units: deg @@ -149,11 +149,11 @@ void ModeTakeoff::update() if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) { //below TAKOFF_LVL_ALT - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100.0); plane.takeoff_calc_roll(); plane.takeoff_calc_pitch(); + plane.takeoff_calc_throttle(true); } else { - if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT ,or above and loitering + if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT, or above and loitering #if AP_FENCE_ENABLED if (!plane.have_autoenabled_fences) { plane.fence.auto_enable_fence_after_takeoff(); @@ -164,7 +164,7 @@ void ModeTakeoff::update() plane.calc_nav_pitch(); plane.calc_throttle(); } else { // still climbing to TAKEOFF_ALT; may be loitering - plane.calc_throttle(); + plane.takeoff_calc_throttle(); plane.takeoff_calc_roll(); plane.takeoff_calc_pitch(); } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index cc4d4dde104e38..ab3dd86ae25080 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -499,47 +499,73 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle) #endif // #if AP_BATTERY_WATT_MAX_ENABLED /* - Apply min/max limits to throttle + Apply min/max safety limits to throttle. */ float Plane::apply_throttle_limits(float throttle_in) { - // convert 0 to 100% (or -100 to +100) into PWM + // Pull the base throttle limits. + // These are usually set to map the ESC operating range. int8_t min_throttle = aparm.throttle_min.get(); int8_t max_throttle = aparm.throttle_max.get(); #if AP_ICENGINE_ENABLED - // apply idle governor + // Apply idle governor. g2.ice_control.update_idle_governor(min_throttle); #endif + // If reverse thrust is enabled not allowed right now, the minimum throttle must not fall below 0. if (min_throttle < 0 && !allow_reverse_thrust()) { // reverse thrust is available but inhibited. min_throttle = 0; } - const bool use_takeoff_throttle_max = + // Query the conditions where TKOFF_THR_MAX applies. + const bool use_takeoff_throttle = #if HAL_QUADPLANE_ENABLED quadplane.in_transition() || #endif (flight_stage == AP_FixedWing::FlightStage::TAKEOFF) || (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING); - if (use_takeoff_throttle_max) { + if (use_takeoff_throttle) { if (aparm.takeoff_throttle_max != 0) { + // Replace max throttle with the takeoff max throttle setting. + // This is typically done to protect against long intervals of large power draw. + // Or (in contrast) to give some extra throttle during the initial climb. max_throttle = aparm.takeoff_throttle_max.get(); } + // Do not allow min throttle to go below a lower threshold. + // This is typically done to protect against premature stalls close to the ground. + const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE); + if (!use_throttle_range || !ahrs.using_airspeed_sensor()) { + // Use a constant max throttle throughout the takeoff or when airspeed readings are not available. + min_throttle = MAX(min_throttle, aparm.takeoff_throttle_max.get()); + } else if (use_throttle_range) { // Use a throttle range through the takeoff. + if (aparm.takeoff_throttle_min.get() != 0) { // This is enabled by TKOFF_MODE==1. + min_throttle = MAX(min_throttle, aparm.takeoff_throttle_min.get()); + } + } } else if (landing.is_flaring()) { + // Allow throttle cutoff when flaring. + // This is to allow the aircraft to bleed speed faster and land with a shut off thruster. min_throttle = 0; } - // compensate for battery voltage drop + // Compensate the limits for battery voltage drop. + // This relaxes the limits when the battery is getting depleted. g2.fwd_batt_cmp.apply_min_max(min_throttle, max_throttle); #if AP_BATTERY_WATT_MAX_ENABLED - // apply watt limiter + // Ensure that the power draw limits are not exceeded. throttle_watt_limiter(min_throttle, max_throttle); #endif + // Do a sanity check on them. Constrain down if necessary. + min_throttle = MIN(min_throttle, max_throttle); + + // Let TECS know about the updated throttle limits. + TECS_controller.set_throttle_min(0.01f*min_throttle); + TECS_controller.set_throttle_max(0.01f*max_throttle); return constrain_float(throttle_in, min_throttle, max_throttle); } diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index faa220412fd914..d45651fbfe8853 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -219,7 +219,29 @@ void Plane::takeoff_calc_pitch(void) } /* - * get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off + * Set the throttle limits to run at during a takeoff. + */ +void Plane::takeoff_calc_throttle(const bool use_max_throttle) { + // This setting will take effect at the next run of TECS::update_pitch_throttle(). + + // Set the maximum throttle limit. + if (aparm.takeoff_throttle_max != 0) { + TECS_controller.set_throttle_max(0.01f*aparm.takeoff_throttle_max); + } + + // Set the minimum throttle limit. + const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE); + if (!use_throttle_range || !ahrs.using_airspeed_sensor() || use_max_throttle) { // Traditional takeoff throttle limit. + TECS_controller.set_throttle_min(0.01f*aparm.takeoff_throttle_max); + } else { // TKOFF_MODE == 1, allow for a throttle range. + if (aparm.takeoff_throttle_min != 0) { // Override THR_MIN. + TECS_controller.set_throttle_min(0.01f*aparm.takeoff_throttle_min); + } + } + calc_throttle(); +} + +/* get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off */ int16_t Plane::get_takeoff_pitch_min_cd(void) { diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index c89771e18c9a3d..a26dbd24ad37d9 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -483,6 +483,46 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_do_set_roi(const Location &roi_loc) return MAV_RESULT_ACCEPTED; } +MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_do_reposition(const mavlink_command_int_t &packet) +{ + const bool change_modes = ((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) == MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; + if (!sub.flightmode->in_guided_mode() && !change_modes) { + return MAV_RESULT_DENIED; + } + + // sanity check location + if (!check_latlng(packet.x, packet.y)) { + return MAV_RESULT_DENIED; + } + + Location request_location; + if (!location_from_command_t(packet, request_location)) { + return MAV_RESULT_DENIED; + } + + if (request_location.sanitize(sub.current_loc)) { + // if the location wasn't already sane don't load it + return MAV_RESULT_DENIED; // failed as the location is not valid + } + + // we need to do this first, as we don't want to change the flight mode unless we can also set the target + if (!sub.mode_guided.guided_set_destination(request_location)) { + return MAV_RESULT_FAILED; + } + + if (!sub.flightmode->in_guided_mode()) { + if (!sub.set_mode(Mode::Number::GUIDED, ModeReason::GCS_COMMAND)) { + return MAV_RESULT_FAILED; + } + // the position won't have been loaded if we had to change the flight mode, so load it again + if (!sub.mode_guided.guided_set_destination(request_location)) { + return MAV_RESULT_FAILED; + } + } + + return MAV_RESULT_ACCEPTED; +} + MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { switch(packet.command) { @@ -496,6 +536,9 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_ case MAV_CMD_DO_MOTOR_TEST: return handle_MAV_CMD_DO_MOTOR_TEST(packet); + case MAV_CMD_DO_REPOSITION: + return handle_command_int_do_reposition(packet); + case MAV_CMD_MISSION_START: return handle_MAV_CMD_MISSION_START(packet); diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index 4ca41e1d8c9e84..c38ec3f4abb6ef 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -22,6 +22,7 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK { MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; + MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet); // override sending of scaled_pressure3 to send on-board temperature: void send_scaled_pressure3() override; diff --git a/ArduSub/mode.h b/ArduSub/mode.h index 4228d0327343c7..11a64471675114 100644 --- a/ArduSub/mode.h +++ b/ArduSub/mode.h @@ -320,6 +320,7 @@ class ModeGuided : public Mode bool has_manual_throttle() const override { return false; } bool allows_arming(bool from_gcs) const override { return true; } bool is_autopilot() const override { return true; } + bool in_guided_mode() const override { return true; } bool guided_limit_check(); void guided_limit_init_time_and_pos(); void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads); diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index e718273703ce62..54bd15f4319d53 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -856,10 +856,14 @@ void GCS_MAVLINK_Rover::handle_set_position_target_local_ned(const mavlink_messa } // check for supported coordinate frames - if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED && - packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED && - packet.coordinate_frame != MAV_FRAME_BODY_NED && - packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) { + switch (packet.coordinate_frame) { + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_OFFSET_NED: + case MAV_FRAME_BODY_NED: + case MAV_FRAME_BODY_OFFSET_NED: + break; + + default: return; } @@ -975,14 +979,19 @@ void GCS_MAVLINK_Rover::handle_set_position_target_global_int(const mavlink_mess return; } // check for supported coordinate frames - if (packet.coordinate_frame != MAV_FRAME_GLOBAL && - packet.coordinate_frame != MAV_FRAME_GLOBAL_INT && - packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && - packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT && - packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT && - packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) { + switch (packet.coordinate_frame) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_INT: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + case MAV_FRAME_GLOBAL_TERRAIN_ALT: + case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT: + break; + + default: return; } + bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; diff --git a/Rover/ReleaseNotes.txt b/Rover/ReleaseNotes.txt index bfccbf5df6fba8..250c049f4caec3 100644 --- a/Rover/ReleaseNotes.txt +++ b/Rover/ReleaseNotes.txt @@ -1,5 +1,14 @@ Rover Release Notes: --------------------- +------------------------------------------------------------------ +Release 4.5.5-beta2 27 July 2024 + +Changes from 4.5.5-beta1 + +1) Board specific enhancements and bug fixes + +- CubeRed's second core disabled at boot to avoid spurious writes to RAM +- CubeRed bootloader's dual endpoint update method fixed +------------------------------------------------------------------ Release 4.5.5-beta1 1st July 2024 Changes from 4.5.4 diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index ed687c3241f95c..fa534addbff4c7 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -290,6 +290,8 @@ AP_HW_FlywooF405HD_AIOv2 1180 AP_HW_ESP32_PERIPH 1205 AP_HW_ESP32S3_PERIPH 1206 +AP_HW_CBUnmanned-CM405-FC 1301 + AP_HW_KHA_ETH 1315 AP_HW_CUBEORANGE_PERIPH 1400 diff --git a/Tools/AP_Periph/compass.cpp b/Tools/AP_Periph/compass.cpp index 2c70ae1cc04ffb..5a4cd5eb842af0 100644 --- a/Tools/AP_Periph/compass.cpp +++ b/Tools/AP_Periph/compass.cpp @@ -20,6 +20,8 @@ #define AP_PERIPH_MAG_HIRES 0 #endif +extern const AP_HAL::HAL &hal; + /* update CAN magnetometer */ diff --git a/Tools/GIT_Test/GIT_Success.txt b/Tools/GIT_Test/GIT_Success.txt index 3ef569921c66cc..91e375800da1b6 100644 --- a/Tools/GIT_Test/GIT_Success.txt +++ b/Tools/GIT_Test/GIT_Success.txt @@ -197,4 +197,5 @@ Kei Kabutomori Hiroaki Kawasaki Kazuhide Juta Ryoichi Shiohama -Masaki Shibuya \ No newline at end of file +Masaki Shibuya +Aryan Roshan trying his first commit \ No newline at end of file diff --git a/Tools/autotest/ArduPlane_Tests/TakeoffAuto1/catapult.txt b/Tools/autotest/ArduPlane_Tests/TakeoffAuto1/catapult.txt new file mode 100644 index 00000000000000..0aa9cd624db827 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffAuto1/catapult.txt @@ -0,0 +1,13 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 +6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 +11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/ArduPlane_Tests/TakeoffAuto2/catapult.txt b/Tools/autotest/ArduPlane_Tests/TakeoffAuto2/catapult.txt new file mode 100644 index 00000000000000..0aa9cd624db827 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffAuto2/catapult.txt @@ -0,0 +1,13 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 +6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 +11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/ArduPlane_Tests/TakeoffAuto3/catapult.txt b/Tools/autotest/ArduPlane_Tests/TakeoffAuto3/catapult.txt new file mode 100644 index 00000000000000..0aa9cd624db827 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffAuto3/catapult.txt @@ -0,0 +1,13 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 +6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 +11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/ArduPlane_Tests/TakeoffAuto4/catapult.txt b/Tools/autotest/ArduPlane_Tests/TakeoffAuto4/catapult.txt new file mode 100644 index 00000000000000..0aa9cd624db827 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffAuto4/catapult.txt @@ -0,0 +1,13 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 +6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 +11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidance/rover-path-planning-fence.txt b/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceAuto/rover-path-planning-fence.txt similarity index 100% rename from Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidance/rover-path-planning-fence.txt rename to Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceAuto/rover-path-planning-fence.txt diff --git a/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidance/rover-path-planning-mission.txt b/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceAuto/rover-path-planning-mission.txt similarity index 100% rename from Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidance/rover-path-planning-mission.txt rename to Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceAuto/rover-path-planning-mission.txt diff --git a/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasier/rover-path-bendyruler-fence.txt b/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierAuto/rover-path-bendyruler-fence.txt similarity index 100% rename from Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasier/rover-path-bendyruler-fence.txt rename to Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierAuto/rover-path-bendyruler-fence.txt diff --git a/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasier/rover-path-bendyruler-mission-easier.txt b/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierAuto/rover-path-bendyruler-mission-easier.txt similarity index 100% rename from Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasier/rover-path-bendyruler-mission-easier.txt rename to Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierAuto/rover-path-bendyruler-mission-easier.txt diff --git a/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierGuided/rover-path-bendyruler-fence.txt b/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierGuided/rover-path-bendyruler-fence.txt new file mode 100644 index 00000000000000..ace97c223c4a40 --- /dev/null +++ b/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierGuided/rover-path-bendyruler-fence.txt @@ -0,0 +1,11 @@ +QGC WPL 110 +0 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071766 -105.230202 0.000000 0 +1 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071014 -105.230247 0.000000 0 +2 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071014 -105.228821 0.000000 0 +3 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071609 -105.228867 0.000000 0 +4 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071602 -105.228172 0.000000 0 +5 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.070858 -105.227982 0.000000 0 +6 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.070789 -105.226219 0.000000 0 +7 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.072453 -105.226379 0.000000 0 +8 0 0 5004 20.000000 0.000000 0.000000 0.000000 40.071609 -105.228172 0.000000 0 +9 0 0 5004 20.000000 0.000000 0.000000 0.000000 40.071625 -105.227982 0.000000 0 diff --git a/Tools/autotest/antennatracker.py b/Tools/autotest/antennatracker.py index b186711637b6bd..3864f4c512e035 100644 --- a/Tools/autotest/antennatracker.py +++ b/Tools/autotest/antennatracker.py @@ -177,8 +177,6 @@ def disabled_tests(self): def GPSForYaw(self): '''Moving baseline GPS yaw''' - self.context_push() - self.load_default_params_file("tracker-gps-for-yaw.parm") self.reboot_sitl() @@ -195,10 +193,6 @@ def GPSForYaw(self): raise NotAchievedException("GPS_RAW not tracking simstate yaw") self.progress(f"yaw match ({gps_raw_hdg} vs {sim_hdg}") - self.context_pop() - - self.reboot_sitl() - def tests(self): '''return list of all tests''' ret = super(AutoTestTracker, self).tests() diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 17b500367509fc..7f70a37a0fe7a2 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -2472,10 +2472,10 @@ def OpticalFlow(self): '''test OpticalFlow in flight''' self.start_subtest("Make sure no crash if no rangefinder") - self.context_push() - - self.set_parameter("SIM_FLOW_ENABLE", 1) - self.set_parameter("FLOW_TYPE", 10) + self.set_parameters({ + "SIM_FLOW_ENABLE": 1, + "FLOW_TYPE": 10, + }) self.set_analog_rangefinder_parameters() @@ -2519,74 +2519,57 @@ def check_optical_flow(mav, m): self.fly_generic_mission("CMAC-copter-navtest.txt") - self.context_pop() - - self.reboot_sitl() - def OpticalFlowLimits(self): '''test EKF navigation limiting''' - ex = None - self.context_push() - try: - - self.set_parameter("SIM_FLOW_ENABLE", 1) - self.set_parameter("FLOW_TYPE", 10) - - self.configure_EKFs_to_use_optical_flow_instead_of_GPS() - - self.set_analog_rangefinder_parameters() - - self.set_parameter("SIM_GPS_DISABLE", 1) - self.set_parameter("SIM_TERRAIN", 0) + self.set_parameters({ + "SIM_FLOW_ENABLE": 1, + "FLOW_TYPE": 10, + "SIM_GPS_DISABLE": 1, + "SIM_TERRAIN": 0, + }) - self.reboot_sitl() + self.configure_EKFs_to_use_optical_flow_instead_of_GPS() - # we can't takeoff in loiter as we need flow healthy - self.takeoff(alt_min=5, mode='ALT_HOLD', require_absolute=False, takeoff_throttle=1800) - self.change_mode('LOITER') + self.set_analog_rangefinder_parameters() - # speed should be limited to <10m/s - self.set_rc(2, 1000) + self.reboot_sitl() - tstart = self.get_sim_time() - timeout = 60 - started_climb = False - while self.get_sim_time_cached() - tstart < timeout: - m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) - spd = math.sqrt(m.vx**2 + m.vy**2) * 0.01 - alt = m.relative_alt*0.001 - - # calculate max speed from altitude above the ground - margin = 2.0 - max_speed = alt * 1.5 + margin - self.progress("%0.1f: Low Speed: %f (want <= %u) alt=%.1f" % - (self.get_sim_time_cached() - tstart, - spd, - max_speed, alt)) - if spd > max_speed: - raise NotAchievedException(("Speed should be limited by" - "EKF optical flow limits")) - - # after 30 seconds start climbing - if not started_climb and self.get_sim_time_cached() - tstart > 30: - started_climb = True - self.set_rc(3, 1900) - self.progress("Moving higher") - - # check altitude is not climbing above 35m - if alt > 35: - raise NotAchievedException("Alt should be limited by EKF optical flow limits") + # we can't takeoff in loiter as we need flow healthy + self.takeoff(alt_min=5, mode='ALT_HOLD', require_absolute=False, takeoff_throttle=1800) + self.change_mode('LOITER') - except Exception as e: - self.print_exception_caught(e) - ex = e + # speed should be limited to <10m/s + self.set_rc(2, 1000) - self.set_rc(2, 1500) + tstart = self.get_sim_time() + timeout = 60 + started_climb = False + while self.get_sim_time_cached() - tstart < timeout: + m = self.assert_receive_message('GLOBAL_POSITION_INT') + spd = math.sqrt(m.vx**2 + m.vy**2) * 0.01 + alt = m.relative_alt*0.001 + + # calculate max speed from altitude above the ground + margin = 2.0 + max_speed = alt * 1.5 + margin + self.progress("%0.1f: Low Speed: %f (want <= %u) alt=%.1f" % + (self.get_sim_time_cached() - tstart, + spd, + max_speed, alt)) + if spd > max_speed: + raise NotAchievedException(("Speed should be limited by" + "EKF optical flow limits")) + + # after 30 seconds start climbing + if not started_climb and self.get_sim_time_cached() - tstart > 30: + started_climb = True + self.set_rc(3, 1900) + self.progress("Moving higher") + + # check altitude is not climbing above 35m + if alt > 35: + raise NotAchievedException("Alt should be limited by EKF optical flow limits") self.reboot_sitl(force=True) - self.context_pop() - - if ex is not None: - raise ex def OpticalFlowCalibration(self): '''test optical flow calibration''' @@ -3259,122 +3242,109 @@ def GuidedEKFLaneChange(self): self.disarm_vehicle(force=True) self.reboot_sitl() - def MotorFail(self, fail_servo=0, fail_mul=0.0, holdtime=30): + def MotorFail(self, ): """Test flight with reduced motor efficiency""" - # we only expect an octocopter to survive ATM: - servo_counts = { - # 2: 6, # hexa - 3: 8, # octa - # 5: 6, # Y6 - } - frame_class = int(self.get_parameter("FRAME_CLASS")) - if frame_class not in servo_counts: - self.progress("Test not relevant for frame_class %u" % frame_class) - return - - servo_count = servo_counts[frame_class] + self.MotorFail_test_frame('octa', 8, frame_class=3) + # self.MotorFail_test_frame('hexa', 6, frame_class=2) + # self.MotorFail_test_frame('y6', 6, frame_class=5) - if fail_servo < 0 or fail_servo > servo_count: - raise ValueError('fail_servo outside range for frame class') - - self.takeoff(10, mode="LOITER") + def MotorFail_test_frame(self, model, servo_count, frame_class, fail_servo=0, fail_mul=0.0, holdtime=30): + self.set_parameters({ + 'FRAME_CLASS': frame_class, + }) + self.customise_SITL_commandline([], model=model) - self.change_alt(alt_min=50) + self.takeoff(25, mode="LOITER") # Get initial values - start_hud = self.mav.recv_match(type='VFR_HUD', blocking=True) - start_attitude = self.mav.recv_match(type='ATTITUDE', blocking=True) + start_hud = self.assert_receive_message('VFR_HUD') + start_attitude = self.assert_receive_message('ATTITUDE') hover_time = 5 - try: - tstart = self.get_sim_time() - int_error_alt = 0 - int_error_yaw_rate = 0 - int_error_yaw = 0 - self.progress("Hovering for %u seconds" % hover_time) - failed = False - while True: - now = self.get_sim_time_cached() - if now - tstart > holdtime + hover_time: - break + tstart = self.get_sim_time() + int_error_alt = 0 + int_error_yaw_rate = 0 + int_error_yaw = 0 + self.progress("Hovering for %u seconds" % hover_time) + failed = False + while True: + now = self.get_sim_time_cached() + if now - tstart > holdtime + hover_time: + break - servo = self.mav.recv_match(type='SERVO_OUTPUT_RAW', - blocking=True) - hud = self.mav.recv_match(type='VFR_HUD', blocking=True) - attitude = self.mav.recv_match(type='ATTITUDE', blocking=True) + servo = self.assert_receive_message('SERVO_OUTPUT_RAW') + hud = self.assert_receive_message('VFR_HUD') + attitude = self.assert_receive_message('ATTITUDE') - if not failed and now - tstart > hover_time: - self.progress("Killing motor %u (%u%%)" % - (fail_servo+1, fail_mul)) - self.set_parameters({ - "SIM_ENGINE_FAIL": fail_servo, - "SIM_ENGINE_MUL": fail_mul, - }) - failed = True + if not failed and now - tstart > hover_time: + self.progress("Killing motor %u (%u%%)" % + (fail_servo+1, fail_mul)) + self.set_parameters({ + "SIM_ENGINE_FAIL": fail_servo, + "SIM_ENGINE_MUL": fail_mul, + }) + failed = True - if failed: - self.progress("Hold Time: %f/%f" % (now-tstart, holdtime)) - - servo_pwm = [servo.servo1_raw, - servo.servo2_raw, - servo.servo3_raw, - servo.servo4_raw, - servo.servo5_raw, - servo.servo6_raw, - servo.servo7_raw, - servo.servo8_raw] - - self.progress("PWM output per motor") - for i, pwm in enumerate(servo_pwm[0:servo_count]): - if pwm > 1900: - state = "oversaturated" - elif pwm < 1200: - state = "undersaturated" - else: - state = "OK" + if failed: + self.progress("Hold Time: %f/%f" % (now-tstart, holdtime)) + + servo_pwm = [ + servo.servo1_raw, + servo.servo2_raw, + servo.servo3_raw, + servo.servo4_raw, + servo.servo5_raw, + servo.servo6_raw, + servo.servo7_raw, + servo.servo8_raw, + ] - if failed and i == fail_servo: - state += " (failed)" + self.progress("PWM output per motor") + for i, pwm in enumerate(servo_pwm[0:servo_count]): + if pwm > 1900: + state = "oversaturated" + elif pwm < 1200: + state = "undersaturated" + else: + state = "OK" - self.progress("servo %u [pwm=%u] [%s]" % (i+1, pwm, state)) + if failed and i == fail_servo: + state += " (failed)" - alt_delta = hud.alt - start_hud.alt - yawrate_delta = attitude.yawspeed - start_attitude.yawspeed - yaw_delta = attitude.yaw - start_attitude.yaw + self.progress("servo %u [pwm=%u] [%s]" % (i+1, pwm, state)) - self.progress("Alt=%fm (delta=%fm)" % (hud.alt, alt_delta)) - self.progress("Yaw rate=%f (delta=%f) (rad/s)" % - (attitude.yawspeed, yawrate_delta)) - self.progress("Yaw=%f (delta=%f) (deg)" % - (attitude.yaw, yaw_delta)) + alt_delta = hud.alt - start_hud.alt + yawrate_delta = attitude.yawspeed - start_attitude.yawspeed + yaw_delta = attitude.yaw - start_attitude.yaw - dt = self.get_sim_time() - now - int_error_alt += abs(alt_delta/dt) - int_error_yaw_rate += abs(yawrate_delta/dt) - int_error_yaw += abs(yaw_delta/dt) - self.progress("## Error Integration ##") - self.progress(" Altitude: %fm" % int_error_alt) - self.progress(" Yaw rate: %f rad/s" % int_error_yaw_rate) - self.progress(" Yaw: %f deg" % int_error_yaw) - self.progress("----") + self.progress("Alt=%fm (delta=%fm)" % (hud.alt, alt_delta)) + self.progress("Yaw rate=%f (delta=%f) (rad/s)" % + (attitude.yawspeed, yawrate_delta)) + self.progress("Yaw=%f (delta=%f) (deg)" % + (attitude.yaw, yaw_delta)) - if int_error_yaw_rate > 0.1: - raise NotAchievedException("Vehicle is spinning") + dt = self.get_sim_time() - now + int_error_alt += abs(alt_delta/dt) + int_error_yaw_rate += abs(yawrate_delta/dt) + int_error_yaw += abs(yaw_delta/dt) + self.progress("## Error Integration ##") + self.progress(" Altitude: %fm" % int_error_alt) + self.progress(" Yaw rate: %f rad/s" % int_error_yaw_rate) + self.progress(" Yaw: %f deg" % int_error_yaw) + self.progress("----") - if alt_delta < -20: - raise NotAchievedException("Vehicle is descending") + if int_error_yaw > 5: + raise NotAchievedException("Vehicle is spinning") - self.set_parameters({ - "SIM_ENGINE_FAIL": 0, - "SIM_ENGINE_MUL": 1.0, - }) - except Exception as e: - self.set_parameters({ - "SIM_ENGINE_FAIL": 0, - "SIM_ENGINE_MUL": 1.0, - }) - raise e + if alt_delta < -20: + raise NotAchievedException("Vehicle is descending") + + self.progress("Fixing motors") + self.set_parameters({ + "SIM_ENGINE_FAIL": 0, + "SIM_ENGINE_MUL": 1.0, + }) self.do_RTL() @@ -3392,83 +3362,67 @@ def hover_for_interval(self, hover_time): def MotorVibration(self): """Test flight with motor vibration""" - self.context_push() - - ex = None - try: - self.set_rc_default() - # magic tridge EKF type that dramatically speeds up the test - self.set_parameters({ - "AHRS_EKF_TYPE": 10, - "INS_LOG_BAT_MASK": 3, - "INS_LOG_BAT_OPT": 0, - "LOG_BITMASK": 958, - "LOG_DISARMED": 0, - "SIM_VIB_MOT_MAX": 350, - # these are real values taken from a 180mm Quad: - "SIM_GYR1_RND": 20, - "SIM_ACC1_RND": 5, - "SIM_ACC2_RND": 5, - "SIM_INS_THR_MIN": 0.1, - }) - self.reboot_sitl() - - # do a simple up-and-down flight to gather data: - self.takeoff(15, mode="ALT_HOLD") - tstart, tend, hover_throttle = self.hover_for_interval(15) - # if we don't reduce vibes here then the landing detector - # may not trigger - self.set_parameter("SIM_VIB_MOT_MAX", 0) - self.do_RTL() - - psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) - # ignore the first 20Hz and look for a peak at -15dB or more - # it should be at about 190Hz, each bin is 1000/1024Hz wide - ignore_bins = int(100 * 1.024) # start at 100Hz to be safe - freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins] - if numpy.amax(psd["X"][ignore_bins:]) < -15 or freq < 100 or freq > 300: - raise NotAchievedException( - "Did not detect a motor peak, found %f at %f dB" % - (freq, numpy.amax(psd["X"][ignore_bins:]))) - else: - self.progress("Detected motor peak at %fHz" % freq) - - # now add a notch and check that post-filter the peak is squashed below 40dB - self.set_parameters({ - "INS_LOG_BAT_OPT": 2, - "INS_HNTC2_ENABLE": 1, - "INS_HNTC2_FREQ": freq, - "INS_HNTC2_ATT": 50, - "INS_HNTC2_BW": freq/2, - "INS_HNTC2_MODE": 0, - "SIM_VIB_MOT_MAX": 350, - }) - self.reboot_sitl() - - # do a simple up-and-down flight to gather data: - self.takeoff(15, mode="ALT_HOLD") - tstart, tend, hover_throttle = self.hover_for_interval(15) - self.set_parameter("SIM_VIB_MOT_MAX", 0) - self.do_RTL() + # magic tridge EKF type that dramatically speeds up the test + self.set_parameters({ + "AHRS_EKF_TYPE": 10, + "INS_LOG_BAT_MASK": 3, + "INS_LOG_BAT_OPT": 0, + "LOG_BITMASK": 958, + "LOG_DISARMED": 0, + "SIM_VIB_MOT_MAX": 350, + # these are real values taken from a 180mm Quad: + "SIM_GYR1_RND": 20, + "SIM_ACC1_RND": 5, + "SIM_ACC2_RND": 5, + "SIM_INS_THR_MIN": 0.1, + }) + self.reboot_sitl() - psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) - freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins] - peakdB = numpy.amax(psd["X"][ignore_bins:]) - if peakdB < -23: - self.progress("Did not detect a motor peak, found %f at %f dB" % (freq, peakdB)) - else: - raise NotAchievedException("Detected peak %.1f Hz %.2f dB" % (freq, peakdB)) - except Exception as e: - self.print_exception_caught(e) - ex = e - self.disarm_vehicle(force=True) + # do a simple up-and-down flight to gather data: + self.takeoff(15, mode="ALT_HOLD") + tstart, tend, hover_throttle = self.hover_for_interval(15) + # if we don't reduce vibes here then the landing detector + # may not trigger + self.set_parameter("SIM_VIB_MOT_MAX", 0) + self.do_RTL() - self.context_pop() + psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) + # ignore the first 20Hz and look for a peak at -15dB or more + # it should be at about 190Hz, each bin is 1000/1024Hz wide + ignore_bins = int(100 * 1.024) # start at 100Hz to be safe + freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins] + if numpy.amax(psd["X"][ignore_bins:]) < -15 or freq < 100 or freq > 300: + raise NotAchievedException( + "Did not detect a motor peak, found %f at %f dB" % + (freq, numpy.amax(psd["X"][ignore_bins:]))) + else: + self.progress("Detected motor peak at %fHz" % freq) + # now add a notch and check that post-filter the peak is squashed below 40dB + self.set_parameters({ + "INS_LOG_BAT_OPT": 2, + "INS_HNTC2_ENABLE": 1, + "INS_HNTC2_FREQ": freq, + "INS_HNTC2_ATT": 50, + "INS_HNTC2_BW": freq/2, + "INS_HNTC2_MODE": 0, + "SIM_VIB_MOT_MAX": 350, + }) self.reboot_sitl() - if ex is not None: - raise ex + # do a simple up-and-down flight to gather data: + self.takeoff(15, mode="ALT_HOLD") + tstart, tend, hover_throttle = self.hover_for_interval(15) + self.set_parameter("SIM_VIB_MOT_MAX", 0) + self.do_RTL() + + psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) + freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins] + peakdB = numpy.amax(psd["X"][ignore_bins:]) + if peakdB < -23: + self.progress("Did not detect a motor peak, found %f at %f dB" % (freq, peakdB)) + else: + raise NotAchievedException("Detected peak %.1f Hz %.2f dB" % (freq, peakdB)) def VisionPosition(self): """Disable GPS navigation, enable Vicon input.""" @@ -3482,91 +3436,62 @@ def VisionPosition(self): old_pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) print("old_pos=%s" % str(old_pos)) - self.context_push() - - ex = None - try: - # configure EKF to use external nav instead of GPS - ahrs_ekf_type = self.get_parameter("AHRS_EKF_TYPE") - if ahrs_ekf_type == 2: - self.set_parameter("EK2_GPS_TYPE", 3) - if ahrs_ekf_type == 3: - self.set_parameters({ - "EK3_SRC1_POSXY": 6, - "EK3_SRC1_VELXY": 6, - "EK3_SRC1_POSZ": 6, - "EK3_SRC1_VELZ": 6, - }) + # configure EKF to use external nav instead of GPS + ahrs_ekf_type = self.get_parameter("AHRS_EKF_TYPE") + if ahrs_ekf_type == 2: + self.set_parameter("EK2_GPS_TYPE", 3) + if ahrs_ekf_type == 3: self.set_parameters({ - "GPS1_TYPE": 0, - "VISO_TYPE": 1, - "SERIAL5_PROTOCOL": 1, + "EK3_SRC1_POSXY": 6, + "EK3_SRC1_VELXY": 6, + "EK3_SRC1_POSZ": 6, + "EK3_SRC1_VELZ": 6, }) - self.reboot_sitl() - # without a GPS or some sort of external prompting, AP - # doesn't send system_time messages. So prompt it: - self.mav.mav.system_time_send(int(time.time() * 1000000), 0) - self.progress("Waiting for non-zero-lat") - tstart = self.get_sim_time() - while True: - self.mav.mav.set_gps_global_origin_send(1, - old_pos.lat, - old_pos.lon, - old_pos.alt) - gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT', - blocking=True) - self.progress("gpi=%s" % str(gpi)) - if gpi.lat != 0: - break - - if self.get_sim_time_cached() - tstart > 60: - raise AutoTestTimeoutException("Did not get non-zero lat") - - self.takeoff() - self.set_rc(1, 1600) - tstart = self.get_sim_time() - while True: - vicon_pos = self.mav.recv_match(type='VISION_POSITION_ESTIMATE', - blocking=True) - # print("vpe=%s" % str(vicon_pos)) - self.mav.recv_match(type='GLOBAL_POSITION_INT', - blocking=True) - # self.progress("gpi=%s" % str(gpi)) - if vicon_pos.x > 40: - break - - if self.get_sim_time_cached() - tstart > 100: - raise AutoTestTimeoutException("Vicon showed no movement") - - # recenter controls: - self.set_rc(1, 1500) - self.progress("# Enter RTL") - self.change_mode('RTL') - self.set_rc(3, 1500) - tstart = self.get_sim_time() - while True: - if self.get_sim_time_cached() - tstart > 200: - raise NotAchievedException("Did not disarm") - self.mav.recv_match(type='GLOBAL_POSITION_INT', - blocking=True) - # print("gpi=%s" % str(gpi)) - self.mav.recv_match(type='SIMSTATE', - blocking=True) - # print("ss=%s" % str(ss)) - # wait for RTL disarm: - if not self.armed(): - break + self.set_parameters({ + "GPS1_TYPE": 0, + "VISO_TYPE": 1, + "SERIAL5_PROTOCOL": 1, + }) + self.reboot_sitl() + # without a GPS or some sort of external prompting, AP + # doesn't send system_time messages. So prompt it: + self.mav.mav.system_time_send(int(time.time() * 1000000), 0) + self.progress("Waiting for non-zero-lat") + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > 60: + raise AutoTestTimeoutException("Did not get non-zero lat") + self.mav.mav.set_gps_global_origin_send(1, + old_pos.lat, + old_pos.lon, + old_pos.alt) + gpi = self.assert_receive_message('GLOBAL_POSITION_INT') + self.progress("gpi=%s" % str(gpi)) + if gpi.lat != 0: + break - except Exception as e: - self.print_exception_caught(e) - ex = e + self.takeoff() + self.set_rc(1, 1600) + tstart = self.get_sim_time() + while True: + vicon_pos = self.assert_receive_message('VISION_POSITION_ESTIMATE') + # print("vpe=%s" % str(vicon_pos)) + # gpi = self.assert_receive_message('GLOBAL_POSITION_INT') + # self.progress("gpi=%s" % str(gpi)) + if vicon_pos.x > 40: + break - self.context_pop() - self.zero_throttle() - self.reboot_sitl() + if self.get_sim_time_cached() - tstart > 100: + raise AutoTestTimeoutException("Vicon showed no movement") - if ex is not None: - raise ex + # recenter controls: + self.set_rc(1, 1500) + self.progress("# Enter RTL") + self.change_mode('RTL') + self.set_rc(3, 1500) + tstart = self.get_sim_time() + # self.install_messageprinter_handlers_context(['SIMSTATE', 'GLOBAL_POSITION_INT']) + self.wait_disarmed(timeout=200) def BodyFrameOdom(self): """Disable GPS navigation, enable input of VISION_POSITION_DELTA.""" @@ -4682,75 +4607,52 @@ def ModeZigZag(self): def SetModesViaModeSwitch(self): '''Set modes via modeswitch''' - self.context_push() - ex = None - try: - fltmode_ch = 5 - self.set_parameter("FLTMODE_CH", fltmode_ch) - self.set_rc(fltmode_ch, 1000) # PWM for mode1 - testmodes = [("FLTMODE1", 4, "GUIDED", 1165), - ("FLTMODE2", 2, "ALT_HOLD", 1295), - ("FLTMODE3", 6, "RTL", 1425), - ("FLTMODE4", 7, "CIRCLE", 1555), - ("FLTMODE5", 1, "ACRO", 1685), - ("FLTMODE6", 17, "BRAKE", 1815), - ] - for mode in testmodes: - (parm, parm_value, name, pwm) = mode - self.set_parameter(parm, parm_value) - - for mode in reversed(testmodes): - (parm, parm_value, name, pwm) = mode - self.set_rc(fltmode_ch, pwm) - self.wait_mode(name) - - for mode in testmodes: - (parm, parm_value, name, pwm) = mode - self.set_rc(fltmode_ch, pwm) - self.wait_mode(name) - - for mode in reversed(testmodes): - (parm, parm_value, name, pwm) = mode - self.set_rc(fltmode_ch, pwm) - self.wait_mode(name) - - except Exception as e: - self.print_exception_caught(e) - ex = e - - self.context_pop() - - if ex is not None: - raise ex + fltmode_ch = 5 + self.set_parameter("FLTMODE_CH", fltmode_ch) + self.set_rc(fltmode_ch, 1000) # PWM for mode1 + testmodes = [("FLTMODE1", 4, "GUIDED", 1165), + ("FLTMODE2", 2, "ALT_HOLD", 1295), + ("FLTMODE3", 6, "RTL", 1425), + ("FLTMODE4", 7, "CIRCLE", 1555), + ("FLTMODE5", 1, "ACRO", 1685), + ("FLTMODE6", 17, "BRAKE", 1815), + ] + for mode in testmodes: + (parm, parm_value, name, pwm) = mode + self.set_parameter(parm, parm_value) + + for mode in reversed(testmodes): + (parm, parm_value, name, pwm) = mode + self.set_rc(fltmode_ch, pwm) + self.wait_mode(name) + + for mode in testmodes: + (parm, parm_value, name, pwm) = mode + self.set_rc(fltmode_ch, pwm) + self.wait_mode(name) + + for mode in reversed(testmodes): + (parm, parm_value, name, pwm) = mode + self.set_rc(fltmode_ch, pwm) + self.wait_mode(name) def SetModesViaAuxSwitch(self): '''"Set modes via auxswitch"''' - self.context_push() - ex = None - try: - fltmode_ch = int(self.get_parameter("FLTMODE_CH")) - self.set_rc(fltmode_ch, 1000) - self.wait_mode("CIRCLE") - self.set_rc(9, 1000) - self.set_rc(10, 1000) - self.set_parameters({ - "RC9_OPTION": 18, # land - "RC10_OPTION": 55, # guided - }) - self.set_rc(9, 1900) - self.wait_mode("LAND") - self.set_rc(10, 1900) - self.wait_mode("GUIDED") - self.set_rc(10, 1000) # this re-polls the mode switch - self.wait_mode("CIRCLE") - except Exception as e: - self.print_exception_caught(e) - ex = e - - self.context_pop() - - if ex is not None: - raise ex + fltmode_ch = int(self.get_parameter("FLTMODE_CH")) + self.set_rc(fltmode_ch, 1000) + self.wait_mode("CIRCLE") + self.set_rc(9, 1000) + self.set_rc(10, 1000) + self.set_parameters({ + "RC9_OPTION": 18, # land + "RC10_OPTION": 55, # guided + }) + self.set_rc(9, 1900) + self.wait_mode("LAND") + self.set_rc(10, 1900) + self.wait_mode("GUIDED") + self.set_rc(10, 1000) # this re-polls the mode switch + self.wait_mode("CIRCLE") def fly_guided_stop(self, timeout=20, @@ -5294,24 +5196,14 @@ def TestGripperMission(self): def SplineLastWaypoint(self): '''Test Spline as last waypoint''' - self.context_push() - ex = None - try: - self.load_mission("copter-spline-last-waypoint.txt") - self.change_mode('LOITER') - self.wait_ready_to_arm() - self.arm_vehicle() - self.change_mode('AUTO') - self.set_rc(3, 1500) - self.wait_altitude(10, 3000, relative=True) - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() + self.load_mission("copter-spline-last-waypoint.txt") + self.change_mode('LOITER') + self.wait_ready_to_arm() + self.arm_vehicle() + self.change_mode('AUTO') + self.set_rc(3, 1500) + self.wait_altitude(10, 3000, relative=True) self.do_RTL() - self.wait_disarmed() - if ex is not None: - raise ex def ManualThrottleModeChange(self): '''Check manual throttle mode changes denied on high throttle''' @@ -6420,90 +6312,60 @@ def extract_median_FTN1_PkAvg_from_current_onboard_log(self, tstart, tend): def PIDNotches(self): """Use dynamic harmonic notch to control motor noise.""" self.progress("Flying with PID notches") - self.context_push() - - ex = None - try: - self.set_parameters({ - "FILT1_TYPE": 1, - "AHRS_EKF_TYPE": 10, - "INS_LOG_BAT_MASK": 3, - "INS_LOG_BAT_OPT": 0, - "INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour - "LOG_BITMASK": 65535, - "LOG_DISARMED": 0, - "SIM_VIB_FREQ_X": 120, # roll - "SIM_VIB_FREQ_Y": 120, # pitch - "SIM_VIB_FREQ_Z": 180, # yaw - "FILT1_NOTCH_FREQ": 120, - "ATC_RAT_RLL_NEF": 1, - "ATC_RAT_PIT_NEF": 1, - "ATC_RAT_YAW_NEF": 1, - "SIM_GYR1_RND": 5, - }) - self.reboot_sitl() - - self.takeoff(10, mode="ALT_HOLD") - - freq, hover_throttle, peakdb1 = self.hover_and_check_matched_frequency_with_fft(5, 20, 350, reverse=True) - - except Exception as e: - self.print_exception_caught(e) - ex = e - - self.context_pop() + self.set_parameters({ + "FILT1_TYPE": 1, + "AHRS_EKF_TYPE": 10, + "INS_LOG_BAT_MASK": 3, + "INS_LOG_BAT_OPT": 0, + "INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour + "LOG_BITMASK": 65535, + "LOG_DISARMED": 0, + "SIM_VIB_FREQ_X": 120, # roll + "SIM_VIB_FREQ_Y": 120, # pitch + "SIM_VIB_FREQ_Z": 180, # yaw + "FILT1_NOTCH_FREQ": 120, + "ATC_RAT_RLL_NEF": 1, + "ATC_RAT_PIT_NEF": 1, + "ATC_RAT_YAW_NEF": 1, + "SIM_GYR1_RND": 5, + }) + self.reboot_sitl() - if ex is not None: - raise ex + self.hover_and_check_matched_frequency_with_fft(dblevel=5, minhz=20, maxhz=350, reverse=True) def ThrottleGainBoost(self): """Use PD and Angle P boost for anti-gravity.""" # basic gyro sample rate test self.progress("Flying with Throttle-Gain Boost") - self.context_push() - - ex = None - try: - # magic tridge EKF type that dramatically speeds up the test - self.set_parameters({ - "AHRS_EKF_TYPE": 10, - "EK2_ENABLE": 0, - "EK3_ENABLE": 0, - "INS_FAST_SAMPLE": 0, - "LOG_BITMASK": 959, - "LOG_DISARMED": 0, - "ATC_THR_G_BOOST": 5.0, - }) - - self.reboot_sitl() - - self.takeoff(10, mode="ALT_HOLD") - hover_time = 15 - self.progress("Hovering for %u seconds" % hover_time) - tstart = self.get_sim_time() - while self.get_sim_time_cached() < tstart + hover_time: - self.mav.recv_match(type='ATTITUDE', blocking=True) - - # fly fast forrest! - self.set_rc(3, 1900) - self.set_rc(2, 1200) - self.wait_groundspeed(5, 1000) - self.set_rc(3, 1500) - self.set_rc(2, 1500) - self.do_RTL() + # magic tridge EKF type that dramatically speeds up the test + self.set_parameters({ + "AHRS_EKF_TYPE": 10, + "EK2_ENABLE": 0, + "EK3_ENABLE": 0, + "INS_FAST_SAMPLE": 0, + "LOG_BITMASK": 959, + "LOG_DISARMED": 0, + "ATC_THR_G_BOOST": 5.0, + }) - except Exception as e: - self.print_exception_caught(e) - ex = e + self.reboot_sitl() - self.context_pop() + self.takeoff(10, mode="ALT_HOLD") + hover_time = 15 + self.progress("Hovering for %u seconds" % hover_time) + tstart = self.get_sim_time() + while self.get_sim_time_cached() < tstart + hover_time: + self.assert_receive_message('ATTITUDE') - # must reboot after we move away from EKF type 10 to EKF2 or EKF3 - self.reboot_sitl() + # fly fast forrest! + self.set_rc(3, 1900) + self.set_rc(2, 1200) + self.wait_groundspeed(5, 1000) + self.set_rc(3, 1500) + self.set_rc(2, 1500) - if ex is not None: - raise ex + self.do_RTL() def test_gyro_fft_harmonic(self, averaging): """Use dynamic harmonic notch to control motor noise with harmonic matching of the first harmonic.""" @@ -7267,55 +7129,41 @@ def AltTypes(self): def PrecisionLoiterCompanion(self): """Use Companion PrecLand backend precision messages to loiter.""" - self.context_push() - - ex = None - try: - self.set_parameters({ - "PLND_ENABLED": 1, - "PLND_TYPE": 1, # enable companion backend: - "RC7_OPTION": 39, # set up a channel switch to enable precision loiter: - }) - self.set_analog_rangefinder_parameters() - self.reboot_sitl() - - self.progress("Waiting for location") - self.change_mode('LOITER') - self.wait_ready_to_arm() + self.set_parameters({ + "PLND_ENABLED": 1, + "PLND_TYPE": 1, # enable companion backend: + "RC7_OPTION": 39, # set up a channel switch to enable precision loiter: + }) + self.set_analog_rangefinder_parameters() + self.reboot_sitl() - # we should be doing precision loiter at this point - start = self.assert_receive_message('LOCAL_POSITION_NED') + self.progress("Waiting for location") + self.change_mode('LOITER') + self.wait_ready_to_arm() - self.takeoff(20, mode='ALT_HOLD') + # we should be doing precision loiter at this point + start = self.assert_receive_message('LOCAL_POSITION_NED') - # move away a little - self.set_rc(2, 1550) - self.wait_distance(5, accuracy=1) - self.set_rc(2, 1500) - self.change_mode('LOITER') + self.takeoff(20, mode='ALT_HOLD') - # turn precision loiter on: - self.context_collect('STATUSTEXT') - self.set_rc(7, 2000) + # move away a little + self.set_rc(2, 1550) + self.wait_distance(5, accuracy=1) + self.set_rc(2, 1500) + self.change_mode('LOITER') - # try to drag aircraft to a position 5 metres north-east-east: - self.precision_loiter_to_pos(start.x + 5, start.y + 10, start.z + 10) - self.wait_statustext("PrecLand: Target Found", check_context=True, timeout=10) - self.wait_statustext("PrecLand: Init Complete", check_context=True, timeout=10) - # .... then northwest - self.precision_loiter_to_pos(start.x + 5, start.y - 10, start.z + 10) + # turn precision loiter on: + self.context_collect('STATUSTEXT') + self.set_rc(7, 2000) - except Exception as e: - self.print_exception_caught(e) - ex = e + # try to drag aircraft to a position 5 metres north-east-east: + self.precision_loiter_to_pos(start.x + 5, start.y + 10, start.z + 10) + self.wait_statustext("PrecLand: Target Found", check_context=True, timeout=10) + self.wait_statustext("PrecLand: Init Complete", check_context=True, timeout=10) + # .... then northwest + self.precision_loiter_to_pos(start.x + 5, start.y - 10, start.z + 10) self.disarm_vehicle(force=True) - self.context_pop() - self.reboot_sitl() - self.progress("All done") - - if ex is not None: - raise ex def loiter_requires_position(self): # ensure we can't switch to LOITER without position @@ -7870,87 +7718,69 @@ def BeaconPosition(self): old_pos = self.get_global_position_int() print("old_pos=%s" % str(old_pos)) - self.context_push() - ex = None - try: - self.set_parameters({ - "BCN_TYPE": 10, - "BCN_LATITUDE": SITL_START_LOCATION.lat, - "BCN_LONGITUDE": SITL_START_LOCATION.lng, - "BCN_ALT": SITL_START_LOCATION.alt, - "BCN_ORIENT_YAW": 0, - "AVOID_ENABLE": 4, - "GPS1_TYPE": 0, - "EK3_ENABLE": 1, - "EK3_SRC1_POSXY": 4, # Beacon - "EK3_SRC1_POSZ": 1, # Baro - "EK3_SRC1_VELXY": 0, # None - "EK3_SRC1_VELZ": 0, # None - "EK2_ENABLE": 0, - "AHRS_EKF_TYPE": 3, - }) - self.reboot_sitl() + self.set_parameters({ + "BCN_TYPE": 10, + "BCN_LATITUDE": SITL_START_LOCATION.lat, + "BCN_LONGITUDE": SITL_START_LOCATION.lng, + "BCN_ALT": SITL_START_LOCATION.alt, + "BCN_ORIENT_YAW": 0, + "AVOID_ENABLE": 4, + "GPS1_TYPE": 0, + "EK3_ENABLE": 1, + "EK3_SRC1_POSXY": 4, # Beacon + "EK3_SRC1_POSZ": 1, # Baro + "EK3_SRC1_VELXY": 0, # None + "EK3_SRC1_VELZ": 0, # None + "EK2_ENABLE": 0, + "AHRS_EKF_TYPE": 3, + }) + self.reboot_sitl() - # turn off GPS arming checks. This may be considered a - # bug that we need to do this. - old_arming_check = int(self.get_parameter("ARMING_CHECK")) - if old_arming_check == 1: - old_arming_check = 1 ^ 25 - 1 - new_arming_check = int(old_arming_check) & ~(1 << 3) - self.set_parameter("ARMING_CHECK", new_arming_check) + # turn off GPS arming checks. This may be considered a + # bug that we need to do this. + old_arming_check = int(self.get_parameter("ARMING_CHECK")) + if old_arming_check == 1: + old_arming_check = 1 ^ 25 - 1 + new_arming_check = int(old_arming_check) & ~(1 << 3) + self.set_parameter("ARMING_CHECK", new_arming_check) - self.reboot_sitl() + self.reboot_sitl() - # require_absolute=True infers a GPS is present - self.wait_ready_to_arm(require_absolute=False) + # require_absolute=True infers a GPS is present + self.wait_ready_to_arm(require_absolute=False) - tstart = self.get_sim_time() - timeout = 20 - while True: - if self.get_sim_time_cached() - tstart > timeout: - raise NotAchievedException("Did not get new position like old position") - self.progress("Fetching location") - new_pos = self.get_global_position_int() - pos_delta = self.get_distance_int(old_pos, new_pos) - max_delta = 1 - self.progress("delta=%u want <= %u" % (pos_delta, max_delta)) - if pos_delta <= max_delta: - break + tstart = self.get_sim_time() + timeout = 20 + while True: + if self.get_sim_time_cached() - tstart > timeout: + raise NotAchievedException("Did not get new position like old position") + self.progress("Fetching location") + new_pos = self.get_global_position_int() + pos_delta = self.get_distance_int(old_pos, new_pos) + max_delta = 1 + self.progress("delta=%u want <= %u" % (pos_delta, max_delta)) + if pos_delta <= max_delta: + break - self.progress("Moving to ensure location is tracked") - self.takeoff(10, mode="STABILIZE") - self.change_mode("CIRCLE") + self.progress("Moving to ensure location is tracked") + self.takeoff(10, mode="STABILIZE") + self.change_mode("CIRCLE") - tstart = self.get_sim_time() - max_delta = 0 - max_allowed_delta = 10 - while True: - if self.get_sim_time_cached() - tstart > timeout: - break + self.context_push() + validator = vehicle_test_suite.TestSuite.ValidateGlobalPositionIntAgainstSimState(self, max_allowed_divergence=10) + self.install_message_hook_context(validator) - pos_delta = self.get_distance_int(self.sim_location_int(), self.get_global_position_int()) - self.progress("pos_delta=%f max_delta=%f max_allowed_delta=%f" % (pos_delta, max_delta, max_allowed_delta)) - if pos_delta > max_delta: - max_delta = pos_delta - if pos_delta > max_allowed_delta: - raise NotAchievedException("Vehicle location not tracking simulated location (%f > %f)" % - (pos_delta, max_allowed_delta)) - self.progress("Tracked location just fine (max_delta=%f)" % max_delta) - self.change_mode("LOITER") - self.wait_groundspeed(0, 0.3, timeout=120) - self.land_and_disarm() + self.delay_sim_time(20) + self.progress("Tracked location just fine") + self.context_pop() - self.assert_current_onboard_log_contains_message("BCN") + self.change_mode("LOITER") + self.wait_groundspeed(0, 0.3, timeout=120) + self.land_and_disarm() + + self.assert_current_onboard_log_contains_message("BCN") - except Exception as e: - self.print_exception_caught(e) - ex = e self.disarm_vehicle(force=True) - self.reboot_sitl() - self.context_pop() - self.reboot_sitl() - if ex is not None: - raise ex def AC_Avoidance_Beacon(self): '''Test beacon avoidance slide behaviour''' @@ -9868,6 +9698,23 @@ def GPSForYaw(self): if ex is not None: raise ex + def SMART_RTL_EnterLeave(self): + '''check SmartRTL behaviour when entering/leaving''' + # we had a bug where we would consume points when re-entering smartrtl + + self.upload_simple_relhome_mission([ + # N E U + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 10), + (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), + ]) + self.set_parameter('AUTO_OPTIONS', 3) + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.change_mode('ALT_HOLD') + self.change_mode('SMART_RTL') + self.change_mode('ALT_HOLD') + self.change_mode('SMART_RTL') + def GPSForYawCompassLearn(self): '''Moving baseline GPS yaw - with compass learning''' self.context_push() @@ -11270,6 +11117,7 @@ def MAV_CMD_NAV_TAKEOFF_command_int(self): def Ch6TuningWPSpeed(self): '''test waypoint speed can be changed via Ch6 tuning knob''' self.set_parameters({ + "RC6_OPTION": 219, # RC6 used for tuning "TUNE": 10, # 10 is waypoint speed "TUNE_MIN": 0.02, # 20cm/s "TUNE_MAX": 1000, # 10m/s @@ -11880,6 +11728,7 @@ def tests2b(self): # this block currently around 9.5mins here self.GSF_reset, self.AP_Avoidance, self.SMART_RTL, + self.SMART_RTL_EnterLeave, self.RTL_TO_RALLY, self.FlyEachFrame, self.GPSBlending, @@ -12033,6 +11882,7 @@ def disabled_tests(self): "FlyMissionTwice": "See https://github.com/ArduPilot/ardupilot/pull/18561", "GPSForYawCompassLearn": "Vehicle currently crashed in spectacular fashion", "CompassMot": "Cuases an arithmetic exception in the EKF", + "SMART_RTL_EnterLeave": "Causes a panic", } diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 6f90634470327e..2af157ab9c8ce2 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -173,7 +173,7 @@ def fly_left_circuit(self): def fly_RTL(self): """Fly to home.""" self.progress("Flying home in RTL") - target_loc = self.homeloc + target_loc = self.home_position_as_mav_location() target_loc.alt += 100 self.change_mode('RTL') self.wait_location(target_loc, @@ -275,8 +275,10 @@ def wait_level_flight(self, accuracy=5, timeout=30): return raise NotAchievedException("Failed to attain level flight") - def change_altitude(self, altitude, accuracy=30): + def change_altitude(self, altitude, accuracy=30, relative=False): """Get to a given altitude.""" + if relative: + altitude += self.home_position_as_mav_location().alt self.change_mode('FBWA') alt_error = self.mav.messages['VFR_HUD'].alt - altitude if alt_error > 0: @@ -293,7 +295,7 @@ def axial_left_roll(self, count=1): """Fly a left axial roll.""" # full throttle! self.set_rc(3, 2000) - self.change_altitude(self.homeloc.alt+300) + self.change_altitude(300, relative=True) # fly the roll in manual self.change_mode('MANUAL') @@ -320,7 +322,7 @@ def inside_loop(self, count=1): """Fly a inside loop.""" # full throttle! self.set_rc(3, 2000) - self.change_altitude(self.homeloc.alt+300) + self.change_altitude(300, relative=True) # fly the loop in manual self.change_mode('MANUAL') @@ -432,7 +434,7 @@ def test_stabilize(self, count=1): # full throttle! self.set_rc(3, 2000) self.set_rc(2, 1300) - self.change_altitude(self.homeloc.alt+300) + self.change_altitude(300, relative=True) self.set_rc(2, 1500) self.change_mode('STABILIZE') @@ -458,7 +460,7 @@ def test_acro(self, count=1): # full throttle! self.set_rc(3, 2000) self.set_rc(2, 1300) - self.change_altitude(self.homeloc.alt+300) + self.change_altitude(300, relative=True) self.set_rc(2, 1500) self.change_mode('ACRO') @@ -1031,95 +1033,61 @@ def fly_home_land_and_disarm(self, timeout=120): def TestFlaps(self): """Test flaps functionality.""" filename = "flaps.txt" - self.context_push() - ex = None - try: + flaps_ch = 5 + flaps_ch_min = 1000 + flaps_ch_trim = 1500 + flaps_ch_max = 2000 - flaps_ch = 5 - flaps_ch_min = 1000 - flaps_ch_trim = 1500 - flaps_ch_max = 2000 + servo_ch = 5 + servo_ch_min = 1200 + servo_ch_trim = 1300 + servo_ch_max = 1800 - servo_ch = 5 - servo_ch_min = 1200 - servo_ch_trim = 1300 - servo_ch_max = 1800 + self.set_parameters({ + "SERVO%u_FUNCTION" % servo_ch: 3, # flapsauto + "RC%u_OPTION" % flaps_ch: 208, # Flaps RCx_OPTION + "LAND_FLAP_PERCNT": 50, + "LOG_DISARMED": 1, + "RTL_AUTOLAND": 1, - self.set_parameters({ - "SERVO%u_FUNCTION" % servo_ch: 3, # flapsauto - "RC%u_OPTION" % flaps_ch: 208, # Flaps RCx_OPTION - "LAND_FLAP_PERCNT": 50, - "LOG_DISARMED": 1, - "RTL_AUTOLAND": 1, - - "RC%u_MIN" % flaps_ch: flaps_ch_min, - "RC%u_MAX" % flaps_ch: flaps_ch_max, - "RC%u_TRIM" % flaps_ch: flaps_ch_trim, - - "SERVO%u_MIN" % servo_ch: servo_ch_min, - "SERVO%u_MAX" % servo_ch: servo_ch_max, - "SERVO%u_TRIM" % servo_ch: servo_ch_trim, - }) + "RC%u_MIN" % flaps_ch: flaps_ch_min, + "RC%u_MAX" % flaps_ch: flaps_ch_max, + "RC%u_TRIM" % flaps_ch: flaps_ch_trim, - self.progress("check flaps are not deployed") - self.set_rc(flaps_ch, flaps_ch_min) - self.wait_servo_channel_value(servo_ch, servo_ch_min, timeout=3) - self.progress("deploy the flaps") - self.set_rc(flaps_ch, flaps_ch_max) - tstart = self.get_sim_time() - self.wait_servo_channel_value(servo_ch, servo_ch_max) - tstop = self.get_sim_time_cached() - delta_time = tstop - tstart - delta_time_min = 0.5 - delta_time_max = 1.5 - if delta_time < delta_time_min or delta_time > delta_time_max: - raise NotAchievedException(( - "Flaps Slew not working (%f seconds)" % (delta_time,))) - self.progress("undeploy flaps") - self.set_rc(flaps_ch, flaps_ch_min) - self.wait_servo_channel_value(servo_ch, servo_ch_min) - - self.progress("Flying mission %s" % filename) - self.load_mission(filename) - self.set_current_waypoint(1) - self.change_mode('AUTO') - self.wait_ready_to_arm() - self.arm_vehicle() - last_mission_current_msg = 0 - last_seq = None - while self.armed(): - m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True) - time_delta = (self.get_sim_time_cached() - - last_mission_current_msg) - if (time_delta > 1 or m.seq != last_seq): - dist = None - x = self.mav.messages.get("NAV_CONTROLLER_OUTPUT", None) - if x is not None: - dist = x.wp_dist - self.progress("MISSION_CURRENT.seq=%u (dist=%s)" % - (m.seq, str(dist))) - last_mission_current_msg = self.get_sim_time_cached() - last_seq = m.seq - # flaps should undeploy at the end - self.wait_servo_channel_value(servo_ch, servo_ch_min, timeout=30) - - # do a short flight in FBWA, watching for flaps - # self.mavproxy.send('switch 4\n') - # self.wait_mode('FBWA') - # self.delay_sim_time(10) - # self.mavproxy.send('switch 6\n') - # self.wait_mode('MANUAL') - # self.delay_sim_time(10) - - self.progress("Flaps OK") - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - if ex: - if self.armed(): - self.disarm_vehicle() - raise ex + "SERVO%u_MIN" % servo_ch: servo_ch_min, + "SERVO%u_MAX" % servo_ch: servo_ch_max, + "SERVO%u_TRIM" % servo_ch: servo_ch_trim, + }) + + self.progress("check flaps are not deployed") + self.set_rc(flaps_ch, flaps_ch_min) + self.wait_servo_channel_value(servo_ch, servo_ch_min, timeout=3) + self.progress("deploy the flaps") + self.set_rc(flaps_ch, flaps_ch_max) + tstart = self.get_sim_time() + self.wait_servo_channel_value(servo_ch, servo_ch_max) + tstop = self.get_sim_time_cached() + delta_time = tstop - tstart + delta_time_min = 0.5 + delta_time_max = 1.5 + if delta_time < delta_time_min or delta_time > delta_time_max: + raise NotAchievedException(( + "Flaps Slew not working (%f seconds)" % (delta_time,))) + self.progress("undeploy flaps") + self.set_rc(flaps_ch, flaps_ch_min) + self.wait_servo_channel_value(servo_ch, servo_ch_min) + + self.progress("Flying mission %s" % filename) + self.load_mission(filename) + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + # flaps should deploy for landing (RC input value used for position?!) + self.wait_servo_channel_value(servo_ch, flaps_ch_trim, timeout=300) + # flaps should undeploy at the end + self.wait_servo_channel_value(servo_ch, servo_ch_min, timeout=30) + + self.progress("Flaps OK") def TestRCRelay(self): '''Test Relay RC Channel Option''' @@ -1284,32 +1252,25 @@ def ThrottleFailsafe(self): self.progress("Ensure long failsafe can trigger when short failsafe disabled") self.context_push() self.context_collect("STATUSTEXT") - ex = None - try: - self.set_parameters({ - "FS_SHORT_ACTN": 3, # 3 means disabled - "SIM_RC_FAIL": 1, - }) - self.wait_statustext("Long failsafe on", check_context=True) - self.wait_mode("RTL") + self.set_parameters({ + "FS_SHORT_ACTN": 3, # 3 means disabled + "SIM_RC_FAIL": 1, + }) + self.wait_statustext("Long failsafe on", check_context=True) + self.wait_mode("RTL") # self.context_clear_collection("STATUSTEXT") - self.set_parameter("SIM_RC_FAIL", 0) - self.wait_text("Long Failsafe Cleared", check_context=True) - self.change_mode("MANUAL") + self.set_parameter("SIM_RC_FAIL", 0) + self.wait_text("Long Failsafe Cleared", check_context=True) + self.change_mode("MANUAL") - self.progress("Trying again with THR_FS_VALUE") - self.set_parameters({ - "THR_FS_VALUE": 960, - "SIM_RC_FAIL": 2, - }) - self.wait_statustext("Long Failsafe on", check_context=True) - self.wait_mode("RTL") - except Exception as e: - self.print_exception_caught(e) - ex = e + self.progress("Trying again with THR_FS_VALUE") + self.set_parameters({ + "THR_FS_VALUE": 960, + "SIM_RC_FAIL": 2, + }) + self.wait_statustext("Long Failsafe on", check_context=True) + self.wait_mode("RTL") self.context_pop() - if ex is not None: - raise ex self.start_subtest("Not use RC throttle input when THR_FAILSAFE==2") self.takeoff(100) @@ -1432,24 +1393,15 @@ def GCSFailsafe(self): def TestGripperMission(self): '''Test Gripper mission items''' - self.context_push() - ex = None - try: - self.set_parameter("RTL_AUTOLAND", 1) - self.load_mission("plane-gripper-mission.txt") - self.set_current_waypoint(1) - self.change_mode('AUTO') - self.wait_ready_to_arm() - self.arm_vehicle() - self.wait_statustext("Gripper Grabbed", timeout=60) - self.wait_statustext("Gripper Released", timeout=60) - self.wait_statustext("Auto disarmed", timeout=60) - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - if ex is not None: - raise ex + self.set_parameter("RTL_AUTOLAND", 1) + self.load_mission("plane-gripper-mission.txt") + self.set_current_waypoint(1) + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + self.wait_statustext("Gripper Grabbed", timeout=60) + self.wait_statustext("Gripper Released", timeout=60) + self.wait_statustext("Auto disarmed", timeout=60) def assert_fence_sys_status(self, present, enabled, health): self.delay_sim_time(1) @@ -1538,213 +1490,166 @@ def MODE_SWITCH_RESET(self): def FenceStatic(self): '''Test Basic Fence Functionality''' - ex = None - try: - self.progress("Checking for bizarre healthy-when-not-present-or-enabled") - self.set_parameter("FENCE_TYPE", 4) # Start by only setting polygon fences, otherwise fence will report present - self.assert_fence_sys_status(False, False, True) - self.load_fence("CMAC-fence.txt") - m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) - if m is not None: - raise NotAchievedException("Got FENCE_STATUS unexpectedly") - self.set_parameter("FENCE_ACTION", 0) # report only - self.assert_fence_sys_status(True, False, True) - self.set_parameter("FENCE_ACTION", 1) # RTL - self.assert_fence_sys_status(True, False, True) - self.do_fence_enable() - self.assert_fence_sys_status(True, True, True) - m = self.assert_receive_message('FENCE_STATUS', timeout=2) - if m.breach_status: - raise NotAchievedException("Breached fence unexpectedly (%u)" % - (m.breach_status)) - self.do_fence_disable() - self.assert_fence_sys_status(True, False, True) - self.set_parameter("FENCE_ACTION", 1) - self.assert_fence_sys_status(True, False, True) - self.set_parameter("FENCE_ACTION", 0) - self.assert_fence_sys_status(True, False, True) - self.clear_fence() - if self.get_parameter("FENCE_TOTAL") != 0: - raise NotAchievedException("Expected zero points remaining") - self.assert_fence_sys_status(False, False, True) - self.progress("Trying to enable fence with no points") - self.do_fence_enable(want_result=mavutil.mavlink.MAV_RESULT_FAILED) - - # test a rather unfortunate behaviour: - self.progress("Killing a live fence with fence-clear") - self.load_fence("CMAC-fence.txt") - self.set_parameter("FENCE_ACTION", 1) # AC_FENCE_ACTION_RTL_AND_LAND == 1. mavutil.mavlink.FENCE_ACTION_RTL == 4 - self.do_fence_enable() - self.assert_fence_sys_status(True, True, True) - self.clear_fence() - self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, False, False, True) - if self.get_parameter("FENCE_TOTAL") != 0: - raise NotAchievedException("Expected zero points remaining") - self.assert_fence_sys_status(False, False, True) - self.do_fence_disable() + self.progress("Checking for bizarre healthy-when-not-present-or-enabled") + self.set_parameter("FENCE_TYPE", 4) # Start by only setting polygon fences, otherwise fence will report present + self.assert_fence_sys_status(False, False, True) + self.load_fence("CMAC-fence.txt") + m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) + if m is not None: + raise NotAchievedException("Got FENCE_STATUS unexpectedly") + self.set_parameter("FENCE_ACTION", 0) # report only + self.assert_fence_sys_status(True, False, True) + self.set_parameter("FENCE_ACTION", 1) # RTL + self.assert_fence_sys_status(True, False, True) + self.do_fence_enable() + self.assert_fence_sys_status(True, True, True) + m = self.assert_receive_message('FENCE_STATUS', timeout=2) + if m.breach_status: + raise NotAchievedException("Breached fence unexpectedly (%u)" % + (m.breach_status)) + self.do_fence_disable() + self.assert_fence_sys_status(True, False, True) + self.set_parameter("FENCE_ACTION", 1) + self.assert_fence_sys_status(True, False, True) + self.set_parameter("FENCE_ACTION", 0) + self.assert_fence_sys_status(True, False, True) + self.clear_fence() + if self.get_parameter("FENCE_TOTAL") != 0: + raise NotAchievedException("Expected zero points remaining") + self.assert_fence_sys_status(False, False, True) + self.progress("Trying to enable fence with no points") + self.do_fence_enable(want_result=mavutil.mavlink.MAV_RESULT_FAILED) + + # test a rather unfortunate behaviour: + self.progress("Killing a live fence with fence-clear") + self.load_fence("CMAC-fence.txt") + self.set_parameter("FENCE_ACTION", 1) # AC_FENCE_ACTION_RTL_AND_LAND == 1. mavutil.mavlink.FENCE_ACTION_RTL == 4 + self.do_fence_enable() + self.assert_fence_sys_status(True, True, True) + self.clear_fence() + self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, False, False, True) + if self.get_parameter("FENCE_TOTAL") != 0: + raise NotAchievedException("Expected zero points remaining") + self.assert_fence_sys_status(False, False, True) + self.do_fence_disable() - # ensure that a fence is present if it is tin can, min alt or max alt - self.progress("Test other fence types (tin-can, min alt, max alt") - self.set_parameter("FENCE_TYPE", 1) # max alt - self.assert_fence_sys_status(True, False, True) - self.set_parameter("FENCE_TYPE", 8) # min alt - self.assert_fence_sys_status(True, False, True) - self.set_parameter("FENCE_TYPE", 2) # tin can - self.assert_fence_sys_status(True, False, True) - - # Test cannot arm if outside of fence and fence is enabled - self.progress("Test Arming while vehicle below FENCE_ALT_MIN") - default_fence_alt_min = self.get_parameter("FENCE_ALT_MIN") - self.set_parameter("FENCE_ALT_MIN", 50) - self.set_parameter("FENCE_TYPE", 8) # Enables minimum altitude breaches - self.do_fence_enable() - self.delay_sim_time(2) # Allow breach to propagate - self.assert_fence_enabled() + # ensure that a fence is present if it is tin can, min alt or max alt + self.progress("Test other fence types (tin-can, min alt, max alt") + self.set_parameter("FENCE_TYPE", 1) # max alt + self.assert_fence_sys_status(True, False, True) + self.set_parameter("FENCE_TYPE", 8) # min alt + self.assert_fence_sys_status(True, False, True) + self.set_parameter("FENCE_TYPE", 2) # tin can + self.assert_fence_sys_status(True, False, True) + + # Test cannot arm if outside of fence and fence is enabled + self.progress("Test Arming while vehicle below FENCE_ALT_MIN") + default_fence_alt_min = self.get_parameter("FENCE_ALT_MIN") + self.set_parameter("FENCE_ALT_MIN", 50) + self.set_parameter("FENCE_TYPE", 8) # Enables minimum altitude breaches + self.do_fence_enable() + self.delay_sim_time(2) # Allow breach to propagate + self.assert_fence_enabled() - self.try_arm(False, "vehicle outside Min Alt fence") - self.do_fence_disable() - self.set_parameter("FENCE_ALT_MIN", default_fence_alt_min) - - # Test arming outside inclusion zone - self.progress("Test arming while vehicle outside of inclusion zone") - self.set_parameter("FENCE_TYPE", 4) # Enables polygon fence types - self.upload_fences_from_locations([( - mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [ - mavutil.location(1.000, 1.000, 0, 0), - mavutil.location(1.000, 1.001, 0, 0), - mavutil.location(1.001, 1.001, 0, 0), - mavutil.location(1.001, 1.000, 0, 0) - ] - )]) - self.delay_sim_time(10) # let fence check run so it loads-from-eeprom - self.do_fence_enable() - self.assert_fence_enabled() - self.delay_sim_time(2) # Allow breach to propagate - self.try_arm(False, "vehicle outside Polygon fence") - self.do_fence_disable() - self.clear_fence() - - self.progress("Test arming while vehicle inside exclusion zone") - self.set_parameter("FENCE_TYPE", 4) # Enables polygon fence types - home_loc = self.mav.location() - locs = [ - mavutil.location(home_loc.lat - 0.001, home_loc.lng - 0.001, 0, 0), - mavutil.location(home_loc.lat - 0.001, home_loc.lng + 0.001, 0, 0), - mavutil.location(home_loc.lat + 0.001, home_loc.lng + 0.001, 0, 0), - mavutil.location(home_loc.lat + 0.001, home_loc.lng - 0.001, 0, 0), - ] - self.upload_fences_from_locations([ - (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, locs), - ]) - self.delay_sim_time(10) # let fence check run so it loads-from-eeprom - self.do_fence_enable() - self.assert_fence_enabled() - self.delay_sim_time(2) # Allow breach to propagate - self.try_arm(False, "vehicle outside Polygon fence") - self.do_fence_disable() - self.clear_fence() + self.try_arm(False, "vehicle outside Min Alt fence") + self.do_fence_disable() + self.set_parameter("FENCE_ALT_MIN", default_fence_alt_min) - except Exception as e: - self.print_exception_caught(e) - ex = e + # Test arming outside inclusion zone + self.progress("Test arming while vehicle outside of inclusion zone") + self.set_parameter("FENCE_TYPE", 4) # Enables polygon fence types + self.upload_fences_from_locations([( + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [ + mavutil.location(1.000, 1.000, 0, 0), + mavutil.location(1.000, 1.001, 0, 0), + mavutil.location(1.001, 1.001, 0, 0), + mavutil.location(1.001, 1.000, 0, 0) + ] + )]) + self.delay_sim_time(10) # let fence check run so it loads-from-eeprom + self.do_fence_enable() + self.assert_fence_enabled() + self.delay_sim_time(2) # Allow breach to propagate + self.try_arm(False, "vehicle outside Polygon fence") + self.do_fence_disable() self.clear_fence() - if ex is not None: - raise ex - - def test_fence_breach_circle_at(self, loc, disable_on_breach=False): - ex = None - try: - self.load_fence("CMAC-fence.txt") - want_radius = 100 - # when ArduPlane is fixed, remove this fudge factor - REALLY_BAD_FUDGE_FACTOR = 1.16 - expected_radius = REALLY_BAD_FUDGE_FACTOR * want_radius - self.set_parameters({ - "RTL_RADIUS": want_radius, - "NAVL1_LIM_BANK": 60, - "FENCE_ACTION": 1, # AC_FENCE_ACTION_RTL_AND_LAND == 1. mavutil.mavlink.FENCE_ACTION_RTL == 4 - }) - - self.wait_ready_to_arm() # need an origin to load fence - self.do_fence_enable() - self.assert_fence_sys_status(True, True, True) + self.progress("Test arming while vehicle inside exclusion zone") + self.set_parameter("FENCE_TYPE", 4) # Enables polygon fence types + home_loc = self.mav.location() + locs = [ + mavutil.location(home_loc.lat - 0.001, home_loc.lng - 0.001, 0, 0), + mavutil.location(home_loc.lat - 0.001, home_loc.lng + 0.001, 0, 0), + mavutil.location(home_loc.lat + 0.001, home_loc.lng + 0.001, 0, 0), + mavutil.location(home_loc.lat + 0.001, home_loc.lng - 0.001, 0, 0), + ] + self.upload_fences_from_locations([ + (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, locs), + ]) + self.delay_sim_time(10) # let fence check run so it loads-from-eeprom + self.do_fence_enable() + self.assert_fence_enabled() + self.delay_sim_time(2) # Allow breach to propagate + self.try_arm(False, "vehicle outside Polygon fence") + self.do_fence_disable() - self.takeoff(alt=45, alt_max=300) + def test_fence_breach_circle_at(self, loc, disable_on_breach=False): + self.load_fence("CMAC-fence.txt") + want_radius = 100 + # when ArduPlane is fixed, remove this fudge factor + REALLY_BAD_FUDGE_FACTOR = 1.16 + expected_radius = REALLY_BAD_FUDGE_FACTOR * want_radius + self.set_parameters({ + "RTL_RADIUS": want_radius, + "NAVL1_LIM_BANK": 60, + "FENCE_ACTION": 1, # AC_FENCE_ACTION_RTL_AND_LAND == 1. mavutil.mavlink.FENCE_ACTION_RTL == 4 + }) - tstart = self.get_sim_time() - while True: - if self.get_sim_time() - tstart > 30: - raise NotAchievedException("Did not breach fence") - m = self.assert_receive_message('FENCE_STATUS', timeout=2) - if m.breach_status == 0: - continue + self.wait_ready_to_arm() # need an origin to load fence - # we've breached; check our state; - if m.breach_type != mavutil.mavlink.FENCE_BREACH_BOUNDARY: - raise NotAchievedException("Unexpected breach type %u" % - (m.breach_type,)) - if m.breach_count == 0: - raise NotAchievedException("Unexpected breach count %u" % - (m.breach_count,)) - self.assert_fence_sys_status(True, True, False) - break + self.do_fence_enable() + self.assert_fence_sys_status(True, True, True) - if disable_on_breach: - self.do_fence_disable() + self.takeoff(alt=45, alt_max=300) - self.wait_circling_point_with_radius(loc, expected_radius) + tstart = self.get_sim_time() + while True: + if self.get_sim_time() - tstart > 30: + raise NotAchievedException("Did not breach fence") + m = self.assert_receive_message('FENCE_STATUS', timeout=2) + if m.breach_status == 0: + continue - self.disarm_vehicle(force=True) - self.reboot_sitl() + # we've breached; check our state; + if m.breach_type != mavutil.mavlink.FENCE_BREACH_BOUNDARY: + raise NotAchievedException("Unexpected breach type %u" % + (m.breach_type,)) + if m.breach_count == 0: + raise NotAchievedException("Unexpected breach count %u" % + (m.breach_count,)) + self.assert_fence_sys_status(True, True, False) + break - except Exception as e: - self.print_exception_caught(e) - ex = e - self.clear_fence() - if ex is not None: - raise ex + self.wait_circling_point_with_radius(loc, expected_radius) + self.do_fence_disable() + self.disarm_vehicle(force=True) + self.reboot_sitl() def FenceRTL(self): '''Test Fence RTL''' self.progress("Testing FENCE_ACTION_RTL no rally point") # have to disable the fence once we've breached or we breach # it as part of the loiter-at-home! - self.test_fence_breach_circle_at(self.home_position_as_mav_location(), - disable_on_breach=True) + self.test_fence_breach_circle_at(self.home_position_as_mav_location()) def FenceRTLRally(self): '''Test Fence RTL Rally''' - ex = None - target_system = 1 - target_component = 1 - try: - self.progress("Testing FENCE_ACTION_RTL with rally point") + self.progress("Testing FENCE_ACTION_RTL with rally point") - self.wait_ready_to_arm() - loc = self.home_relative_loc_ne(50, -50) - - self.set_parameter("RALLY_TOTAL", 1) - self.mav.mav.rally_point_send(target_system, - target_component, - 0, # sequence number - 1, # total count - int(loc.lat * 1e7), - int(loc.lng * 1e7), - 15, - 0, # "break" alt?! - 0, # "land dir" - 0) # flags - self.delay_sim_time(1) - if self.mavproxy is not None: - self.mavproxy.send("rally list\n") - self.test_fence_breach_circle_at(loc) - except Exception as e: - self.print_exception_caught(e) - ex = e - self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) - if ex is not None: - raise ex + self.wait_ready_to_arm() + loc = self.home_relative_loc_ne(50, -50) + self.upload_rally_points_from_locations([loc]) + self.test_fence_breach_circle_at(loc) def FenceRetRally(self): """ Tests the FENCE_RET_RALLY flag, either returning to fence return point, @@ -1754,7 +1659,6 @@ def FenceRetRally(self): self.progress("Testing FENCE_ACTION_RTL with fence rally point") self.wait_ready_to_arm() - self.homeloc = self.mav.location() # Grab a location for fence return point, and upload it. fence_loc = self.home_position_as_mav_location() @@ -1784,18 +1688,7 @@ def FenceRetRally(self): # Grab a location for rally point, and upload it. rally_loc = self.home_relative_loc_ne(-50, 50) - self.set_parameter("RALLY_TOTAL", 1) - self.mav.mav.rally_point_send(target_system, - target_component, - 0, # sequence number - 1, # total count - int(rally_loc.lat * 1e7), - int(rally_loc.lng * 1e7), - 15, - 0, # "break" alt?! - 0, # "land dir" - 0) # flags - self.delay_sim_time(1) + self.upload_rally_points_from_locations([rally_loc]) return_radius = 100 return_alt = 80 @@ -1821,7 +1714,7 @@ def FenceRetRally(self): self.delay_sim_time(15) # Fly up before re-triggering fence breach. Fly to fence return point - self.change_altitude(self.homeloc.alt+30) + self.change_altitude(30, relative=True) self.set_parameters({ "FENCE_RET_RALLY": 0, "FENCE_ALT_MIN": 60, @@ -1867,12 +1760,12 @@ def terrain_wait_path(loc1, loc2, steps): self.progress("Got required terrain") self.wait_ready_to_arm() - self.homeloc = self.mav.location() + homeloc = self.mav.location() - guided_loc = mavutil.location(-35.39723762, 149.07284612, self.homeloc.alt+99.0, 0) - rally_loc = mavutil.location(-35.3654952000, 149.1558698000, self.homeloc.alt+100, 0) + guided_loc = mavutil.location(-35.39723762, 149.07284612, homeloc.alt+99.0, 0) + rally_loc = mavutil.location(-35.3654952000, 149.1558698000, homeloc.alt+100, 0) - terrain_wait_path(self.homeloc, rally_loc, 10) + terrain_wait_path(homeloc, rally_loc, 10) # set a rally point to the west of home self.upload_rally_points_from_locations([rally_loc]) @@ -1999,20 +1892,8 @@ def run_subtest(self, desc, func): def fly_ahrs2_test(self): '''check secondary estimator is looking OK''' - ahrs2 = self.mav.recv_match(type='AHRS2', blocking=True, timeout=1) - if ahrs2 is None: - raise NotAchievedException("Did not receive AHRS2 message") - self.progress("AHRS2: %s" % str(ahrs2)) - - # check location - gpi = self.mav.recv_match( - type='GLOBAL_POSITION_INT', - blocking=True, - timeout=5 - ) - if gpi is None: - raise NotAchievedException("Did not receive GLOBAL_POSITION_INT message") - self.progress("GPI: %s" % str(gpi)) + ahrs2 = self.assert_receive_message('AHRS2', verbose=1) + gpi = self.assert_receive_message('GLOBAL_POSITION_INT', verbose=1) if self.get_distance_int(gpi, ahrs2) > 10: raise NotAchievedException("Secondary location looks bad") @@ -2025,10 +1906,6 @@ def MainFlight(self): self.progress("Asserting we do support transfer of fence via mission item protocol") self.assert_capability(mavutil.mavlink.MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) - # grab home position: - self.mav.recv_match(type='HOME_POSITION', blocking=True) - self.homeloc = self.mav.location() - self.run_subtest("Takeoff", self.takeoff) self.run_subtest("Set Attitude Target", self.set_attitude_target) @@ -2322,9 +2199,8 @@ def sample_enable_parameter(self): def RangeFinder(self): '''Test RangeFinder Basic Functionality''' - self.context_push() self.progress("Making sure we don't ordinarily get RANGEFINDER") - self.assert_not_receive_message('RANGEFDINDER') + self.assert_not_receive_message('RANGEFINDER') self.set_analog_rangefinder_parameters() @@ -2338,12 +2214,8 @@ def RangeFinder(self): self.wait_ready_to_arm() self.arm_vehicle() self.wait_waypoint(5, 5, max_dist=100) - rf = self.mav.recv_match(type="RANGEFINDER", timeout=1, blocking=True) - if rf is None: - raise NotAchievedException("Did not receive rangefinder message") - gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1) - if gpi is None: - raise NotAchievedException("Did not receive GLOBAL_POSITION_INT message") + rf = self.assert_receive_message('RANGEFINDER') + gpi = self.assert_receive_message('GLOBAL_POSITION_INT') if abs(rf.distance - gpi.relative_alt/1000.0) > 3: raise NotAchievedException( "rangefinder alt (%s) disagrees with global-position-int.relative_alt (%s)" % @@ -2354,9 +2226,6 @@ def RangeFinder(self): if not self.current_onboard_log_contains_message("RFND"): raise NotAchievedException("No RFND messages in log") - self.context_pop() - self.reboot_sitl() - def rc_defaults(self): ret = super(AutoTestPlane, self).rc_defaults() ret[3] = 1000 @@ -2429,12 +2298,7 @@ def SimADSB(self): self.reboot_sitl() self.assert_receive_message('ADSB_VEHICLE', timeout=30) - def ADSB(self): - '''Test ADSB''' - self.ADSB_f_action_rtl() - self.ADSB_r_action_resume_or_loiter() - - def ADSB_r_action_resume_or_loiter(self): + def ADSBResumeActionResumeLoiter(self): '''ensure we resume auto mission or enter loiter''' self.set_parameters({ "ADSB_TYPE": 1, @@ -2472,68 +2336,58 @@ def ADSB_r_action_resume_or_loiter(self): self.fly_home_land_and_disarm() - def ADSB_f_action_rtl(self): - self.context_push() - ex = None - try: - # message ADSB_VEHICLE 37 -353632614 1491652305 0 584070 0 0 0 "bob" 3 1 255 17 - self.set_parameter("RC12_OPTION", 38) # avoid-adsb - self.set_rc(12, 2000) - self.set_parameters({ - "ADSB_TYPE": 1, - "AVD_ENABLE": 1, - "AVD_F_ACTION": mavutil.mavlink.MAV_COLLISION_ACTION_RTL, - }) - self.reboot_sitl() - self.wait_ready_to_arm() - here = self.mav.location() - self.change_mode("FBWA") - self.delay_sim_time(2) # TODO: work out why this is required... - self.test_adsb_send_threatening_adsb_message(here) - self.progress("Waiting for collision message") - m = self.assert_receive_message('COLLISION', timeout=4) - if m.threat_level != 2: - raise NotAchievedException("Expected some threat at least") - if m.action != mavutil.mavlink.MAV_COLLISION_ACTION_RTL: - raise NotAchievedException("Incorrect action; want=%u got=%u" % - (mavutil.mavlink.MAV_COLLISION_ACTION_RTL, m.action)) - self.wait_mode("RTL") - - self.progress("Sending far-away ABSD_VEHICLE message") - self.mav.mav.adsb_vehicle_send( - 37, # ICAO address - int(here.lat+1 * 1e7), - int(here.lng * 1e7), - mavutil.mavlink.ADSB_ALTITUDE_TYPE_PRESSURE_QNH, - int(here.alt*1000 + 10000), # 10m up - 0, # heading in cdeg - 0, # horizontal velocity cm/s - 0, # vertical velocity cm/s - "bob".encode("ascii"), # callsign - mavutil.mavlink.ADSB_EMITTER_TYPE_LIGHT, - 1, # time since last communication - 65535, # flags - 17 # squawk - ) - self.wait_for_collision_threat_to_clear() - self.change_mode("FBWA") + def ADSBFailActionRTL(self): + '''test ADSB avoidance action of RTL''' + # message ADSB_VEHICLE 37 -353632614 1491652305 0 584070 0 0 0 "bob" 3 1 255 17 + self.set_parameter("RC12_OPTION", 38) # avoid-adsb + self.set_rc(12, 2000) + self.set_parameters({ + "ADSB_TYPE": 1, + "AVD_ENABLE": 1, + "AVD_F_ACTION": mavutil.mavlink.MAV_COLLISION_ACTION_RTL, + }) + self.reboot_sitl() + self.wait_ready_to_arm() + here = self.mav.location() + self.change_mode("FBWA") + self.delay_sim_time(2) # TODO: work out why this is required... + self.test_adsb_send_threatening_adsb_message(here) + self.progress("Waiting for collision message") + m = self.assert_receive_message('COLLISION', timeout=4) + if m.threat_level != 2: + raise NotAchievedException("Expected some threat at least") + if m.action != mavutil.mavlink.MAV_COLLISION_ACTION_RTL: + raise NotAchievedException("Incorrect action; want=%u got=%u" % + (mavutil.mavlink.MAV_COLLISION_ACTION_RTL, m.action)) + self.wait_mode("RTL") - self.progress("Disabling ADSB-avoidance with RC channel") - self.set_rc(12, 1000) - self.delay_sim_time(1) # let the switch get polled - self.test_adsb_send_threatening_adsb_message(here) - m = self.mav.recv_match(type='COLLISION', blocking=True, timeout=4) - self.progress("Got (%s)" % str(m)) - if m is not None: - raise NotAchievedException("Got collision message when I shouldn't have") + self.progress("Sending far-away ABSD_VEHICLE message") + self.mav.mav.adsb_vehicle_send( + 37, # ICAO address + int(here.lat+1 * 1e7), + int(here.lng * 1e7), + mavutil.mavlink.ADSB_ALTITUDE_TYPE_PRESSURE_QNH, + int(here.alt*1000 + 10000), # 10m up + 0, # heading in cdeg + 0, # horizontal velocity cm/s + 0, # vertical velocity cm/s + "bob".encode("ascii"), # callsign + mavutil.mavlink.ADSB_EMITTER_TYPE_LIGHT, + 1, # time since last communication + 65535, # flags + 17 # squawk + ) + self.wait_for_collision_threat_to_clear() + self.change_mode("FBWA") - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - self.reboot_sitl() - if ex is not None: - raise ex + self.progress("Disabling ADSB-avoidance with RC channel") + self.set_rc(12, 1000) + self.delay_sim_time(1) # let the switch get polled + self.test_adsb_send_threatening_adsb_message(here) + m = self.mav.recv_match(type='COLLISION', blocking=True, timeout=4) + self.progress("Got (%s)" % str(m)) + if m is not None: + raise NotAchievedException("Got collision message when I shouldn't have") def GuidedRequest(self, target_system=1, target_component=1): '''Test handling of MISSION_ITEM in guided mode''' @@ -2634,9 +2488,7 @@ def LOITER(self): now = self.get_sim_time_cached() if now - tstart > 60: break - m = self.mav.recv_match(type='VFR_HUD', blocking=True, timeout=5) - if m is None: - raise NotAchievedException("Did not get VFR_HUD") + m = self.assert_receive_message('VFR_HUD') new_throttle = m.throttle alt = m.alt m = self.assert_receive_message('ATTITUDE', timeout=5) @@ -3463,9 +3315,6 @@ def IMUTempCal(self): def EKFlaneswitch(self): '''Test EKF3 Affinity and Lane Switching''' - self.context_push() - ex = None - # new lane swtich available only with EK3 self.set_parameters({ "EK3_ENABLE": 1, @@ -3496,165 +3345,149 @@ def statustext_hook(mav, message): return newlane = int(message.text[-1]) self.lane_switches.append(newlane) - self.install_message_hook(statustext_hook) + self.install_message_hook_context(statustext_hook) # get flying self.takeoff(alt=50) self.change_mode('CIRCLE') - try: - ################################################################### - self.progress("Checking EKF3 Lane Switching trigger from all sensors") - ################################################################### - self.start_subtest("ACCELEROMETER: Change z-axis offset") - # create an accelerometer error by changing the Z-axis offset - self.context_collect("STATUSTEXT") - old_parameter = self.get_parameter("INS_ACCOFFS_Z") - self.wait_statustext( - text="EKF3 lane switch", - timeout=30, - the_function=self.set_parameter("INS_ACCOFFS_Z", old_parameter + 5), - check_context=True) - if self.lane_switches != [1]: - raise NotAchievedException("Expected lane switch 1, got %s" % str(self.lane_switches[-1])) - # Cleanup - self.set_parameter("INS_ACCOFFS_Z", old_parameter) - self.context_clear_collection("STATUSTEXT") - self.wait_heading(0, accuracy=10, timeout=60) - self.wait_heading(180, accuracy=10, timeout=60) - ################################################################### - self.start_subtest("BAROMETER: Freeze to last measured value") - self.context_collect("STATUSTEXT") - # create a barometer error by inhibiting any pressure change while changing altitude - old_parameter = self.get_parameter("SIM_BAR2_FREEZE") - self.set_parameter("SIM_BAR2_FREEZE", 1) - self.wait_statustext( - text="EKF3 lane switch", - timeout=30, - the_function=lambda: self.set_rc(2, 2000), - check_context=True) - if self.lane_switches != [1, 0]: - raise NotAchievedException("Expected lane switch 0, got %s" % str(self.lane_switches[-1])) - # Cleanup - self.set_rc(2, 1500) - self.set_parameter("SIM_BAR2_FREEZE", old_parameter) - self.context_clear_collection("STATUSTEXT") - self.wait_heading(0, accuracy=10, timeout=60) - self.wait_heading(180, accuracy=10, timeout=60) - ################################################################### - self.start_subtest("GPS: Apply GPS Velocity Error in NED") - self.context_push() - self.context_collect("STATUSTEXT") - - # create a GPS velocity error by adding a random 2m/s - # noise on each axis - def sim_gps_verr(): - self.set_parameters({ - "SIM_GPS_VERR_X": self.get_parameter("SIM_GPS_VERR_X") + 2, - "SIM_GPS_VERR_Y": self.get_parameter("SIM_GPS_VERR_Y") + 2, - "SIM_GPS_VERR_Z": self.get_parameter("SIM_GPS_VERR_Z") + 2, - }) - self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=sim_gps_verr, check_context=True) - if self.lane_switches != [1, 0, 1]: - raise NotAchievedException("Expected lane switch 1, got %s" % str(self.lane_switches[-1])) - # Cleanup - self.context_pop() - self.context_clear_collection("STATUSTEXT") - self.wait_heading(0, accuracy=10, timeout=60) - self.wait_heading(180, accuracy=10, timeout=60) - ################################################################### - self.start_subtest("MAGNETOMETER: Change X-Axis Offset") - self.context_collect("STATUSTEXT") - # create a magnetometer error by changing the X-axis offset - old_parameter = self.get_parameter("SIM_MAG2_OFS_X") - self.wait_statustext( - text="EKF3 lane switch", - timeout=30, - the_function=self.set_parameter("SIM_MAG2_OFS_X", old_parameter + 150), - check_context=True) - if self.lane_switches != [1, 0, 1, 0]: - raise NotAchievedException("Expected lane switch 0, got %s" % str(self.lane_switches[-1])) - # Cleanup - self.set_parameter("SIM_MAG2_OFS_X", old_parameter) - self.context_clear_collection("STATUSTEXT") - self.wait_heading(0, accuracy=10, timeout=60) - self.wait_heading(180, accuracy=10, timeout=60) - ################################################################### - self.start_subtest("AIRSPEED: Fail to constant value") - self.context_push() - self.context_collect("STATUSTEXT") - - old_parameter = self.get_parameter("SIM_ARSPD_FAIL") - - def fail_speed(): - self.change_mode("GUIDED") - loc = self.mav.location() - self.run_cmd_int( - mavutil.mavlink.MAV_CMD_DO_REPOSITION, - p5=int(loc.lat * 1e7), - p6=int(loc.lng * 1e7), - p7=50, # alt - ) - self.delay_sim_time(5) - # create an airspeed sensor error by freezing to the - # current airspeed then changing the airspeed demand - # to a higher value and waiting for the TECS speed - # loop to diverge - m = self.mav.recv_match(type='VFR_HUD', blocking=True) - self.set_parameter("SIM_ARSPD_FAIL", m.airspeed) - self.run_cmd( - mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, - p1=0, # airspeed - p2=30, - p3=-1, # throttle / no change - p4=0, # absolute values - ) - self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=fail_speed, check_context=True) - if self.lane_switches != [1, 0, 1, 0, 1]: - raise NotAchievedException("Expected lane switch 1, got %s" % str(self.lane_switches[-1])) - # Cleanup - self.set_parameter("SIM_ARSPD_FAIL", old_parameter) - self.change_mode('CIRCLE') - self.context_pop() - self.context_clear_collection("STATUSTEXT") - self.wait_heading(0, accuracy=10, timeout=60) - self.wait_heading(180, accuracy=10, timeout=60) - ################################################################### - self.progress("GYROSCOPE: Change Y-Axis Offset") - self.context_collect("STATUSTEXT") - # create a gyroscope error by changing the Y-axis offset - old_parameter = self.get_parameter("INS_GYR2OFFS_Y") - self.wait_statustext( - text="EKF3 lane switch", - timeout=30, - the_function=self.set_parameter("INS_GYR2OFFS_Y", old_parameter + 1), - check_context=True) - if self.lane_switches != [1, 0, 1, 0, 1, 0]: - raise NotAchievedException("Expected lane switch 0, got %s" % str(self.lane_switches[-1])) - # Cleanup - self.set_parameter("INS_GYR2OFFS_Y", old_parameter) - self.context_clear_collection("STATUSTEXT") - ################################################################### - - self.disarm_vehicle(force=True) + ################################################################### + self.progress("Checking EKF3 Lane Switching trigger from all sensors") + ################################################################### + self.start_subtest("ACCELEROMETER: Change z-axis offset") + # create an accelerometer error by changing the Z-axis offset + self.context_collect("STATUSTEXT") + old_parameter = self.get_parameter("INS_ACCOFFS_Z") + self.wait_statustext( + text="EKF3 lane switch", + timeout=30, + the_function=self.set_parameter("INS_ACCOFFS_Z", old_parameter + 5), + check_context=True) + if self.lane_switches != [1]: + raise NotAchievedException("Expected lane switch 1, got %s" % str(self.lane_switches[-1])) + # Cleanup + self.set_parameter("INS_ACCOFFS_Z", old_parameter) + self.context_clear_collection("STATUSTEXT") + self.wait_heading(0, accuracy=10, timeout=60) + self.wait_heading(180, accuracy=10, timeout=60) + ################################################################### + self.start_subtest("BAROMETER: Freeze to last measured value") + self.context_collect("STATUSTEXT") + # create a barometer error by inhibiting any pressure change while changing altitude + old_parameter = self.get_parameter("SIM_BAR2_FREEZE") + self.set_parameter("SIM_BAR2_FREEZE", 1) + self.wait_statustext( + text="EKF3 lane switch", + timeout=30, + the_function=lambda: self.set_rc(2, 2000), + check_context=True) + if self.lane_switches != [1, 0]: + raise NotAchievedException("Expected lane switch 0, got %s" % str(self.lane_switches[-1])) + # Cleanup + self.set_rc(2, 1500) + self.set_parameter("SIM_BAR2_FREEZE", old_parameter) + self.context_clear_collection("STATUSTEXT") + self.wait_heading(0, accuracy=10, timeout=60) + self.wait_heading(180, accuracy=10, timeout=60) + ################################################################### + self.start_subtest("GPS: Apply GPS Velocity Error in NED") + self.context_push() + self.context_collect("STATUSTEXT") - except Exception as e: - self.print_exception_caught(e) - ex = e + # create a GPS velocity error by adding a random 2m/s + # noise on each axis + def sim_gps_verr(): + self.set_parameters({ + "SIM_GPS_VERR_X": self.get_parameter("SIM_GPS_VERR_X") + 2, + "SIM_GPS_VERR_Y": self.get_parameter("SIM_GPS_VERR_Y") + 2, + "SIM_GPS_VERR_Z": self.get_parameter("SIM_GPS_VERR_Z") + 2, + }) + self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=sim_gps_verr, check_context=True) + if self.lane_switches != [1, 0, 1]: + raise NotAchievedException("Expected lane switch 1, got %s" % str(self.lane_switches[-1])) + # Cleanup + self.context_pop() + self.context_clear_collection("STATUSTEXT") + self.wait_heading(0, accuracy=10, timeout=60) + self.wait_heading(180, accuracy=10, timeout=60) + ################################################################### + self.start_subtest("MAGNETOMETER: Change X-Axis Offset") + self.context_collect("STATUSTEXT") + # create a magnetometer error by changing the X-axis offset + old_parameter = self.get_parameter("SIM_MAG2_OFS_X") + self.wait_statustext( + text="EKF3 lane switch", + timeout=30, + the_function=self.set_parameter("SIM_MAG2_OFS_X", old_parameter + 150), + check_context=True) + if self.lane_switches != [1, 0, 1, 0]: + raise NotAchievedException("Expected lane switch 0, got %s" % str(self.lane_switches[-1])) + # Cleanup + self.set_parameter("SIM_MAG2_OFS_X", old_parameter) + self.context_clear_collection("STATUSTEXT") + self.wait_heading(0, accuracy=10, timeout=60) + self.wait_heading(180, accuracy=10, timeout=60) + ################################################################### + self.start_subtest("AIRSPEED: Fail to constant value") + self.context_push() + self.context_collect("STATUSTEXT") - self.remove_message_hook(statustext_hook) + old_parameter = self.get_parameter("SIM_ARSPD_FAIL") + def fail_speed(): + self.change_mode("GUIDED") + loc = self.mav.location() + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_REPOSITION, + p5=int(loc.lat * 1e7), + p6=int(loc.lng * 1e7), + p7=50, # alt + ) + self.delay_sim_time(5) + # create an airspeed sensor error by freezing to the + # current airspeed then changing the airspeed demand + # to a higher value and waiting for the TECS speed + # loop to diverge + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + self.set_parameter("SIM_ARSPD_FAIL", m.airspeed) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, + p1=0, # airspeed + p2=30, + p3=-1, # throttle / no change + p4=0, # absolute values + ) + self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=fail_speed, check_context=True) + if self.lane_switches != [1, 0, 1, 0, 1]: + raise NotAchievedException("Expected lane switch 1, got %s" % str(self.lane_switches[-1])) + # Cleanup + self.set_parameter("SIM_ARSPD_FAIL", old_parameter) + self.change_mode('CIRCLE') self.context_pop() + self.context_clear_collection("STATUSTEXT") + self.wait_heading(0, accuracy=10, timeout=60) + self.wait_heading(180, accuracy=10, timeout=60) + ################################################################### + self.progress("GYROSCOPE: Change Y-Axis Offset") + self.context_collect("STATUSTEXT") + # create a gyroscope error by changing the Y-axis offset + old_parameter = self.get_parameter("INS_GYR2OFFS_Y") + self.wait_statustext( + text="EKF3 lane switch", + timeout=30, + the_function=self.set_parameter("INS_GYR2OFFS_Y", old_parameter + 1), + check_context=True) + if self.lane_switches != [1, 0, 1, 0, 1, 0]: + raise NotAchievedException("Expected lane switch 0, got %s" % str(self.lane_switches[-1])) + # Cleanup + self.set_parameter("INS_GYR2OFFS_Y", old_parameter) + self.context_clear_collection("STATUSTEXT") + ################################################################### - # some parameters need reboot to take effect - self.reboot_sitl() - - if ex is not None: - raise ex + self.disarm_vehicle(force=True) def FenceAltCeilFloor(self): '''Tests the fence ceiling and floor''' - fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE self.set_parameters({ "FENCE_TYPE": 9, # Set fence type to max and min alt "FENCE_ACTION": 0, # Set action to report @@ -3663,36 +3496,32 @@ def FenceAltCeilFloor(self): }) # Grab Home Position - self.mav.recv_match(type='HOME_POSITION', blocking=True) - self.homeloc = self.mav.location() + self.wait_ready_to_arm() + startpos = self.mav.location() cruise_alt = 150 self.takeoff(cruise_alt) + # note that while we enable the fence here, since the action + # is set to report-only the fence continues to show as + # not-enabled in the assert calls below self.do_fence_enable() self.progress("Fly above ceiling and check for breach") - self.change_altitude(self.homeloc.alt + cruise_alt + 80) - m = self.mav.recv_match(type='SYS_STATUS', blocking=True) - self.progress("Got (%s)" % str(m)) - if ((m.onboard_control_sensors_health & fence_bit)): - raise NotAchievedException("Fence Ceiling did not breach") + self.change_altitude(startpos.alt + cruise_alt + 80) + self.assert_fence_sys_status(True, False, False) - self.progress("Return to cruise alt and check for breach clear") - self.change_altitude(self.homeloc.alt + cruise_alt) + self.progress("Return to cruise alt") + self.change_altitude(startpos.alt + cruise_alt) - m = self.mav.recv_match(type='SYS_STATUS', blocking=True) - self.progress("Got (%s)" % str(m)) - if (not (m.onboard_control_sensors_health & fence_bit)): - raise NotAchievedException("Fence breach did not clear") + self.progress("Ensure breach has clearned") + self.assert_fence_sys_status(True, False, True) self.progress("Fly below floor and check for breach") - self.change_altitude(self.homeloc.alt + cruise_alt - 80) + self.change_altitude(startpos.alt + cruise_alt - 80) - m = self.mav.recv_match(type='SYS_STATUS', blocking=True) - self.progress("Got (%s)" % str(m)) - if ((m.onboard_control_sensors_health & fence_bit)): - raise NotAchievedException("Fence Floor did not breach") + self.progress("Ensure breach has clearned") + self.assert_fence_sys_status(True, False, False) self.do_fence_disable() @@ -3713,9 +3542,6 @@ def FenceMinAltAutoEnable(self): # check we can takeoff again for i in [1, 2]: # Grab Home Position - self.mav.recv_match(type='HOME_POSITION', blocking=True) - self.homeloc = self.mav.location() - self.wait_ready_to_arm() self.arm_vehicle() # max alt fence should now be enabled @@ -3754,9 +3580,6 @@ def FenceMinAltEnableAutoland(self): }) # Grab Home Position - self.mav.recv_match(type='HOME_POSITION', blocking=True) - self.homeloc = self.mav.location() - self.wait_ready_to_arm() self.arm_vehicle() @@ -3781,9 +3604,6 @@ def FenceMinAltEnableAutoland(self): self.set_rc(3, 1000) # lower throttle # Now check we can land self.fly_home_land_and_disarm() - self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) - self.set_current_waypoint(0, check_afterwards=False) - self.set_rc(3, 1000) # lower throttle def FenceMinAltAutoEnableAbort(self): '''Tests autoenablement of the alt min fence and fences on arming''' @@ -3797,10 +3617,6 @@ def FenceMinAltAutoEnableAbort(self): "RTL_AUTOLAND" : 2, }) - # Grab Home Position - self.mav.recv_match(type='HOME_POSITION', blocking=True) - self.homeloc = self.mav.location() - self.wait_ready_to_arm() self.arm_vehicle() @@ -3837,7 +3653,6 @@ def FenceMinAltAutoEnableAbort(self): self.change_mode("AUTO") self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) self.fly_home_land_and_disarm(timeout=150) - self.wait_disarmed() def FenceAutoEnableDisableSwitch(self): '''Tests autoenablement of regular fences and manual disablement''' @@ -3859,7 +3674,6 @@ def FenceAutoEnableDisableSwitch(self): fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE # Grab Home Position self.mav.recv_match(type='HOME_POSITION', blocking=True) - self.homeloc = self.mav.location() self.set_rc_from_map({7: 1000}) # Turn fence off with aux function self.wait_ready_to_arm() @@ -3868,7 +3682,7 @@ def FenceAutoEnableDisableSwitch(self): self.progress("Fly above ceiling and check there is no breach") self.set_rc(3, 2000) - self.change_altitude(self.homeloc.alt + cruise_alt + 80) + self.change_altitude(cruise_alt + 80, relative=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) self.progress("Got (%s)" % str(m)) if (not (m.onboard_control_sensors_health & fence_bit)): @@ -3876,10 +3690,10 @@ def FenceAutoEnableDisableSwitch(self): self.progress("Return to cruise alt") self.set_rc(3, 1500) - self.change_altitude(self.homeloc.alt + cruise_alt) + self.change_altitude(cruise_alt, relative=True) self.progress("Fly below floor and check for no breach") - self.change_altitude(self.homeloc.alt + 25) + self.change_altitude(25, relative=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) self.progress("Got (%s)" % str(m)) if (not (m.onboard_control_sensors_health & fence_bit)): @@ -3887,7 +3701,7 @@ def FenceAutoEnableDisableSwitch(self): self.progress("Fly above floor and check fence is not re-enabled") self.set_rc(3, 2000) - self.change_altitude(self.homeloc.alt + 75) + self.change_altitude(75, relative=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) self.progress("Got (%s)" % str(m)) if (m.onboard_control_sensors_enabled & fence_bit): @@ -3895,7 +3709,7 @@ def FenceAutoEnableDisableSwitch(self): self.progress("Return to cruise alt") self.set_rc(3, 1500) - self.change_altitude(self.homeloc.alt + cruise_alt) + self.change_altitude(cruise_alt, relative=True) self.fly_home_land_and_disarm(timeout=250) def FenceCircleExclusionAutoEnable(self): @@ -3908,10 +3722,6 @@ def FenceCircleExclusionAutoEnable(self): "RTL_AUTOLAND" : 2, }) - # Grab Home Position - self.mav.recv_match(type='HOME_POSITION', blocking=True) - self.homeloc = self.mav.location() - fence_loc = self.home_position_as_mav_location() self.location_offset_ne(fence_loc, 300, 0) @@ -3922,23 +3732,12 @@ def FenceCircleExclusionAutoEnable(self): } )]) - self.wait_ready_to_arm() - self.arm_vehicle() - self.takeoff(alt=50, mode='TAKEOFF') self.change_mode("FBWA") self.set_rc(3, 1100) # lower throttle self.progress("Waiting for RTL") - tstart = self.get_sim_time() - mode = "RTL" - while not self.mode_is(mode, drain_mav=False): - self.mav.messages['HEARTBEAT'].custom_mode - self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( - self.mav.flightmode, mode, self.get_altitude(relative=True))) - if (self.get_sim_time_cached() > tstart + 120): - raise WaitModeTimeout("Did not change mode") - self.progress("Got mode %s" % mode) + self.wait_mode('RTL') # Now check we can land self.fly_home_land_and_disarm() @@ -4572,6 +4371,349 @@ def LandingDrift(self): self.wait_disarmed(timeout=180) + def TakeoffAuto1(self): + '''Test the behaviour of an AUTO takeoff, pt1. + + Conditions: + - ARSPD_USE=1 + - TKOFF_OPTIONS[0]=0 + - TKOFF_THR_MAX < THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 1.0, + "THR_MAX": 100.0, + "TKOFF_THR_MAX": 80.0, + "TKOFF_THR_MINACC": 3.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + }) + + # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m. + self.load_mission("catapult.txt", strict=True) + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Wait until we're midway through the climb. + test_alt = 50 + self.wait_altitude(test_alt, test_alt+2, relative=True) + + # Ensure that by then the aircraft still goes at max allowed throttle. + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")) + + # Wait for landing waypoint. + self.wait_current_waypoint(11, timeout=1200) + self.wait_disarmed(120) + + def TakeoffAuto2(self): + '''Test the behaviour of an AUTO takeoff, pt2. + + Conditions: + - ARSPD_USE=1 + - TKOFF_OPTIONS[0]=0 + - TKOFF_THR_MAX > THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 0.0, + "THR_MAX": 80.0, + "TKOFF_THR_MAX": 100.0, + "TKOFF_THR_MINACC": 3.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + }) + + # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m. + self.load_mission("catapult.txt", strict=True) + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Wait until we're midway through the climb. + test_alt = 50 + self.wait_altitude(test_alt, test_alt+2, relative=True) + + # Ensure that by then the aircraft still goes at max allowed throttle. + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")) + + # Wait for landing waypoint. + self.wait_current_waypoint(11, timeout=1200) + self.wait_disarmed(120) + + def TakeoffAuto3(self): + '''Test the behaviour of an AUTO takeoff, pt3. + + Conditions: + - ARSPD_USE=1 + - TKOFF_OPTIONS[0]=1 + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 1.0, + "THR_MAX": 80.0, + "THR_MIN": 0.0, + "TKOFF_OPTIONS": 1.0, + "TKOFF_THR_MAX": 100.0, + "TKOFF_THR_MINACC": 3.0, + "TECS_PITCH_MAX": 35.0, + "TKOFF_THR_MAX_T": 3.0, + "PTCH_LIM_MAX_DEG": 35.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + }) + + # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m. + self.load_mission("catapult.txt", strict=True) + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Ensure that TKOFF_THR_MAX_T is respected. + self.delay_sim_time(self.get_parameter("TKOFF_THR_MAX_T")-1) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")-1)) + + # Ensure that after that the aircraft does not go full throttle anymore. + test_alt = 50 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")-10, operator.lt) + + # Wait for landing waypoint. + self.wait_current_waypoint(11, timeout=1200) + self.wait_disarmed(120) + + def TakeoffAuto4(self): + '''Test the behaviour of an AUTO takeoff, pt4. + + Conditions: + - ARSPD_USE=0 + - TKOFF_OPTIONS[0]=1 + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 0.0, + "THR_MAX": 80.0, + "THR_MIN": 0.0, + "TKOFF_OPTIONS": 1.0, + "TKOFF_THR_MAX": 100.0, + "TKOFF_THR_MINACC": 3.0, + "TECS_PITCH_MAX": 35.0, + "TKOFF_THR_MAX_T": 3.0, + "PTCH_LIM_MAX_DEG": 35.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + }) + + # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m. + self.load_mission("catapult.txt", strict=True) + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Ensure that TKOFF_THR_MAX_T is respected. + self.delay_sim_time(self.get_parameter("TKOFF_THR_MAX_T")-1) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-10, operator.ge) + + # Ensure that after that the aircraft still goes to maximum throttle. + test_alt = 50 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-10, operator.ge) + + # Wait for landing waypoint. + self.wait_current_waypoint(11, timeout=1200) + self.wait_disarmed(120) + + def TakeoffTakeoff1(self): + '''Test the behaviour of a takeoff in TAKEOFF mode, pt1. + + Conditions: + - ARSPD_USE=1 + - TKOFF_OPTIONS[0]=0 + - TKOFF_THR_MAX < THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 1.0, + "THR_MAX": 100.0, + "TKOFF_LVL_ALT": 30.0, + "TKOFF_ALT": 100.0, + "TKOFF_OPTIONS": 0.0, + "TKOFF_THR_MINACC": 3.0, + "TKOFF_THR_MAX": 80.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + }) + self.change_mode("TAKEOFF") + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Check whether we're at max throttle below TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")-10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")) + + # Check whether we're still at max throttle past TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")+10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-1, operator.ge) + + # Wait for the takeoff to complete. + target_alt = self.get_parameter("TKOFF_ALT") + self.wait_altitude(target_alt-5, target_alt, relative=True) + + self.fly_home_land_and_disarm() + + def TakeoffTakeoff2(self): + '''Test the behaviour of a takeoff in TAKEOFF mode, pt2. + + Conditions: + - ARSPD_USE=1 + - TKOFF_OPTIONS[0]=1 + - TKOFF_THR_MAX < THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 1.0, + "THR_MAX": 100.0, + "TKOFF_LVL_ALT": 80.0, + "TKOFF_ALT": 150.0, + "TKOFF_OPTIONS": 1.0, + "TKOFF_THR_MINACC": 3.0, + "TKOFF_THR_MAX": 80.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + }) + self.change_mode("TAKEOFF") + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Check whether we're at max throttle below TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")-10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-1, operator.ge) + + # Check whether we've receded from max throttle past TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")+10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MIN"))-1, operator.ge) + + # Wait for the takeoff to complete. + target_alt = self.get_parameter("TKOFF_ALT") + self.wait_altitude(target_alt-5, target_alt, relative=True) + + self.fly_home_land_and_disarm() + + def TakeoffTakeoff3(self): + '''Test the behaviour of a takeoff in TAKEOFF mode, pt3. + + This is the same as case #1, but with disabled airspeed sensor. + + Conditions: + - ARSPD_USE=0 + - TKOFF_OPTIONS[0]=0 + - TKOFF_THR_MAX < THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 0.0, + "THR_MAX": 100.0, + "TKOFF_LVL_ALT": 30.0, + "TKOFF_ALT": 100.0, + "TKOFF_OPTIONS": 0.0, + "TKOFF_THR_MINACC": 3.0, + "TKOFF_THR_MAX": 80.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + }) + self.change_mode("TAKEOFF") + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Check whether we're at max throttle below TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")-10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")) + + # Check whether we're still at max throttle past TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")+10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-1, operator.ge) + + # Wait for the takeoff to complete. + target_alt = self.get_parameter("TKOFF_ALT") + self.wait_altitude(target_alt-5, target_alt, relative=True) + + self.fly_home_land_and_disarm() + def DCMFallback(self): '''Really annoy the EKF and force fallback''' self.reboot_sitl() @@ -5873,7 +6015,8 @@ def tests(self): self.FenceNoFenceReturnPoint, self.FenceNoFenceReturnPointInclusion, self.FenceDisableUnderAction, - self.ADSB, + self.ADSBFailActionRTL, + self.ADSBResumeActionResumeLoiter, self.SimADSB, self.Button, self.FRSkySPort, @@ -5912,6 +6055,13 @@ def tests(self): self.AHRS_ORIENTATION, self.AHRSTrim, self.LandingDrift, + self.TakeoffAuto1, + self.TakeoffAuto2, + self.TakeoffAuto3, + self.TakeoffAuto4, + self.TakeoffTakeoff1, + self.TakeoffTakeoff2, + self.TakeoffTakeoff3, self.ForcedDCM, self.DCMFallback, self.MAVFTP, diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 635785a22897f0..e33b4a588b181e 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -36,6 +36,22 @@ class Joystick(): Lateral = 6 +# Values for EK3_MAG_CAL +class MagCal(): + WHEN_FLYING = 0 + WHEN_MANOEUVRING = 1 + NEVER = 2 + AFTER_FIRST_CLIMB = 3 + ALWAYS = 4 + + +# Values for XKFS.MAG_FUSION +class MagFuseSel(): + NOT_FUSING = 0 + FUSE_YAW = 1 + FUSE_MAG = 2 + + class AutoTestSub(vehicle_test_suite.TestSuite): @staticmethod def get_not_armable_mode_list(): @@ -739,6 +755,35 @@ def MAV_CMD_CONDITION_YAW(self): self._MAV_CMD_CONDITION_YAW(self.run_cmd) self._MAV_CMD_CONDITION_YAW(self.run_cmd_int) + def MAV_CMD_DO_REPOSITION(self): + """Move vehicle using MAV_CMD_DO_REPOSITION""" + self.wait_ready_to_arm() + self.arm_vehicle() + + # Dive so that rangefinder is in range, required for MAV_FRAME_GLOBAL_TERRAIN_ALT + start_altitude = -25 + pwm = 1300 if self.get_altitude(relative=True) > start_altitude else 1700 + self.set_rc(Joystick.Throttle, pwm) + self.wait_altitude(altitude_min=start_altitude-1, altitude_max=start_altitude, relative=False, timeout=120) + self.set_rc(Joystick.Throttle, 1500) + self.change_mode('GUIDED') + + loc = self.mav.location() + + # Reposition, alt relative to surface + loc = self.offset_location_ne(loc, 10, 10) + loc.alt = start_altitude + self.send_do_reposition(loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT) + self.wait_location(loc, timeout=120) + + # Reposition, alt relative to seafloor + loc = self.offset_location_ne(loc, 10, 10) + loc.alt = -start_altitude + self.send_do_reposition(loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT) + self.wait_location(loc, timeout=120) + + self.disarm_vehicle() + def TerrainMission(self): """Mission using surface tracking""" @@ -821,7 +866,7 @@ def SetGlobalOrigin(self): # restart GPS driver self.reboot_sitl() - def backup_home(self): + def backup_origin(self): """Test ORIGIN_LAT and ORIGIN_LON parameters""" self.context_push() @@ -852,6 +897,55 @@ def backup_home(self): self.disarm_vehicle() self.context_pop() + def assert_mag_fusion_selection(self, expect_sel): + """Get the most recent XKFS message and check the MAG_FUSION value""" + self.progress("Expect mag fusion selection %d" % expect_sel) + mlog = self.dfreader_for_current_onboard_log() + found_sel = MagFuseSel.NOT_FUSING + while True: + m = mlog.recv_match(type='XKFS') + if m is None: + break + found_sel = m.MAG_FUSION + if found_sel != expect_sel: + raise NotAchievedException("Expected mag fusion selection %d, found %d" % (expect_sel, found_sel)) + + def fuse_mag(self): + """Test EK3_MAG_CAL values""" + + # WHEN_FLYING: switch to FUSE_MAG after sub is armed for 5 seconds; switch to FUSE_YAW on disarm + self.set_parameters({'EK3_MAG_CAL': MagCal.WHEN_FLYING}) + self.reboot_sitl() + self.wait_ready_to_arm() + self.assert_mag_fusion_selection(MagFuseSel.FUSE_YAW) + self.arm_vehicle() + self.delay_sim_time(10) + self.assert_mag_fusion_selection(MagFuseSel.FUSE_MAG) + self.disarm_vehicle() + self.delay_sim_time(1) + self.assert_mag_fusion_selection(MagFuseSel.FUSE_YAW) + + # AFTER_FIRST_CLIMB: switch to FUSE_MAG after sub is armed and descends 0.5m; switch to FUSE_YAW on disarm + self.set_parameters({'EK3_MAG_CAL': MagCal.AFTER_FIRST_CLIMB}) + self.reboot_sitl() + self.wait_ready_to_arm() + self.assert_mag_fusion_selection(MagFuseSel.FUSE_YAW) + altitude = self.get_altitude(relative=True) + self.arm_vehicle() + self.set_rc(Joystick.Throttle, 1300) + self.wait_altitude(altitude_min=altitude-4, altitude_max=altitude-3, relative=False, timeout=60) + self.set_rc(Joystick.Throttle, 1500) + self.assert_mag_fusion_selection(MagFuseSel.FUSE_MAG) + self.disarm_vehicle() + self.delay_sim_time(1) + self.assert_mag_fusion_selection(MagFuseSel.FUSE_YAW) + + # ALWAYS + self.set_parameters({'EK3_MAG_CAL': MagCal.ALWAYS}) + self.reboot_sitl() + self.wait_ready_to_arm() + self.assert_mag_fusion_selection(MagFuseSel.FUSE_MAG) + def tests(self): '''return list of all tests''' ret = super(AutoTestSub, self).tests() @@ -877,9 +971,11 @@ def tests(self): self.MAV_CMD_MISSION_START, self.MAV_CMD_DO_CHANGE_SPEED, self.MAV_CMD_CONDITION_YAW, + self.MAV_CMD_DO_REPOSITION, self.TerrainMission, self.SetGlobalOrigin, - self.backup_home, + self.backup_origin, + self.fuse_mag, ]) return ret diff --git a/Tools/autotest/helicopter.py b/Tools/autotest/helicopter.py index 91c96e03643c18..14fbb1513ddd8d 100644 --- a/Tools/autotest/helicopter.py +++ b/Tools/autotest/helicopter.py @@ -224,101 +224,67 @@ def hover(self): def PosHoldTakeOff(self): """ensure vehicle stays put until it is ready to fly""" - self.context_push() - - ex = None - try: - self.set_parameter("PILOT_TKOFF_ALT", 700) - self.change_mode('POSHOLD') - self.zero_throttle() - self.set_rc(8, 1000) - self.wait_ready_to_arm() - # Arm - self.arm_vehicle() - self.progress("Raising rotor speed") - self.set_rc(8, 2000) - self.progress("wait for rotor runup to complete") - self.wait_servo_channel_value(8, 1659, timeout=10) - self.delay_sim_time(20) - # check we are still on the ground... - m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) - max_relalt_mm = 1000 - if abs(m.relative_alt) > max_relalt_mm: - raise NotAchievedException("Took off prematurely (abs(%f)>%f)" % - (m.relative_alt, max_relalt_mm)) - - self.progress("Pushing collective past half-way") - self.set_rc(3, 1600) - self.delay_sim_time(0.5) - self.hover() - - # make sure we haven't already reached alt: - m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) - max_initial_alt = 1500 - if abs(m.relative_alt) > max_initial_alt: - raise NotAchievedException("Took off too fast (%f > %f" % - (abs(m.relative_alt), max_initial_alt)) - - self.progress("Monitoring takeoff-to-alt") - self.wait_altitude(6.9, 8, relative=True) - - self.progress("Making sure we stop at our takeoff altitude") - tstart = self.get_sim_time() - while self.get_sim_time() - tstart < 5: - m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) - delta = abs(7000 - m.relative_alt) - self.progress("alt=%f delta=%f" % (m.relative_alt/1000, - delta/1000)) - if delta > 1000: - raise NotAchievedException("Failed to maintain takeoff alt") - self.progress("takeoff OK") - except Exception as e: - self.print_exception_caught(e) - ex = e + self.set_parameter("PILOT_TKOFF_ALT", 700) + self.change_mode('POSHOLD') + self.zero_throttle() + self.set_rc(8, 1000) + self.wait_ready_to_arm() + # Arm + self.arm_vehicle() + self.progress("Raising rotor speed") + self.set_rc(8, 2000) + self.progress("wait for rotor runup to complete") + self.wait_servo_channel_value(8, 1659, timeout=10) + self.delay_sim_time(20) + # check we are still on the ground... + max_relalt = 1 # metres + relative_alt = self.get_altitude(relative=True) + if abs(relative_alt) > max_relalt: + raise NotAchievedException("Took off prematurely (abs(%f)>%f)" % + (relative_alt, max_relalt)) + + self.progress("Pushing collective past half-way") + self.set_rc(3, 1600) + self.delay_sim_time(0.5) + self.hover() - self.land_and_disarm() + # make sure we haven't already reached alt: + relative_alt = self.get_altitude(relative=True) + max_initial_alt = 1.5 # metres + if abs(relative_alt) > max_initial_alt: + raise NotAchievedException("Took off too fast (%f > %f" % + (abs(relative_alt), max_initial_alt)) - self.context_pop() + self.progress("Monitoring takeoff-to-alt") + self.wait_altitude(6, 8, relative=True, minimum_duration=5) + self.progress("takeoff OK") - if ex is not None: - raise ex + self.land_and_disarm() def StabilizeTakeOff(self): """Fly stabilize takeoff""" - self.context_push() - - ex = None - try: - self.change_mode('STABILIZE') - self.set_rc(3, 1000) - self.set_rc(8, 1000) - self.wait_ready_to_arm() - self.arm_vehicle() - self.set_rc(8, 2000) - self.progress("wait for rotor runup to complete") - self.wait_servo_channel_value(8, 1659, timeout=10) - self.delay_sim_time(20) - # check we are still on the ground... - m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) - if abs(m.relative_alt) > 100: - raise NotAchievedException("Took off prematurely") - self.progress("Pushing throttle past half-way") - self.set_rc(3, 1650) - - self.progress("Monitoring takeoff") - self.wait_altitude(6.9, 8, relative=True) - - self.progress("takeoff OK") - except Exception as e: - self.print_exception_caught(e) - ex = e + self.change_mode('STABILIZE') + self.set_rc(3, 1000) + self.set_rc(8, 1000) + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_rc(8, 2000) + self.progress("wait for rotor runup to complete") + self.wait_servo_channel_value(8, 1659, timeout=10) + self.delay_sim_time(20) + # check we are still on the ground... + relative_alt = self.get_altitude(relative=True) + if abs(relative_alt) > 0.1: + raise NotAchievedException("Took off prematurely") + self.progress("Pushing throttle past half-way") + self.set_rc(3, 1650) - self.land_and_disarm() + self.progress("Monitoring takeoff") + self.wait_altitude(6.9, 8, relative=True) - self.context_pop() + self.progress("takeoff OK") - if ex is not None: - raise ex + self.land_and_disarm() def SplineWaypoint(self, timeout=600): """ensure basic spline functionality works""" @@ -331,13 +297,7 @@ def SplineWaypoint(self, timeout=600): self.delay_sim_time(20) self.change_mode("AUTO") self.set_rc(3, 1500) - tstart = self.get_sim_time() - while True: - if self.get_sim_time() - tstart > timeout: - raise AutoTestTimeoutException("Vehicle did not disarm after mission") - if not self.armed(): - break - self.delay_sim_time(1) + self.wait_disarmed(timeout=600) self.progress("Lowering rotor speed") self.set_rc(8, 1000) @@ -380,13 +340,15 @@ def ManAutoRotation(self, timeout=600): """Check autorotation power recovery behaviour""" RAMP_TIME = 4 AROT_RAMP_TIME = 2 - self.set_parameter("H_RSC_AROT_MN_EN", 1) - self.set_parameter("H_RSC_AROT_ENG_T", AROT_RAMP_TIME) - self.set_parameter("H_RSC_AROT_IDLE", 20) - self.set_parameter("H_RSC_RAMP_TIME", RAMP_TIME) - self.set_parameter("H_RSC_IDLE", 0) start_alt = 100 # metres - self.set_parameter("PILOT_TKOFF_ALT", start_alt * 100) + self.set_parameters({ + "H_RSC_AROT_MN_EN": 1, + "H_RSC_AROT_ENG_T": AROT_RAMP_TIME, + "H_RSC_AROT_IDLE": 20, + "H_RSC_RAMP_TIME": RAMP_TIME, + "H_RSC_IDLE": 0, + "PILOT_TKOFF_ALT": start_alt * 100, + }) self.change_mode('POSHOLD') self.set_rc(3, 1000) self.set_rc(8, 1000) @@ -680,7 +642,14 @@ def fly_mission(self, filename, strict=True): def AirspeedDrivers(self, timeout=600): '''Test AirSpeed drivers''' + # Copter's airspeed sensors are off by default + self.set_parameters({ + "ARSPD_ENABLE": 1, + "ARSPD_TYPE": 2, # Analog airspeed driver + "ARSPD_PIN": 1, # Analog airspeed driver pin for SITL + }) # set the start location to CMAC to use same test script as other vehicles + self.sitl_start_loc = mavutil.location(-35.362881, 149.165222, 582.000000, 90.0) # CMAC self.customise_SITL_commandline(["--home", "%s,%s,%s,%s" % (-35.362881, 149.165222, 582.000000, 90.0)]) @@ -703,12 +672,6 @@ def check_airspeeds(mav, m): if delta > 3: raise NotAchievedException("Airspeed mismatch (as1=%f as2=%f)" % (airspeed[0], airspeed[1])) - # Copter's airspeed sensors are off by default - self.set_parameter("ARSPD_ENABLE", 1) - self.set_parameter("ARSPD_TYPE", 2) # Analog airspeed driver - self.set_parameter("ARSPD_PIN", 1) # Analog airspeed driver pin for SITL - self.reboot_sitl() - airspeed_sensors = [ ("MS5525", 3, 1), ("DLVR", 7, 2), @@ -734,8 +697,6 @@ def check_airspeeds(mav, m): self.disarm_vehicle() self.context_pop() - self.reboot_sitl() - def TurbineStart(self, timeout=200): """Check Turbine Start Feature""" RAMP_TIME = 4 @@ -806,43 +767,28 @@ def TurbineStart(self, timeout=200): def PIDNotches(self): """Use dynamic harmonic notch to control motor noise.""" self.progress("Flying with PID notches") - self.context_push() - - ex = None - try: - self.set_parameters({ - "FILT1_TYPE": 1, - "FILT2_TYPE": 1, - "AHRS_EKF_TYPE": 10, - "INS_LOG_BAT_MASK": 3, - "INS_LOG_BAT_OPT": 0, - "INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour - "LOG_BITMASK": 65535, - "LOG_DISARMED": 0, - "SIM_VIB_FREQ_X": 120, # roll - "SIM_VIB_FREQ_Y": 120, # pitch - "SIM_VIB_FREQ_Z": 180, # yaw - "FILT1_NOTCH_FREQ": 120, - "FILT2_NOTCH_FREQ": 180, - "ATC_RAT_RLL_NEF": 1, - "ATC_RAT_PIT_NEF": 1, - "ATC_RAT_YAW_NEF": 2, - "SIM_GYR1_RND": 5, - }) - self.reboot_sitl() - - self.takeoff(10, mode="ALT_HOLD") - - freq, hover_throttle, peakdb1 = self.hover_and_check_matched_frequency_with_fft(5, 20, 350, reverse=True) - - except Exception as e: - self.print_exception_caught(e) - ex = e - - self.context_pop() + self.set_parameters({ + "FILT1_TYPE": 1, + "FILT2_TYPE": 1, + "AHRS_EKF_TYPE": 10, + "INS_LOG_BAT_MASK": 3, + "INS_LOG_BAT_OPT": 0, + "INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour + "LOG_BITMASK": 65535, + "LOG_DISARMED": 0, + "SIM_VIB_FREQ_X": 120, # roll + "SIM_VIB_FREQ_Y": 120, # pitch + "SIM_VIB_FREQ_Z": 180, # yaw + "FILT1_NOTCH_FREQ": 120, + "FILT2_NOTCH_FREQ": 180, + "ATC_RAT_RLL_NEF": 1, + "ATC_RAT_PIT_NEF": 1, + "ATC_RAT_YAW_NEF": 2, + "SIM_GYR1_RND": 5, + }) + self.reboot_sitl() - if ex is not None: - raise ex + self.hover_and_check_matched_frequency_with_fft(5, 20, 350, reverse=True, takeoff=True) def AutoTune(self): """Test autotune mode""" diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index b11920ead0361d..ad4deeb0f3f0da 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -227,108 +227,98 @@ def drive_left_circuit(self): def Sprayer(self): """Test sprayer functionality.""" - self.context_push() - ex = None - try: - rc_ch = 5 - pump_ch = 5 - spinner_ch = 6 - pump_ch_min = 1050 - pump_ch_trim = 1520 - pump_ch_max = 1950 - spinner_ch_min = 975 - spinner_ch_trim = 1510 - spinner_ch_max = 1975 + rc_ch = 5 + pump_ch = 5 + spinner_ch = 6 + pump_ch_min = 1050 + pump_ch_trim = 1520 + pump_ch_max = 1950 + spinner_ch_min = 975 + spinner_ch_trim = 1510 + spinner_ch_max = 1975 - self.set_parameters({ - "SPRAY_ENABLE": 1, + self.set_parameters({ + "SPRAY_ENABLE": 1, - "SERVO%u_FUNCTION" % pump_ch: 22, - "SERVO%u_MIN" % pump_ch: pump_ch_min, - "SERVO%u_TRIM" % pump_ch: pump_ch_trim, - "SERVO%u_MAX" % pump_ch: pump_ch_max, + "SERVO%u_FUNCTION" % pump_ch: 22, + "SERVO%u_MIN" % pump_ch: pump_ch_min, + "SERVO%u_TRIM" % pump_ch: pump_ch_trim, + "SERVO%u_MAX" % pump_ch: pump_ch_max, - "SERVO%u_FUNCTION" % spinner_ch: 23, - "SERVO%u_MIN" % spinner_ch: spinner_ch_min, - "SERVO%u_TRIM" % spinner_ch: spinner_ch_trim, - "SERVO%u_MAX" % spinner_ch: spinner_ch_max, + "SERVO%u_FUNCTION" % spinner_ch: 23, + "SERVO%u_MIN" % spinner_ch: spinner_ch_min, + "SERVO%u_TRIM" % spinner_ch: spinner_ch_trim, + "SERVO%u_MAX" % spinner_ch: spinner_ch_max, - "SIM_SPR_ENABLE": 1, - "SIM_SPR_PUMP": pump_ch, - "SIM_SPR_SPIN": spinner_ch, + "SIM_SPR_ENABLE": 1, + "SIM_SPR_PUMP": pump_ch, + "SIM_SPR_SPIN": spinner_ch, - "RC%u_OPTION" % rc_ch: 15, - "LOG_DISARMED": 1, - }) + "RC%u_OPTION" % rc_ch: 15, + "LOG_DISARMED": 1, + }) - self.reboot_sitl() + self.reboot_sitl() - self.wait_ready_to_arm() - self.arm_vehicle() + self.wait_ready_to_arm() + self.arm_vehicle() - self.progress("test bootup state - it's zero-output!") - self.wait_servo_channel_value(spinner_ch, 0) - self.wait_servo_channel_value(pump_ch, 0) + self.progress("test bootup state - it's zero-output!") + self.wait_servo_channel_value(spinner_ch, 0) + self.wait_servo_channel_value(pump_ch, 0) - self.progress("Enable sprayer") - self.set_rc(rc_ch, 2000) + self.progress("Enable sprayer") + self.set_rc(rc_ch, 2000) - self.progress("Testing zero-speed state") - self.wait_servo_channel_value(spinner_ch, spinner_ch_min) - self.wait_servo_channel_value(pump_ch, pump_ch_min) + self.progress("Testing zero-speed state") + self.wait_servo_channel_value(spinner_ch, spinner_ch_min) + self.wait_servo_channel_value(pump_ch, pump_ch_min) - self.progress("Testing turning it off") - self.set_rc(rc_ch, 1000) - self.wait_servo_channel_value(spinner_ch, spinner_ch_min) - self.wait_servo_channel_value(pump_ch, pump_ch_min) + self.progress("Testing turning it off") + self.set_rc(rc_ch, 1000) + self.wait_servo_channel_value(spinner_ch, spinner_ch_min) + self.wait_servo_channel_value(pump_ch, pump_ch_min) - self.progress("Testing turning it back on") - self.set_rc(rc_ch, 2000) - self.wait_servo_channel_value(spinner_ch, spinner_ch_min) - self.wait_servo_channel_value(pump_ch, pump_ch_min) + self.progress("Testing turning it back on") + self.set_rc(rc_ch, 2000) + self.wait_servo_channel_value(spinner_ch, spinner_ch_min) + self.wait_servo_channel_value(pump_ch, pump_ch_min) - self.progress("Testing speed-ramping") - self.set_rc(3, 1700) # start driving forward + self.progress("Testing speed-ramping") + self.set_rc(3, 1700) # start driving forward - # this is somewhat empirical... - self.wait_servo_channel_value(pump_ch, 1695, timeout=60) + # this is somewhat empirical... + self.wait_servo_channel_value(pump_ch, 1695, timeout=60) - self.progress("Turning it off again") - self.set_rc(rc_ch, 1000) - self.wait_servo_channel_value(spinner_ch, spinner_ch_min) - self.wait_servo_channel_value(pump_ch, pump_ch_min) + self.progress("Turning it off again") + self.set_rc(rc_ch, 1000) + self.wait_servo_channel_value(spinner_ch, spinner_ch_min) + self.wait_servo_channel_value(pump_ch, pump_ch_min) - self.start_subtest("Sprayer Mission") - self.load_mission("sprayer-mission.txt") - self.change_mode("AUTO") + self.start_subtest("Sprayer Mission") + self.load_mission("sprayer-mission.txt") + self.change_mode("AUTO") # self.send_debug_trap() - self.progress("Waiting for sprayer to start") - self.wait_servo_channel_value(pump_ch, 1250, timeout=60, comparator=operator.gt) - self.progress("Waiting for sprayer to stop") - self.wait_servo_channel_value(pump_ch, pump_ch_min, timeout=120) + self.progress("Waiting for sprayer to start") + self.wait_servo_channel_value(pump_ch, 1250, timeout=60, comparator=operator.gt) + self.progress("Waiting for sprayer to stop") + self.wait_servo_channel_value(pump_ch, pump_ch_min, timeout=120) - self.start_subtest("Checking mavlink commands") - self.change_mode("MANUAL") - self.progress("Starting Sprayer") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=1) - - self.progress("Testing speed-ramping") - self.set_rc(3, 1700) # start driving forward - self.wait_servo_channel_value(pump_ch, 1690, timeout=60, comparator=operator.gt) - self.start_subtest("Stopping Sprayer") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=0) - self.wait_servo_channel_value(pump_ch, pump_ch_min) - self.set_rc(3, 1000) # start driving forward - - self.progress("Sprayer OK") - except Exception as e: - self.print_exception_caught(e) - ex = e - self.disarm_vehicle(force=True) - self.context_pop() - self.reboot_sitl() - if ex: - raise ex + self.start_subtest("Checking mavlink commands") + self.change_mode("MANUAL") + self.progress("Starting Sprayer") + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=1) + + self.progress("Testing speed-ramping") + self.set_rc(3, 1700) # start driving forward + self.wait_servo_channel_value(pump_ch, 1690, timeout=60, comparator=operator.gt) + self.start_subtest("Stopping Sprayer") + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=0) + self.wait_servo_channel_value(pump_ch, pump_ch_min) + self.set_rc(3, 1000) # stop driving forward + + self.progress("Sprayer OK") + self.disarm_vehicle() def DriveMaxRCIN(self, timeout=30): """Drive rover at max RC inputs""" @@ -538,42 +528,30 @@ def RTL_SPEED(self, timeout=120): def AC_Avoidance(self): '''Test AC Avoidance switch''' - self.context_push() - ex = None - try: - self.load_fence("rover-fence-ac-avoid.txt") - self.set_parameters({ - "FENCE_ENABLE": 0, - "PRX1_TYPE": 10, - "RC10_OPTION": 40, # proximity-enable - }) - self.reboot_sitl() - # start = self.mav.location() - self.wait_ready_to_arm() - self.arm_vehicle() - # first make sure we can breach the fence: - self.set_rc(10, 1000) - self.change_mode("ACRO") - self.set_rc(3, 1550) - self.wait_distance_to_home(25, 100000, timeout=60) - self.change_mode("RTL") - self.wait_statustext("Reached destination", timeout=60) - # now enable avoidance and make sure we can't: - self.set_rc(10, 2000) - self.change_mode("ACRO") - self.wait_groundspeed(0, 0.7, timeout=60) - # watch for speed zero - self.wait_groundspeed(0, 0.2, timeout=120) - - except Exception as e: - self.print_exception_caught(e) - ex = e - self.disarm_vehicle(force=True) - self.context_pop() - self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + self.load_fence("rover-fence-ac-avoid.txt") + self.set_parameters({ + "FENCE_ENABLE": 0, + "PRX1_TYPE": 10, + "RC10_OPTION": 40, # proximity-enable + }) self.reboot_sitl() - if ex: - raise ex + # start = self.mav.location() + self.wait_ready_to_arm() + self.arm_vehicle() + # first make sure we can breach the fence: + self.set_rc(10, 1000) + self.change_mode("ACRO") + self.set_rc(3, 1550) + self.wait_distance_to_home(25, 100000, timeout=60) + self.change_mode("RTL") + self.wait_statustext("Reached destination", timeout=60) + # now enable avoidance and make sure we can't: + self.set_rc(10, 2000) + self.change_mode("ACRO") + self.wait_groundspeed(0, 0.7, timeout=60) + # watch for speed zero + self.wait_groundspeed(0, 0.2, timeout=120) + self.disarm_vehicle() def ServoRelayEvents(self): '''Test ServoRelayEvents''' @@ -745,75 +723,53 @@ def MAVProxy_SetModeUsingMode(self): def ModeSwitch(self): ''''Set modes via modeswitch''' - self.context_push() - ex = None - try: - self.set_parameter("MODE_CH", 8) - self.set_rc(8, 1000) - # mavutil.mavlink.ROVER_MODE_HOLD: - self.set_parameter("MODE6", 4) - # mavutil.mavlink.ROVER_MODE_ACRO - self.set_parameter("MODE5", 1) - self.set_rc(8, 1800) # PWM for mode6 - self.wait_mode("HOLD") - self.set_rc(8, 1700) # PWM for mode5 - self.wait_mode("ACRO") - self.set_rc(8, 1800) # PWM for mode6 - self.wait_mode("HOLD") - self.set_rc(8, 1700) # PWM for mode5 - self.wait_mode("ACRO") - except Exception as e: - self.print_exception_caught(e) - ex = e - - self.context_pop() - - if ex is not None: - raise ex + self.set_parameter("MODE_CH", 8) + self.set_rc(8, 1000) + # mavutil.mavlink.ROVER_MODE_HOLD: + self.set_parameter("MODE6", 4) + # mavutil.mavlink.ROVER_MODE_ACRO + self.set_parameter("MODE5", 1) + self.set_rc(8, 1800) # PWM for mode6 + self.wait_mode("HOLD") + self.set_rc(8, 1700) # PWM for mode5 + self.wait_mode("ACRO") + self.set_rc(8, 1800) # PWM for mode6 + self.wait_mode("HOLD") + self.set_rc(8, 1700) # PWM for mode5 + self.wait_mode("ACRO") def AuxModeSwitch(self): '''Set modes via auxswitches''' - self.context_push() - ex = None - try: - # from mavproxy_rc.py - mapping = [0, 1165, 1295, 1425, 1555, 1685, 1815] - self.set_parameter("MODE1", 1) # acro - self.set_rc(8, mapping[1]) - self.wait_mode('ACRO') - - self.set_rc(9, 1000) - self.set_rc(10, 1000) - self.set_parameters({ - "RC9_OPTION": 53, # steering - "RC10_OPTION": 54, # hold - }) - self.set_rc(9, 1900) - self.wait_mode("STEERING") - self.set_rc(10, 1900) - self.wait_mode("HOLD") - - # reset both switches - should go back to ACRO - self.set_rc(9, 1000) - self.set_rc(10, 1000) - self.wait_mode("ACRO") - - self.set_rc(9, 1900) - self.wait_mode("STEERING") - self.set_rc(10, 1900) - self.wait_mode("HOLD") - - self.set_rc(10, 1000) # this re-polls the mode switch - self.wait_mode("ACRO") - self.set_rc(9, 1000) - except Exception as e: - self.print_exception_caught(e) - ex = e + # from mavproxy_rc.py + mapping = [0, 1165, 1295, 1425, 1555, 1685, 1815] + self.set_parameter("MODE1", 1) # acro + self.set_rc(8, mapping[1]) + self.wait_mode('ACRO') - self.context_pop() + self.set_rc(9, 1000) + self.set_rc(10, 1000) + self.set_parameters({ + "RC9_OPTION": 53, # steering + "RC10_OPTION": 54, # hold + }) + self.set_rc(9, 1900) + self.wait_mode("STEERING") + self.set_rc(10, 1900) + self.wait_mode("HOLD") + + # reset both switches - should go back to ACRO + self.set_rc(9, 1000) + self.set_rc(10, 1000) + self.wait_mode("ACRO") - if ex is not None: - raise ex + self.set_rc(9, 1900) + self.wait_mode("STEERING") + self.set_rc(10, 1900) + self.wait_mode("HOLD") + + self.set_rc(10, 1000) # this re-polls the mode switch + self.wait_mode("ACRO") + self.set_rc(9, 1000) def RCOverridesCancel(self): '''Test RC overrides Cancel''' @@ -888,337 +844,314 @@ def RCOverridesCancel(self): def RCOverrides(self): '''Test RC overrides''' - self.context_push() self.set_parameter("SYSID_MYGCS", self.mav.source_system) - ex = None - try: - self.set_parameter("RC12_OPTION", 46) - self.reboot_sitl() - - self.change_mode('MANUAL') - self.wait_ready_to_arm() - self.set_rc(3, 1500) # throttle at zero - self.arm_vehicle() - # start moving forward a little: - normal_rc_throttle = 1700 - self.set_rc(3, normal_rc_throttle) - self.wait_groundspeed(5, 100) - - # allow overrides: - self.set_rc(12, 2000) - - # now override to stop: - throttle_override = 1500 + self.set_parameter("RC12_OPTION", 46) + self.reboot_sitl() - tstart = self.get_sim_time_cached() - while True: - if self.get_sim_time_cached() - tstart > 10: - raise AutoTestTimeoutException("Did not reach speed") - self.progress("Sending throttle of %u" % (throttle_override,)) - self.mav.mav.rc_channels_override_send( - 1, # target system - 1, # targe component - 65535, # chan1_raw - 65535, # chan2_raw - throttle_override, # chan3_raw - 65535, # chan4_raw - 65535, # chan5_raw - 65535, # chan6_raw - 65535, # chan7_raw - 65535) # chan8_raw - - m = self.mav.recv_match(type='VFR_HUD', blocking=True) - want_speed = 2.0 - self.progress("Speed=%f want=<%f" % (m.groundspeed, want_speed)) - if m.groundspeed < want_speed: - break + self.change_mode('MANUAL') + self.wait_ready_to_arm() + self.set_rc(3, 1500) # throttle at zero + self.arm_vehicle() + # start moving forward a little: + normal_rc_throttle = 1700 + self.set_rc(3, normal_rc_throttle) + self.wait_groundspeed(5, 100) - # now override to stop - but set the switch on the RC - # transmitter to deny overrides; this should send the - # speed back up to 5 metres/second: - self.set_rc(12, 1000) + # allow overrides: + self.set_rc(12, 2000) - throttle_override = 1500 - tstart = self.get_sim_time_cached() - while True: - if self.get_sim_time_cached() - tstart > 10: - raise AutoTestTimeoutException("Did not speed back up") - self.progress("Sending throttle of %u" % (throttle_override,)) - self.mav.mav.rc_channels_override_send( - 1, # target system - 1, # targe component - 65535, # chan1_raw - 65535, # chan2_raw - throttle_override, # chan3_raw - 65535, # chan4_raw - 65535, # chan5_raw - 65535, # chan6_raw - 65535, # chan7_raw - 65535) # chan8_raw - - m = self.mav.recv_match(type='VFR_HUD', blocking=True) - want_speed = 5.0 - self.progress("Speed=%f want=>%f" % (m.groundspeed, want_speed)) - - if m.groundspeed > want_speed: - break + # now override to stop: + throttle_override = 1500 - # re-enable RC overrides - self.set_rc(12, 2000) - - # check we revert to normal RC inputs when gcs overrides cease: - self.progress("Waiting for RC to revert to normal RC input") - self.wait_rc_channel_value(3, normal_rc_throttle, timeout=10) - - self.start_subtest("Check override time of zero disables overrides") - old = self.get_parameter("RC_OVERRIDE_TIME") - ch = 2 - self.set_rc(ch, 1000) - channels = [65535] * 18 - ch_override_value = 1700 - channels[ch-1] = ch_override_value - channels[7] = 1234 # that's channel 8! - self.progress("Sending override message %u" % ch_override_value) - self.mav.mav.rc_channels_override_send( - 1, # target system - 1, # targe component - *channels - ) - # long timeout required here as we may have sent a lot of - # things via MAVProxy... - self.wait_rc_channel_value(ch, ch_override_value, timeout=30) - self.set_parameter("RC_OVERRIDE_TIME", 0) - self.wait_rc_channel_value(ch, 1000) - self.set_parameter("RC_OVERRIDE_TIME", old) - self.wait_rc_channel_value(ch, ch_override_value) - - ch_override_value = 1720 - channels[ch-1] = ch_override_value - self.progress("Sending override message %u" % ch_override_value) + tstart = self.get_sim_time_cached() + while True: + if self.get_sim_time_cached() - tstart > 10: + raise AutoTestTimeoutException("Did not reach speed") + self.progress("Sending throttle of %u" % (throttle_override,)) self.mav.mav.rc_channels_override_send( 1, # target system 1, # targe component - *channels - ) - self.wait_rc_channel_value(ch, ch_override_value, timeout=10) - self.set_parameter("RC_OVERRIDE_TIME", 0) - self.wait_rc_channel_value(ch, 1000) - self.set_parameter("RC_OVERRIDE_TIME", old) + 65535, # chan1_raw + 65535, # chan2_raw + throttle_override, # chan3_raw + 65535, # chan4_raw + 65535, # chan5_raw + 65535, # chan6_raw + 65535, # chan7_raw + 65535) # chan8_raw - self.progress("Ensuring timeout works") - self.wait_rc_channel_value(ch, 1000, timeout=5) - self.delay_sim_time(10) + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + want_speed = 2.0 + self.progress("Speed=%f want=<%f" % (m.groundspeed, want_speed)) + if m.groundspeed < want_speed: + break - self.set_parameter("RC_OVERRIDE_TIME", 10) - self.progress("Sending override message") + # now override to stop - but set the switch on the RC + # transmitter to deny overrides; this should send the + # speed back up to 5 metres/second: + self.set_rc(12, 1000) - ch_override_value = 1730 - channels[ch-1] = ch_override_value - self.progress("Sending override message %u" % ch_override_value) - self.mav.mav.rc_channels_override_send( - 1, # target system - 1, # targe component - *channels - ) - self.wait_rc_channel_value(ch, ch_override_value, timeout=10) - tstart = self.get_sim_time() - self.progress("Waiting for channel to revert to 1000 in ~10s") - self.wait_rc_channel_value(ch, 1000, timeout=15) - delta = self.get_sim_time() - tstart - if delta > 12: - raise NotAchievedException("Took too long to revert RC channel value (delta=%f)" % delta) - min_delta = 9 - if delta < min_delta: - raise NotAchievedException("Didn't take long enough to revert RC channel value (delta=%f want>=%f)" % - (delta, min_delta)) - self.progress("Disabling RC override timeout") - self.set_parameter("RC_OVERRIDE_TIME", -1) - ch_override_value = 1740 - channels[ch-1] = ch_override_value - self.progress("Sending override message %u" % ch_override_value) + throttle_override = 1500 + tstart = self.get_sim_time_cached() + while True: + if self.get_sim_time_cached() - tstart > 10: + raise AutoTestTimeoutException("Did not speed back up") + self.progress("Sending throttle of %u" % (throttle_override,)) self.mav.mav.rc_channels_override_send( 1, # target system 1, # targe component - *channels - ) - self.wait_rc_channel_value(ch, ch_override_value, timeout=10) - tstart = self.get_sim_time() - while True: - # warning: this is get_sim_time() and can slurp messages on you! - delta = self.get_sim_time() - tstart - if delta > 20: - break - m = self.assert_receive_message('RC_CHANNELS', timeout=1) - channel_field = "chan%u_raw" % ch - m_value = getattr(m, channel_field) - if m_value != ch_override_value: - raise NotAchievedException("Value reverted after %f seconds when it should not have (got=%u) (want=%u)" % (delta, m_value, ch_override_value)) # noqa - self.set_parameter("RC_OVERRIDE_TIME", old) + 65535, # chan1_raw + 65535, # chan2_raw + throttle_override, # chan3_raw + 65535, # chan4_raw + 65535, # chan5_raw + 65535, # chan6_raw + 65535, # chan7_raw + 65535) # chan8_raw - self.delay_sim_time(10) + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + want_speed = 5.0 + self.progress("Speed=%f want=>%f" % (m.groundspeed, want_speed)) - self.start_subtest("Checking higher-channel semantics") - self.context_push() - self.set_parameter("RC_OVERRIDE_TIME", 30) + if m.groundspeed > want_speed: + break - ch = 11 - rc_value = 1010 - self.set_rc(ch, rc_value) + # re-enable RC overrides + self.set_rc(12, 2000) + + # check we revert to normal RC inputs when gcs overrides cease: + self.progress("Waiting for RC to revert to normal RC input") + self.wait_rc_channel_value(3, normal_rc_throttle, timeout=10) + + self.start_subtest("Check override time of zero disables overrides") + old = self.get_parameter("RC_OVERRIDE_TIME") + ch = 2 + self.set_rc(ch, 1000) + channels = [65535] * 18 + ch_override_value = 1700 + channels[ch-1] = ch_override_value + channels[7] = 1234 # that's channel 8! + self.progress("Sending override message %u" % ch_override_value) + self.mav.mav.rc_channels_override_send( + 1, # target system + 1, # targe component + *channels + ) + # long timeout required here as we may have sent a lot of + # things via MAVProxy... + self.wait_rc_channel_value(ch, ch_override_value, timeout=30) + self.set_parameter("RC_OVERRIDE_TIME", 0) + self.wait_rc_channel_value(ch, 1000) + self.set_parameter("RC_OVERRIDE_TIME", old) + self.wait_rc_channel_value(ch, ch_override_value) + + ch_override_value = 1720 + channels[ch-1] = ch_override_value + self.progress("Sending override message %u" % ch_override_value) + self.mav.mav.rc_channels_override_send( + 1, # target system + 1, # targe component + *channels + ) + self.wait_rc_channel_value(ch, ch_override_value, timeout=10) + self.set_parameter("RC_OVERRIDE_TIME", 0) + self.wait_rc_channel_value(ch, 1000) + self.set_parameter("RC_OVERRIDE_TIME", old) - channels = [65535] * 18 - ch_override_value = 1234 - channels[ch-1] = ch_override_value - self.progress("Sending override message ch%u=%u" % (ch, ch_override_value)) - self.mav.mav.rc_channels_override_send( - 1, # target system - 1, # targe component - *channels - ) - self.progress("Wait for override value") - self.wait_rc_channel_value(ch, ch_override_value, timeout=10) + self.progress("Ensuring timeout works") + self.wait_rc_channel_value(ch, 1000, timeout=5) + self.delay_sim_time(10) - self.progress("Sending return-to-RC-input value") - channels[ch-1] = 65534 - self.mav.mav.rc_channels_override_send( - 1, # target system - 1, # targe component - *channels - ) - self.wait_rc_channel_value(ch, rc_value, timeout=10) + self.set_parameter("RC_OVERRIDE_TIME", 10) + self.progress("Sending override message") + + ch_override_value = 1730 + channels[ch-1] = ch_override_value + self.progress("Sending override message %u" % ch_override_value) + self.mav.mav.rc_channels_override_send( + 1, # target system + 1, # targe component + *channels + ) + self.wait_rc_channel_value(ch, ch_override_value, timeout=10) + tstart = self.get_sim_time() + self.progress("Waiting for channel to revert to 1000 in ~10s") + self.wait_rc_channel_value(ch, 1000, timeout=15) + delta = self.get_sim_time() - tstart + if delta > 12: + raise NotAchievedException("Took too long to revert RC channel value (delta=%f)" % delta) + min_delta = 9 + if delta < min_delta: + raise NotAchievedException("Didn't take long enough to revert RC channel value (delta=%f want>=%f)" % + (delta, min_delta)) + self.progress("Disabling RC override timeout") + self.set_parameter("RC_OVERRIDE_TIME", -1) + ch_override_value = 1740 + channels[ch-1] = ch_override_value + self.progress("Sending override message %u" % ch_override_value) + self.mav.mav.rc_channels_override_send( + 1, # target system + 1, # targe component + *channels + ) + self.wait_rc_channel_value(ch, ch_override_value, timeout=10) + tstart = self.get_sim_time() + while True: + # warning: this is get_sim_time() and can slurp messages on you! + delta = self.get_sim_time() - tstart + if delta > 20: + break + m = self.assert_receive_message('RC_CHANNELS', timeout=1) + channel_field = "chan%u_raw" % ch + m_value = getattr(m, channel_field) + if m_value != ch_override_value: + raise NotAchievedException("Value reverted after %f seconds when it should not have (got=%u) (want=%u)" % (delta, m_value, ch_override_value)) # noqa + self.set_parameter("RC_OVERRIDE_TIME", old) + + self.delay_sim_time(10) + + self.start_subtest("Checking higher-channel semantics") + self.context_push() + self.set_parameter("RC_OVERRIDE_TIME", 30) + + ch = 11 + rc_value = 1010 + self.set_rc(ch, rc_value) + + channels = [65535] * 18 + ch_override_value = 1234 + channels[ch-1] = ch_override_value + self.progress("Sending override message ch%u=%u" % (ch, ch_override_value)) + self.mav.mav.rc_channels_override_send( + 1, # target system + 1, # targe component + *channels + ) + self.progress("Wait for override value") + self.wait_rc_channel_value(ch, ch_override_value, timeout=10) + + self.progress("Sending return-to-RC-input value") + channels[ch-1] = 65534 + self.mav.mav.rc_channels_override_send( + 1, # target system + 1, # targe component + *channels + ) + self.wait_rc_channel_value(ch, rc_value, timeout=10) + + channels[ch-1] = ch_override_value + self.progress("Sending override message ch%u=%u" % (ch, ch_override_value)) + self.mav.mav.rc_channels_override_send( + 1, # target system + 1, # targe component + *channels + ) + self.progress("Wait for override value") + self.wait_rc_channel_value(ch, ch_override_value, timeout=10) - channels[ch-1] = ch_override_value - self.progress("Sending override message ch%u=%u" % (ch, ch_override_value)) + # make we keep the override vaue for at least 10 seconds: + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > 10: + break + # try both ignore values: + ignore_value = 0 + if self.get_sim_time_cached() - tstart > 5: + ignore_value = 65535 + self.progress("Sending ignore value %u" % ignore_value) + channels[ch-1] = ignore_value self.mav.mav.rc_channels_override_send( 1, # target system 1, # targe component *channels ) - self.progress("Wait for override value") - self.wait_rc_channel_value(ch, ch_override_value, timeout=10) - - # make we keep the override vaue for at least 10 seconds: - tstart = self.get_sim_time() - while True: - if self.get_sim_time_cached() - tstart > 10: - break - # try both ignore values: - ignore_value = 0 - if self.get_sim_time_cached() - tstart > 5: - ignore_value = 65535 - self.progress("Sending ignore value %u" % ignore_value) - channels[ch-1] = ignore_value - self.mav.mav.rc_channels_override_send( - 1, # target system - 1, # targe component - *channels - ) - if self.get_rc_channel_value(ch) != ch_override_value: - raise NotAchievedException("Did not maintain value") - - self.context_pop() + if self.get_rc_channel_value(ch) != ch_override_value: + raise NotAchievedException("Did not maintain value") - self.end_subtest("Checking higher-channel semantics") + self.context_pop() - except Exception as e: - self.print_exception_caught(e) - ex = e + self.end_subtest("Checking higher-channel semantics") self.disarm_vehicle() - self.context_pop() - self.reboot_sitl() - - if ex is not None: - raise ex def MANUAL_CONTROL(self): '''Test mavlink MANUAL_CONTROL''' - self.context_push() - self.set_parameter("SYSID_MYGCS", self.mav.source_system) - ex = None - try: - self.set_parameter("RC12_OPTION", 46) # enable/disable rc overrides - self.reboot_sitl() - - self.change_mode("MANUAL") - self.wait_ready_to_arm() - self.zero_throttle() - self.arm_vehicle() - self.progress("start moving forward a little") - normal_rc_throttle = 1700 - self.set_rc(3, normal_rc_throttle) - self.wait_groundspeed(5, 100) + self.set_parameters({ + "SYSID_MYGCS": self.mav.source_system, + "RC12_OPTION": 46, # enable/disable rc overrides + }) + self.reboot_sitl() - self.progress("allow overrides") - self.set_rc(12, 2000) + self.change_mode("MANUAL") + self.wait_ready_to_arm() + self.arm_vehicle() + self.progress("start moving forward a little") + normal_rc_throttle = 1700 + self.set_rc(3, normal_rc_throttle) + self.wait_groundspeed(5, 100) - self.progress("now override to stop") - throttle_override_normalized = 0 - expected_throttle = 0 # in VFR_HUD + self.progress("allow overrides") + self.set_rc(12, 2000) - tstart = self.get_sim_time_cached() - while True: - if self.get_sim_time_cached() - tstart > 10: - raise AutoTestTimeoutException("Did not reach speed") - self.progress("Sending normalized throttle of %d" % (throttle_override_normalized,)) - self.mav.mav.manual_control_send( - 1, # target system - 32767, # x (pitch) - 32767, # y (roll) - throttle_override_normalized, # z (thrust) - 32767, # r (yaw) - 0) # button mask - - m = self.mav.recv_match(type='VFR_HUD', blocking=True) - want_speed = 2.0 - self.progress("Speed=%f want=<%f throttle=%u want=%u" % - (m.groundspeed, want_speed, m.throttle, expected_throttle)) - if m.groundspeed < want_speed and m.throttle == expected_throttle: - break + self.progress("now override to stop") + throttle_override_normalized = 0 + expected_throttle = 0 # in VFR_HUD - self.progress("now override to stop - but set the switch on the RC transmitter to deny overrides; this should send the speed back up to 5 metres/second") # noqa - self.set_rc(12, 1000) + tstart = self.get_sim_time_cached() + while True: + if self.get_sim_time_cached() - tstart > 10: + raise AutoTestTimeoutException("Did not reach speed") + self.progress("Sending normalized throttle of %d" % (throttle_override_normalized,)) + self.mav.mav.manual_control_send( + 1, # target system + 32767, # x (pitch) + 32767, # y (roll) + throttle_override_normalized, # z (thrust) + 32767, # r (yaw) + 0) # button mask + + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + want_speed = 2.0 + self.progress("Speed=%f want=<%f throttle=%u want=%u" % + (m.groundspeed, want_speed, m.throttle, expected_throttle)) + if m.groundspeed < want_speed and m.throttle == expected_throttle: + break - throttle_override_normalized = 500 - expected_throttle = 36 # in VFR_HUD, corresponding to normal_rc_throttle adjusted for channel min/max + self.progress("now override to stop - but set the switch on the RC transmitter to deny overrides; this should send the speed back up to 5 metres/second") # noqa + self.set_rc(12, 1000) - tstart = self.get_sim_time_cached() - while True: - if self.get_sim_time_cached() - tstart > 10: - raise AutoTestTimeoutException("Did not stop") - self.progress("Sending normalized throttle of %u" % (throttle_override_normalized,)) - self.mav.mav.manual_control_send( - 1, # target system - 32767, # x (pitch) - 32767, # y (roll) - throttle_override_normalized, # z (thrust) - 32767, # r (yaw) - 0) # button mask - - m = self.mav.recv_match(type='VFR_HUD', blocking=True) - want_speed = 5.0 - - self.progress("Speed=%f want=>%f throttle=%u want=%u" % - (m.groundspeed, want_speed, m.throttle, expected_throttle)) - if m.groundspeed > want_speed and m.throttle == expected_throttle: - break + throttle_override_normalized = 500 + expected_throttle = 36 # in VFR_HUD, corresponding to normal_rc_throttle adjusted for channel min/max - # re-enable RC overrides - self.set_rc(12, 2000) + tstart = self.get_sim_time_cached() + while True: + if self.get_sim_time_cached() - tstart > 10: + raise AutoTestTimeoutException("Did not stop") + self.progress("Sending normalized throttle of %u" % (throttle_override_normalized,)) + self.mav.mav.manual_control_send( + 1, # target system + 32767, # x (pitch) + 32767, # y (roll) + throttle_override_normalized, # z (thrust) + 32767, # r (yaw) + 0) # button mask + + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + want_speed = 5.0 + + self.progress("Speed=%f want=>%f throttle=%u want=%u" % + (m.groundspeed, want_speed, m.throttle, expected_throttle)) + if m.groundspeed > want_speed and m.throttle == expected_throttle: + break - # check we revert to normal RC inputs when gcs overrides cease: - self.progress("Waiting for RC to revert to normal RC input") - self.wait_rc_channel_value(3, normal_rc_throttle, timeout=10) + # re-enable RC overrides + self.set_rc(12, 2000) - except Exception as e: - self.print_exception_caught(e) - ex = e + # check we revert to normal RC inputs when gcs overrides cease: + self.progress("Waiting for RC to revert to normal RC input") + self.wait_rc_channel_value(3, normal_rc_throttle, timeout=10) self.disarm_vehicle() - self.context_pop() - self.reboot_sitl() - - if ex is not None: - raise ex def CameraMission(self): '''Test Camera Mission Items''' @@ -4861,54 +4794,41 @@ def MotorTest(self): self.wait_servo_channel_value(3, self.get_parameter("RC3_TRIM", 5), timeout=10) self.wait_disarmed() - def test_poly_fence_object_avoidance_guided(self, target_system=1, target_component=1): + def PolyFenceObjectAvoidanceGuided(self, target_system=1, target_component=1): + '''PolyFence object avoidance tests - guided mode''' if not self.mavproxy_can_do_mision_item_protocols(): return self.test_poly_fence_object_avoidance_guided_pathfinding( target_system=target_system, target_component=target_component) - return - # twosquares is currently disabled because of the requirement to have an inclusion fence (which it doesn't have ATM) - # self.test_poly_fence_object_avoidance_guided_two_squares( - # target_system=target_system, - # target_component=target_component) + self.test_poly_fence_object_avoidance_guided_two_squares( + target_system=target_system, + target_component=target_component) - def test_poly_fence_object_avoidance_auto(self, target_system=1, target_component=1): + def PolyFenceObjectAvoidanceAuto(self, target_system=1, target_component=1): + '''PolyFence object avoidance tests - auto mode''' mavproxy = self.start_mavproxy() self.load_fence_using_mavproxy(mavproxy, "rover-path-planning-fence.txt") self.stop_mavproxy(mavproxy) # self.load_fence("rover-path-planning-fence.txt") self.load_mission("rover-path-planning-mission.txt") - self.context_push() - ex = None - try: - self.set_parameters({ - "AVOID_ENABLE": 3, - "OA_TYPE": 2, - "FENCE_MARGIN": 0, # FIXME: https://github.com/ArduPilot/ardupilot/issues/11601 - }) - self.reboot_sitl() - self.change_mode('AUTO') - self.wait_ready_to_arm() - self.arm_vehicle() - self.set_parameter("FENCE_ENABLE", 1) - if self.mavproxy is not None: - self.mavproxy.send("fence list\n") - # target_loc is copied from the mission file - target_loc = mavutil.location(40.073799, -105.229156) - self.wait_location(target_loc, height_accuracy=None, timeout=300) - # mission has RTL as last item - self.wait_distance_to_home(3, 7, timeout=300) - self.disarm_vehicle() - except Exception as e: - self.disarm_vehicle(force=True) - self.print_exception_caught(e) - ex = e - self.context_pop() + self.set_parameters({ + "AVOID_ENABLE": 3, + "OA_TYPE": 2, + "FENCE_MARGIN": 0, # FIXME: https://github.com/ArduPilot/ardupilot/issues/11601 + }) self.reboot_sitl() - if ex is not None: - raise ex + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_parameter("FENCE_ENABLE", 1) + # target_loc is copied from the mission file + target_loc = mavutil.location(40.073799, -105.229156) + self.wait_location(target_loc, height_accuracy=None, timeout=300) + # mission has RTL as last item + self.wait_distance_to_home(3, 7, timeout=300) + self.disarm_vehicle() def send_guided_mission_item(self, loc, target_system=1, target_component=1): self.mav.mav.mission_item_send( @@ -4930,73 +4850,52 @@ def send_guided_mission_item(self, loc, target_system=1, target_component=1): def test_poly_fence_object_avoidance_guided_pathfinding(self, target_system=1, target_component=1): self.load_fence("rover-path-planning-fence.txt") - self.context_push() - ex = None - try: - self.set_parameters({ - "AVOID_ENABLE": 3, - "OA_TYPE": 2, - "FENCE_MARGIN": 0, # FIXME: https://github.com/ArduPilot/ardupilot/issues/11601 - }) - self.reboot_sitl() - self.change_mode('GUIDED') - self.wait_ready_to_arm() - self.arm_vehicle() - self.set_parameter("FENCE_ENABLE", 1) - if self.mavproxy is not None: - self.mavproxy.send("fence list\n") - target_loc = mavutil.location(40.073800, -105.229172) - self.send_guided_mission_item(target_loc, - target_system=target_system, - target_component=target_component) - self.wait_location(target_loc, timeout=300) - self.do_RTL(timeout=300) - self.disarm_vehicle() - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() + self.set_parameters({ + "AVOID_ENABLE": 3, + "OA_TYPE": 2, + "FENCE_MARGIN": 0, # FIXME: https://github.com/ArduPilot/ardupilot/issues/11601 + }) self.reboot_sitl() - if ex is not None: - raise ex + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_parameter("FENCE_ENABLE", 1) + target_loc = mavutil.location(40.073800, -105.229172) + self.send_guided_mission_item(target_loc, + target_system=target_system, + target_component=target_component) + self.wait_location(target_loc, timeout=300) + self.do_RTL(timeout=300) + self.disarm_vehicle() def WheelEncoders(self): '''make sure wheel encoders are generally working''' - ex = None - try: - self.set_parameters({ - "WENC_TYPE": 10, - "EK3_ENABLE": 1, - "AHRS_EKF_TYPE": 3, - }) - self.reboot_sitl() - self.change_mode("LOITER") - self.wait_ready_to_arm() - self.change_mode("MANUAL") - self.arm_vehicle() - self.set_rc(3, 1600) + self.set_parameters({ + "WENC_TYPE": 10, + "EK3_ENABLE": 1, + "AHRS_EKF_TYPE": 3, + }) + self.reboot_sitl() + self.change_mode("LOITER") + self.wait_ready_to_arm() + self.change_mode("MANUAL") + self.arm_vehicle() + self.set_rc(3, 1600) - m = self.assert_receive_message('WHEEL_DISTANCE', timeout=5) + m = self.assert_receive_message('WHEEL_DISTANCE', timeout=5) - tstart = self.get_sim_time() - while True: - if self.get_sim_time_cached() - tstart > 10: - break - dist_home = self.distance_to_home(use_cached_home=True) - m = self.mav.messages.get("WHEEL_DISTANCE") - delta = abs(m.distance[0] - dist_home) - self.progress("dist-home=%f wheel-distance=%f delta=%f" % - (dist_home, m.distance[0], delta)) - if delta > 5: - raise NotAchievedException("wheel distance incorrect") - self.disarm_vehicle() - except Exception as e: - self.print_exception_caught(e) - self.disarm_vehicle() - ex = e - self.reboot_sitl() - if ex is not None: - raise ex + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > 10: + break + dist_home = self.distance_to_home(use_cached_home=True) + m = self.mav.messages.get("WHEEL_DISTANCE") + delta = abs(m.distance[0] - dist_home) + self.progress("dist-home=%f wheel-distance=%f delta=%f" % + (dist_home, m.distance[0], delta)) + if delta > 5: + raise NotAchievedException("wheel distance incorrect") + self.disarm_vehicle() def test_poly_fence_object_avoidance_guided_two_squares(self, target_system=1, target_component=1): self.start_subtest("Ensure we can steer around obstacles in guided mode") @@ -5108,159 +5007,86 @@ def PolyFenceAvoidance(self, target_system=1, target_component=1): self.disarm_vehicle() - def test_poly_fence_object_avoidance_guided_bendy_ruler(self, target_system=1, target_component=1): - if not self.mavproxy_can_do_mision_item_protocols(): - return - self.load_fence("rover-path-bendyruler-fence.txt") - self.context_push() - ex = None - try: - self.set_parameters({ - "AVOID_ENABLE": 3, - "OA_TYPE": 1, - "OA_LOOKAHEAD": 50, - }) - self.reboot_sitl() - self.change_mode('GUIDED') - self.wait_ready_to_arm() - self.arm_vehicle() - self.set_parameters({ - "FENCE_ENABLE": 1, - "WP_RADIUS": 5, - }) - if self.mavproxy is not None: - self.mavproxy.send("fence list\n") - target_loc = mavutil.location(40.071060, -105.227734, 0, 0) - self.send_guided_mission_item(target_loc, - target_system=target_system, - target_component=target_component) - # FIXME: we don't get within WP_RADIUS of our target?! - self.wait_location(target_loc, timeout=300, accuracy=15) - self.do_RTL(timeout=300) - self.disarm_vehicle() - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - self.disarm_vehicle() + def PolyFenceObjectAvoidanceBendyRuler(self, target_system=1, target_component=1): + '''PolyFence object avoidance tests - bendy ruler''' + self.load_fence_using_mavwp("rover-path-bendyruler-fence.txt") + self.set_parameters({ + "AVOID_ENABLE": 3, + "OA_TYPE": 1, + "FENCE_ENABLE": 1, + "WP_RADIUS": 5, + }) self.reboot_sitl() - if ex is not None: - raise ex - - def PolyFenceObjectAvoidanceBendyRulerEasier(self, target_system=1, target_component=1): - '''PolyFence object avoidance tests - easier bendy ruler test''' - if not self.mavproxy_can_do_mision_item_protocols(): - return - self.test_poly_fence_object_avoidance_auto_bendy_ruler_easier( - target_system=target_system, target_component=target_component) - self.test_poly_fence_object_avoidance_guided_bendy_ruler_easier( - target_system=target_system, target_component=target_component) + self.set_parameters({ + "OA_BR_LOOKAHEAD": 50, + }) + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + target_loc = mavutil.location(40.071060, -105.227734, 1584, 0) + self.send_guided_mission_item(target_loc, + target_system=target_system, + target_component=target_component) + # FIXME: we don't get within WP_RADIUS of our target?! + self.wait_location(target_loc, timeout=300, accuracy=15) + self.do_RTL(timeout=300) + self.disarm_vehicle() - def test_poly_fence_object_avoidance_guided_bendy_ruler_easier(self, target_system=1, target_component=1): + def PolyFenceObjectAvoidanceBendyRulerEasierGuided(self, target_system=1, target_component=1): '''finish-line issue means we can't complete the harder one. This test can go away once we've nailed that one. The only difference here is the target point. ''' - if not self.mavproxy_can_do_mision_item_protocols(): - return - self.load_fence("rover-path-bendyruler-fence.txt") - self.context_push() - ex = None - try: - self.set_parameters({ - "AVOID_ENABLE": 3, - "OA_TYPE": 1, - "OA_LOOKAHEAD": 50, - }) - self.reboot_sitl() - self.change_mode('GUIDED') - self.wait_ready_to_arm() - self.arm_vehicle() - self.set_parameters({ - "FENCE_ENABLE": 1, - "WP_RADIUS": 5, - }) - if self.mavproxy is not None: - self.mavproxy.send("fence list\n") - target_loc = mavutil.location(40.071260, -105.227000, 0, 0) - self.send_guided_mission_item(target_loc, - target_system=target_system, - target_component=target_component) - # FIXME: we don't get within WP_RADIUS of our target?! - self.wait_location(target_loc, timeout=300, accuracy=15) - self.do_RTL(timeout=300) - self.disarm_vehicle() - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - self.disarm_vehicle() + self.load_fence_using_mavwp("rover-path-bendyruler-fence.txt") + self.set_parameters({ + "AVOID_ENABLE": 3, + "OA_TYPE": 1, + "FENCE_ENABLE": 1, + "WP_RADIUS": 5, + }) self.reboot_sitl() - if ex is not None: - raise ex + self.set_parameters({ + "OA_BR_LOOKAHEAD": 60, + }) + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + target_loc = mavutil.location(40.071260, -105.227000, 1584, 0) + self.send_guided_mission_item(target_loc, + target_system=target_system, + target_component=target_component) + # FIXME: we don't get within WP_RADIUS of our target?! + self.wait_location(target_loc, timeout=300, accuracy=15) + self.do_RTL(timeout=300) + self.disarm_vehicle() - def test_poly_fence_object_avoidance_auto_bendy_ruler_easier(self, target_system=1, target_component=1): + def PolyFenceObjectAvoidanceBendyRulerEasierAuto(self, target_system=1, target_component=1): '''finish-line issue means we can't complete the harder one. This test can go away once we've nailed that one. The only difference here is the target point. ''' - if not self.mavproxy_can_do_mision_item_protocols(): - return - - self.load_fence("rover-path-bendyruler-fence.txt") + self.load_fence_using_mavwp("rover-path-bendyruler-fence.txt") self.load_mission("rover-path-bendyruler-mission-easier.txt") - self.context_push() - ex = None - try: - self.set_parameters({ - "AVOID_ENABLE": 3, - "OA_TYPE": 1, - "OA_LOOKAHEAD": 50, - }) - self.reboot_sitl() - self.change_mode('AUTO') - self.wait_ready_to_arm() - self.arm_vehicle() - self.set_parameters({ - "FENCE_ENABLE": 1, - "WP_RADIUS": 5, - }) - if self.mavproxy is not None: - self.mavproxy.send("fence list\n") - target_loc = mavutil.location(40.071260, -105.227000, 0, 0) - # target_loc is copied from the mission file - self.wait_location(target_loc, timeout=300) - # mission has RTL as last item - self.wait_distance_to_home(3, 7, timeout=300) - self.disarm_vehicle() - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - self.disarm_vehicle() - self.reboot_sitl() - if ex is not None: - raise ex - - def PolyFenceObjectAvoidance(self, target_system=1, target_component=1): - '''PolyFence object avoidance tests''' - self.test_poly_fence_object_avoidance_auto( - target_system=target_system, - target_component=target_component) - self.test_poly_fence_object_avoidance_guided( - target_system=target_system, - target_component=target_component) - def PolyFenceObjectAvoidanceBendyRuler(self, target_system=1, target_component=1): - '''PolyFence object avoidance tests - bendy ruler''' - if not self.mavproxy_can_do_mision_item_protocols(): - return - # bendy Ruler isn't as flexible as Dijkstra for planning, so - # it gets a simpler test: - self.test_poly_fence_object_avoidance_guided_bendy_ruler( - target_system=target_system, - target_component=target_component, - ) + self.set_parameters({ + "AVOID_ENABLE": 3, + "OA_TYPE": 1, # BendyRuler + "FENCE_ENABLE": 1, + "WP_RADIUS": 5, + }) + self.reboot_sitl() + self.set_parameters({ + "OA_BR_LOOKAHEAD": 60, + }) + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + target_loc = mavutil.location(40.071260, -105.227000, 1584, 0) + # target_loc is copied from the mission file + self.wait_location(target_loc, timeout=300) + # mission has RTL as last item + self.wait_distance_to_home(3, 7, timeout=300) + self.disarm_vehicle() def test_scripting_simple_loop(self): self.start_subtest("Scripting simple loop") @@ -6951,10 +6777,12 @@ def tests(self): self.SkidSteer, self.PolyFence, self.PolyFenceAvoidance, - self.PolyFenceObjectAvoidance, + self.PolyFenceObjectAvoidanceAuto, + self.PolyFenceObjectAvoidanceGuided, self.PolyFenceObjectAvoidanceBendyRuler, self.SendToComponents, - self.PolyFenceObjectAvoidanceBendyRulerEasier, + self.PolyFenceObjectAvoidanceBendyRulerEasierGuided, + self.PolyFenceObjectAvoidanceBendyRulerEasierAuto, self.SlewRate, self.Scripting, self.ScriptingSteeringAndThrottle, @@ -6997,6 +6825,7 @@ def disabled_tests(self): return { "SlewRate": "got timing report failure on CI", "MAV_CMD_NAV_SET_YAW_SPEED": "compiled out of code by default", + "PolyFenceObjectAvoidanceBendyRuler": "unreliable", } def rc_defaults(self): diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index f3458fee9d9575..67cdbf54e4cdb0 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -2112,6 +2112,14 @@ def load_fence(self, filename): (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, locs), ]) + def load_fence_using_mavwp(self, filename): + filepath = os.path.join(testdir, self.current_test_name_directory, filename) + if not os.path.exists(filepath): + filepath = self.generic_mission_filepath_for_filename(filename) + self.progress("Loading fence from (%s)" % str(filepath)) + items = self.mission_item_protocol_items_from_filepath(mavwp.MissionItemProtocol_Fence, filepath) + self.check_fence_upload_download(items) + def send_reboot_command(self): self.mav.mav.command_long_send(self.sysid_thismav(), 1, @@ -5124,7 +5132,7 @@ def load_mission(self, filename, strict=True): os.path.join(testdir, self.current_test_name_directory, filename), strict=strict) - def wp_to_mission_item_int(self, wp): + def wp_to_mission_item_int(self, wp, mission_type): '''convert a MISSION_ITEM to a MISSION_ITEM_INT. We always send as MISSION_ITEM_INT to give cm level accuracy Swiped from mavproxy_wp.py @@ -5145,19 +5153,35 @@ def wp_to_mission_item_int(self, wp): wp.param4, int(wp.x*1.0e7), int(wp.y*1.0e7), - wp.z) + wp.z, + mission_type, + ) return wp_int - def mission_from_filepath(self, filepath, target_system=1, target_component=1): + def mission_item_protocol_items_from_filepath(self, + loaderclass, + filepath, + target_system=1, + target_component=1, + ): '''returns a list of mission-item-ints from filepath''' - self.progress("filepath: %s" % filepath) - self.progress("Loading mission (%s)" % os.path.basename(filepath)) - wploader = mavwp.MAVWPLoader( + # self.progress("filepath: %s" % filepath) + self.progress("Loading {loaderclass.itemstype()} (%s)" % os.path.basename(filepath)) + wploader = loaderclass( target_system=target_system, target_component=target_component ) wploader.load(filepath) - return [self.wp_to_mission_item_int(x) for x in wploader.wpoints] + return [self.wp_to_mission_item_int(x, wploader.mav_mission_type()) for x in wploader.wpoints] # noqa:502 + + def mission_from_filepath(self, filepath, target_system=1, target_component=1): + '''returns a list of mission-item-ints from filepath''' + return self.mission_item_protocol_items_from_filepath( + mavwp.MAVWPLoader, + filepath, + target_system=target_system, + target_component=target_component, + ) def sitl_home_string_from_mission(self, filename): '''return a string of the form "lat,lng,yaw,alt" from the home @@ -5178,6 +5202,10 @@ def get_home_tuple_from_mission(self, filename): os.path.join(testdir, self.current_test_name_directory, filename) ) + def get_home_location_from_mission(self, filename): + (home_lat, home_lon, home_alt, heading) = self.get_home_tuple_from_mission("rover-path-planning-mission.txt") + return mavutil.location(home_lat, home_lon) + def get_home_tuple_from_mission_filepath(self, filepath): '''gets item 0 from the mission file, returns a tuple suitable for passing to customise_SITL_commandline as --home. Yaw will be @@ -5571,6 +5599,10 @@ def set_rc(self, chan, pwm, timeout=20): """Setup a simulated RC control to a PWM value""" self.set_rc_from_map({chan: pwm}, timeout=timeout) + def set_servo(self, chan, pwm): + """Replicate the functionality of MAVProxy: servo set """ + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=chan, p2=pwm) + def location_offset_ne(self, location, north, east): '''move location in metres''' print("old: %f %f" % (location.lat, location.lng)) @@ -9074,7 +9106,7 @@ def upload_using_mission_protocol(self, mission_type, items): raise NotAchievedException("received request for item from wrong mission type") if items[m.seq].mission_type != mission_type: - raise NotAchievedException("supplied item not of correct mission type") + raise NotAchievedException(f"supplied item not of correct mission type (want={mission_type} got={items[m.seq].mission_type}") # noqa:501 if items[m.seq].target_system != target_system: raise NotAchievedException("supplied item not of correct target system") if items[m.seq].target_component != target_component: diff --git a/Tools/bootloaders/BotBloxSwitch_bl.bin b/Tools/bootloaders/BotBloxSwitch_bl.bin index 77f7357d05cdb3..f183d8d66dddad 100755 Binary files a/Tools/bootloaders/BotBloxSwitch_bl.bin and b/Tools/bootloaders/BotBloxSwitch_bl.bin differ diff --git a/Tools/bootloaders/CUAV-7-Nano_bl.bin b/Tools/bootloaders/CUAV-7-Nano_bl.bin new file mode 100644 index 00000000000000..60b99e1b3d7254 Binary files /dev/null and b/Tools/bootloaders/CUAV-7-Nano_bl.bin differ diff --git a/Tools/bootloaders/CUAV-7-Nano_bl.hex b/Tools/bootloaders/CUAV-7-Nano_bl.hex new file mode 100644 index 00000000000000..863c1db590b304 --- /dev/null +++ b/Tools/bootloaders/CUAV-7-Nano_bl.hex @@ -0,0 +1,2517 @@ +:020000040800F2 +:1000000000060020E1020008E3020008E302000805 +:10001000E3020008E3020008E3020008E30200082C +:10002000E3020008E3020008E30200080D7700087D +:10003000E3020008E3020008E3020008E30200080C +:10004000E3020008E3020008E3020008E3020008FC +:10005000E3020008E302000819860008458600084C +:10006000718600089D860008C986000819460008A8 +:10007000414600086D46000899460008C54600083C +:10008000ED46000819470008E3020008E3020008F3 +:10009000E3020008E3020008E3020008F586000816 +:1000A000E3020008E3020008E3020008E30200089C +:1000B000E3020008E3020008E3020008E30200088C +:1000C000E3020008E3020008E3020008E30200087C +:1000D000E3020008E3020008E3020008E1870008E9 +:1000E00059870008E3020008E3020008E302000861 +:1000F000E3020008E3020008E302000845470008A5 +:10010000E3020008E30200081D880008E30200087B +:10011000E3020008F5870008E3020008E302000894 +:100120007147000899470008C5470008F1470008D3 +:100130001D480008E3020008E3020008E30200088B +:10014000E3020008E3020008E3020008E3020008FB +:1001500045480008714800089D480008E30200086F +:10016000E3020008E3020008E3020008E3020008DB +:10017000E3020008F9820008E3020008E302000835 +:10018000E3020008E302000809880008E30200080F +:10019000E3020008E3020008E3020008E3020008AB +:1001A000E3020008E3020008E3020008E30200089B +:1001B000E3020008E3020008E3020008E30200088B +:1001C000E3020008E3020008E3020008E30200087B +:1001D000E3020008E5820008E3020008E3020008E9 +:1001E000E3020008E3020008E3020008E30200085B +:1001F000E3020008E3020008E3020008E30200084B +:10020000E3020008E3020008E3020008E30200083A +:10021000E3020008E3020008E3020008E30200082A +:10022000E3020008E3020008E3020008E30200081A +:10023000CD870008E3020008E3020008E30200089B +:10024000E3020008E3020008E3020008E3020008FA +:10025000E3020008E3020008E3020008E3020008EA +:10026000E3020008E3020008E3020008E3020008DA +:10027000E3020008E3020008E3020008E3020008CA +:10028000E3020008E3020008E3020008E3020008BA +:10029000E3020008E3020008E3020008E3020008AA +:1002A000E3020008E3020008E3020008E30200089A +:1002B000E3020008E3020008E3020008E30200088A +:1002C000E3020008E3020008E3020008E30200087A +:1002D000E3020008E3020008E3020008E30200086A +:1002E00002E000F000F8FEE772B6374880F30888B5 +:1002F000364880F3098836483649086040F20000E5 +:10030000CCF200004EF63471CEF200010860BFF36B +:100310004F8FBFF36F8F40F20000C0F2F0004EF637 +:100320008851CEF200010860BFF34F8FBFF36F8F8B +:100330004FF00000E1EE100A4EF63C71CEF20001E3 +:100340000860062080F31488BFF36F8F07F040FA2F +:1003500006F0ACFB4FF055301F491B4A91423CBFA1 +:1003600041F8040BFAE71D49184A91423CBF41F895 +:10037000040BFAE71A491B4A1B4B9A423EBF51F83D +:10038000040B42F8040BF8E700201849184A914280 +:100390003CBF41F8040BFAE707F058FA06F00AFCF4 +:1003A000144C154DAC4203DA54F8041B8847F9E7A6 +:1003B00000F0C4F9114C124DAC4203DA54F8041B9E +:1003C0008847F9E707F040BA000600200022002025 +:1003D0000000000808ED00E00000002000060020FA +:1003E000B89C0008002200207422002078220020FF +:1003F00034670020E0020008E0020008E002000884 +:10040000E00200082DE9F04F2DED108AC1F80CD064 +:10041000D0F80CD0BDEC108ABDE8F08F002383F338 +:1004200011882846A047002006F022F9FEE706F0D2 +:100430007BF800DFFEE7000053B94AB9002908BF86 +:1004400000281CBF4FF0FF314FF0FF3000F074B9AF +:10045000ADF1080C6DE904CE00F006F8DDF804E01B +:10046000DDE9022304B070472DE9F047089D0446FA +:100470008E46002B4DD18A42944669D9B2FA82F257 +:1004800052B101FA02F3C2F1200120FA01F10CFA93 +:1004900002FC41EA030E94404FEA1C48210CBEFBCB +:1004A000F8F61FFA8CF708FB16E341EA034306FB54 +:1004B00007F199420AD91CEB030306F1FF3080F0E3 +:1004C0001F81994240F21C81023E63445B1AA4B230 +:1004D000B3FBF8F008FB103344EA034400FB07F7D2 +:1004E000A7420AD91CEB040400F1FF3380F00A8113 +:1004F000A74240F207816444023840EA0640E41B08 +:1005000000261DB1D4400023C5E900433146BDE8B3 +:10051000F0878B4209D9002D00F0EF800026C5E955 +:10052000000130463146BDE8F087B3FA83F6002E6D +:100530004AD18B4202D3824200F2F980841A61EBE5 +:10054000030301209E46002DE0D0C5E9004EDDE703 +:1005500002B9FFDEB2FA82F2002A40F09280A1EBEB +:100560000C014FEA1C471FFA8CFE0126200CB1FB40 +:10057000F7F307FB131140EA01410EFB03F0884239 +:1005800008D91CEB010103F1FF3802D2884200F2C6 +:10059000CB804346091AA4B2B1FBF7F007FB101158 +:1005A00044EA01440EFB00FEA64508D91CEB0404F6 +:1005B00000F1FF3102D2A64500F2BB800846A4EB51 +:1005C0000E0440EA03409CE7C6F12007B34022FA3C +:1005D00007FC4CEA030C20FA07F401FA06F31C436B +:1005E000F9404FEA1C4900FA06F3B1FBF9F8200C78 +:1005F0001FFA8CFE09FB181140EA014108FB0EF0BE +:10060000884202FA06F20BD91CEB010108F1FF3A0D +:1006100080F08880884240F28580A8F10208614419 +:10062000091AA4B2B1FBF9F009FB101144EA014127 +:1006300000FB0EFE8E4508D91CEB010100F1FF34D2 +:100640006CD28E456AD90238614440EA0840A0FB6A +:100650000294A1EB0E01A142C846A64656D353D040 +:100660005DB1B3EB080261EB0E0101FA07F722FA64 +:1006700006F3F1401F43C5E9007100263146BDE88D +:10068000F087C2F12003D8400CFA02FC21FA03F3F0 +:10069000914001434FEA1C471FFA8CFEB3FBF7F071 +:1006A00007FB10360B0C43EA064300FB0EF69E4296 +:1006B00004FA02F408D91CEB030300F1FF382FD22F +:1006C0009E422DD9023863449B1B89B2B3FBF7F6D7 +:1006D00007FB163341EA034106FB0EF38B4208D9B0 +:1006E0001CEB010106F1FF3816D28B4214D9023EF1 +:1006F0006144C91A46EA004638E72E46284605E70F +:100700000646E3E61846F8E64B45A9D2B9EB0208DF +:1007100064EB0C0E0138A3E74646EAE7204694E76F +:100720004046D1E7D0467BE7023B614432E73046A2 +:1007300009E76444023842E7704700BF38B501F06A +:10074000FFF901F0C1FB06F0CBFE054606F0D4FF31 +:100750000446D0B90F4B9D4219D001339D4241F25E +:10076000883504BF01240025002006F0C3FE0CB12B +:1007700000F078F801F048FB00F026FD08B100F029 +:1007800071F8284600F01CF9F9E70025ECE705466A +:10079000EAE700BF010007B008B501F09DF9A0F13C +:1007A00020035842584108BD07B541F21203022107 +:1007B00001A8ADF8043001F0ADF903B05DF804FB19 +:1007C00038B5302383F31188174803680BB106F05E +:1007D00033F80023154A4FF47A71134806F022F8D3 +:1007E000002383F31188124C236813B12368013B63 +:1007F0002360636813B16368013B63600D4D2B7820 +:1008000033B963687BB9022001F060FA3223636078 +:100810002B78032B07D163682BB9022001F056FA1D +:100820004FF47A73636038BD78220020C107000856 +:100830009823002090220020084B187003280CD821 +:10084000DFE800F008050208022001F031BA0220BA +:1008500001F01EBA024B00225A607047902200201D +:1008600098230020F8B5504B504A1C4619680131B6 +:1008700000F0998004339342F8D162684C4B9A425D +:1008800040F291804B4B9B6803F1006303F500330A +:100890009A4280F08880002001F06CF90220FFF776 +:1008A000CBFF454B0021D3F8E820C3F8E810D3F87C +:1008B0001021C3F81011D3F81021D3F8EC20C3F89D +:1008C000EC10D3F81421C3F81411D3F81421D3F881 +:1008D000F020C3F8F010D3F81821C3F81811D3F89A +:1008E0001821D3F8802042F00062C3F88020D3F8AA +:1008F000802022F00062C3F88020D3F88020D3F853 +:10090000802042F00072C3F88020D3F8802022F0CB +:100910000072C3F88020D3F8803072B64FF0E02325 +:10092000C3F8084DD4E90004BFF34F8FBFF36F8FB6 +:10093000224AC2F88410BFF34F8F536923F48033E7 +:100940005361BFF34F8FD2F8803043F6E076C3F3A4 +:10095000C905C3F34E335B0103EA060C29464CEA92 +:1009600081770139C2F87472F9D2203B13F1200F5C +:10097000F2D1BFF34F8FBFF36F8FBFF34F8FBFF332 +:100980006F8F536923F4003353610023C2F8503250 +:10099000BFF34F8FBFF36F8F302383F311888546EA +:1009A00080F308882047F8BD0000020820000208F4 +:1009B000FFFF0108002200200044025800ED00E083 +:1009C0002DE9F04F93B0B44B2022FF2100900AA8EC +:1009D0009D6801F0C3F9B14A1378A3B90121B04869 +:1009E00011700360302383F3118803680BB105F0A5 +:1009F00023FF0023AB4A4FF47A71A94805F012FF98 +:100A0000002383F31188009B13B1A74B009A1A604F +:100A1000A64A1378032B03D000231370A24A536015 +:100A20004FF0000A009CD3465646D146012001F003 +:100A30003FF924B19C4B1B68002B00F0268200205C +:100A400001F04AF80390039B002BF2DB012001F038 +:100A50001FF9039B213B1F2BE8D801A252F823F07A +:100A6000E10A0008090B00089D0B00082D0A000888 +:100A70002D0A00082D0A00082F0C0008FF0D0008A1 +:100A8000190D00087B0D0008A30D0008C90D000812 +:100A90002D0A0008DB0D00082D0A00084D0E000885 +:100AA000810B00082D0A0008910E0008ED0A0008CD +:100AB000810B00082D0A00087B0D00082D0A000894 +:100AC0002D0A00082D0A00082D0A00082D0A00082A +:100AD0002D0A00082D0A00082D0A00089D0B0008A9 +:100AE0000220FFF759FE002840F0F981009B022107 +:100AF00005A8BAF1000F08BF1C4641F21233ADF849 +:100B0000143001F007F891E74FF47A7000F0E4FF39 +:100B1000071EEBDB0220FFF73FFE0028E6D0013F77 +:100B2000052F00F2DE81DFE807F0030A0D1013360F +:100B30000523042105A8059300F0ECFF17E004212C +:100B40005548F9E704215A48F6E704215948F3E7E4 +:100B50004FF01C08404608F1040801F00DF804218C +:100B6000059005A800F0D6FFB8F12C0FF2D10120B6 +:100B70004FF0000900FA07F747EA0B0B5FFA8BFB0F +:100B800001F038F926B10BF00B030B2B08BF002442 +:100B9000FFF70AFE4AE704214748CDE7002EA5D01B +:100BA0000BF00B030B2BA1D10220FFF7F5FD07463D +:100BB00000289BD00120002600F0DCFF0220FFF778 +:100BC0003BFE1FFA86F8404600F0E4FF0446B0B151 +:100BD000039940460136A1F140025142514100F0D3 +:100BE000E9FF0028EDD1BA46044641F21213022172 +:100BF00005A83E46ADF8143000F08CFF16E72546F8 +:100C00000120FFF719FE244B9B68AB4207D9284609 +:100C100000F0B2FF013040F067810435F3E70025B2 +:100C2000224BBA463E461D701F4B5D60A8E7002E62 +:100C30003FF45CAF0BF00B030B2B7FF457AF02209C +:100C4000FFF7FAFD322000F047FFB0F10008FFF691 +:100C50004DAF18F003077FF449AF0F4A08EB0503C7 +:100C6000926893423FF642AFB8F5807F3FF73EAFC0 +:100C7000124BB845019323DD4FF47A7000F02CFF3E +:100C80000390039A002AFFF631AF039A0137019BC4 +:100C900003F8012BEDE700BF002200209423002081 +:100CA00078220020C107000898230020902200200D +:100CB00004220020082200200C2200209422002080 +:100CC000C820FFF769FD074600283FF40FAF1F2D2E +:100CD00011D8C5F120020AAB25F003008449424532 +:100CE000184428BF4246019201F012F8019AFF21F0 +:100CF0007F4801F033F84FEAA803C8F387027C4924 +:100D00002846019301F032F8064600283FF46DAF03 +:100D1000019B05EB830533E70220FFF73DFD00282B +:100D20003FF4E4AE00F064FF00283FF4DFAE00279C +:100D3000B846704B9B68BB4218D91F2F11D80A9B2D +:100D400001330ED027F0030312AA134453F8203CBA +:100D500005934046042205A9043701F00BF98046AB +:100D6000E7E7384600F008FF0590F2E7CDF8148079 +:100D7000042105A800F0CEFE02E70023642104A8A8 +:100D8000049300F0BDFE00287FF4B0AE0220FFF710 +:100D900003FD00283FF4AAAE049800F01FFF059061 +:100DA000E6E70023642104A8049300F0A9FE0028CC +:100DB0007FF49CAE0220FFF7EFFC00283FF496AED4 +:100DC000049800F00DFFEAE70220FFF7E5FC002899 +:100DD0003FF48CAE00F01CFFE1E70220FFF7DCFCE3 +:100DE00000283FF483AE05A9142000F017FF074642 +:100DF0000421049004A800F08DFE3946B9E73220A2 +:100E000000F06AFE071EFFF671AEBB077FF46EAE00 +:100E1000384A07EB0903926893423FF667AE022017 +:100E2000FFF7BAFC00283FF461AE27F003074F44F8 +:100E3000B9453FF4A5AE484609F1040900F09CFE0F +:100E40000421059005A800F065FEF1E74FF47A70E3 +:100E5000FFF7A2FC00283FF449AE00F0C9FE0028CD +:100E600044D00A9B01330BD008220AA9002000F0CD +:100E70007DFF00283AD02022FF210AA800F06EFF53 +:100E8000FFF792FC1C4805F0F9FB13B0BDE8F08FAA +:100E9000002E3FF42BAE0BF00B030B2B7FF426AE92 +:100EA0000023642105A8059300F02AFE07460028C8 +:100EB0007FF41CAE0220FFF76FFC804600283FF451 +:100EC00015AEFFF771FC41F2883005F0D7FB0598AD +:100ED00000F0D8FF46463C4600F08CFFA6E50646EB +:100EE0004EE64FF0000901E6BA467EE637467CE65C +:100EF0009422002000220020A086010070470000FC +:100F000070470000704700002DE9F04100F5803780 +:100F1000044616463B7C5BB9C0681030204400F0A4 +:100F2000F7FEE5683544B5F5004FE56002D816B127 +:100F3000BDE8F081DEB905F07F0605F11000002163 +:100F4000C6F180062044F6B232462E4400F006FF79 +:100F5000A06804F11008324600F10060414600F537 +:100F6000003006F069F830B901233B74E0E74FF434 +:100F700000463546ECE7A26805F1100140463244D0 +:100F80002144A260E268521BE26000F0C1FE022030 +:100F9000BDE8F04100F09ABE183000F0E9BC000056 +:100FA00010B5044601F002F8204610BD10B5044605 +:100FB00000F0FCFF204610BDC3B280B2A3F1410295 +:100FC000052A02D8373800B27047613B052B94BF21 +:100FD00057383038F7E70000F8B50446154608469C +:100FE00003220C4900F09EFE014688B908346F1CAC +:100FF00015F91100FFF7E0FF064617F9110001315E +:10100000FFF7DAFF102940EA061004F8010BEFD1D0 +:10101000F8BD00BF709200082DE9F04FADF53F7D9F +:101020000746416801222AA802F0A2FE002840F0EB +:1010300087800646824681461125DFF80C81DFF85D +:101040000CB101AB4FF4805241462AA802F0F0FFE8 +:10105000002875D1019AB2F5805F71D8002A65D059 +:101060000446019A9442ECD2282D0FD008DC132DAF +:101070002DD01E2D39D0112D13D00134A4B2F0E79C +:10108000322D2DD0372D2FD02D2DF6D13B68121BB0 +:1010900008EB040138461B692D259847BDF804402C +:1010A000EBE7121B022A09D9594608EB040000F0AD +:1010B00039FE18B902342825A4B2DEE718F8043046 +:1010C0003A2B3DD00A2B1CBFA1461325D5E718F8B3 +:1010D00004300A2B34D03A2B04BFA2463225CCE789 +:1010E00018F80430202BC8D0264618F804300A2BF4 +:1010F0001AD1AAEB090208EB090102A811254F2A0F +:1011000028BF4F2208F0A0F8A21B08EB060116A882 +:101110004F2A28BF4F2208F097F83B6816AA02A969 +:10112000DB6838469847A8E71E25A6E73B6838469F +:1011300004491B69984701200DF53F7DBDE8F08FFC +:101140000020F9E772930008A023002074920008A1 +:1011500000F1180110B5044686B00846019100F070 +:10116000F1FB2046FFF758FF60B1019902A800F09B +:1011700049FC102204F1080102A808F025F8B0FA91 +:1011800080F0400906B010BD70B504460025EEB2EF +:10119000304600F0FFFC58B100213046013500F028 +:1011A00009FD08B9002070BD022000F08FFDEEE7B8 +:1011B0002046FFF731FF0028F4D004F58034207C6E +:1011C00080F00100EFE70000F0B5C9B006F0A0F92B +:1011D00000F008FF18B90025284649B0F0BD69465F +:1011E0002A4802F0E7FF00284BD1294C204603F0A3 +:1011F00011F8284803F00EF8274803F00BF82146B1 +:10120000224803F083F80028E5D1702000F0C8FEE2 +:10121000064610B1214B44600360336830469B683A +:101220009847054600282ED01A4F1948394603F032 +:101230006DF805460028CED1194800F0B1FE0446ED +:1012400038B1184B4760036000F58033C0E90255A0 +:101250001D74236820469B689847054628B10E49AF +:101260000C4803F053F80028B5D1336830465B686A +:1012700098471CB1236820465B68984700F09AFEA7 +:10128000AAE70025FAE70446EFE700BF78920008D6 +:10129000889200089F920008B5920008D892000832 +:1012A00014000100F49200082DE9F04FD44A8DB0EB +:1012B0000B68D0F804A001931A440368D14E1A4475 +:1012C000D1F81C90DFF8B4C3DFF8B4B3D0E902342E +:1012D000634003EA0A03634013444A6802920AEB3C +:1012E0007363029CC84A2244C468224484688AEA20 +:1012F00004051D40654015448A68039203EB35558B +:10130000039CC24A2244846822448AEA03042C4093 +:1013100084EA0A041444CA6805EBF43404921644BF +:1013200083EA0502224056445A4032440E6905962B +:1013300004EBB222059FB64E3E441E4485EA0403E8 +:1013400013406B4033444E69069602EB7363069F6D +:10135000B04E3E442E4484EA02051D4065403544AB +:101360008E69079603EB3555079FAB4E3E442644E6 +:1013700082EA03042C4054403444A84E4E4405EB0A +:10138000F434164483EA050222405A4032440E6A7D +:10139000089604EBB222089FA14E3E441E4485EA03 +:1013A000040313406B4033444E6A099602EB7363A7 +:1013B000099F9C4ED1F830E03E44D1F83880F34488 +:1013C0002E4484EA02051D40654035448E6AA6F528 +:1013D000244703EB35550A964F3F274482EA03041E +:1013E0002C4054403C44CF6A0B9705EBF4340B9EE1 +:1013F0008D4F3744029E174483EA050222405A402B +:101400003A448A4F774404EBB2221F4485EA04032E +:1014100013406B403B444F6BBC4402EB7363654429 +:1014200084EA020C0CEA030C8CEA040C6544DFF835 +:1014300054C2C44403EB3555A44482EA03042C404F +:1014400054406444D1F83CC0794905EBF43461441C +:10145000114483EA050222405A400A44754904EBCC +:10146000B2223144079E194484EA02032B406340B0 +:101470000B44714902EBF36331440B9E0D4482EA45 +:1014800003012140514029446C4D03EBF151354497 +:10149000019E254483EA010414405C402C44684DBD +:1014A00001EBB4443544069E154481EA04021A4017 +:1014B0004A402A44634D04EB323235440A9E1D44AF +:1014C00084EA02030B4063402B445F4D02EBF3635D +:1014D0003544059E0D4482EA0301214051402944D0 +:1014E0005A4D03EBF1516544254483EA010414404D +:1014F0005C402C44564D01EBB4443544099E1544E0 +:1015000081EA04021A404A402A44524D04EB323226 +:101510003544049E1D4484EA02030B4063402B447F +:101520004D4D02EBF36345440D4482EA0301214033 +:1015300051402944494D03EBF1513544089E2C4458 +:1015400083EA010515405D402C44454D01EBB44450 +:101550003544039E2A4481EA04051D404D402A4437 +:10156000404D04EB32323D442B4484EA020593445F +:101570000D4065402B443C4D02EBF3633544069E21 +:10158000294482EA0305254055402944374D03EBA1 +:10159000F1514D442C4483EA010515405D4025443A +:1015A00001EBB54581EA050404EA03024A405A44C6 +:1015B000A6F5B82B089E05EB3232ABF2BE6B544059 +:1015C0005B4423442A4C344402EB33730B9E0C449B +:1015D00085EA020159402144264C344403EB715101 +:1015E000029E254482EA03044C402544224C444494 +:1015F00001EB3545144483EA01026A40224443E08A +:1016000078A46AD7EECEBDC156B7C7E8DB702024F8 +:10161000AF0F7CF52AC68747134630A8019546FDD3 +:10162000D8988069AFF7448BBED75C892211906B44 +:101630002108B44962251EF640B340C0515A5E26C7 +:10164000AAC7B6E95D102FD65314440281E6A1D88B +:10165000C8FBD3E7E6CDE121D60737C3870DD5F424 +:10166000ED145A4505E9E3A9F8A3EFFCD9026F6729 +:1016700081F6718722619D6D0C38E5FD937198FDAF +:101680008A4C2A8D8E4379A6934C344405EB722202 +:10169000059E1C4481EA0503534023448F4C344487 +:1016A00002EB33730A9E0C4485EA0201594021443F +:1016B0008B4C4C4403EB7151254482EA03044C40AB +:1016C0002C44884D354401EB3444019E154483EA93 +:1016D000010262402A44844D3D4404EB72221D44C1 +:1016E00081EA040353402B44804D354402EB3373AD +:1016F000049E294484EA02055D4029447C4D35441A +:1017000003EB7151079E254482EA03044C402C44AC +:10171000784D354401EB3444099E2A4483EA01059F +:1017200065401544744A324404EB7525039E134406 +:1017300081EA04026A401A44704B734405EB32722A +:101740000B4484EA0501514019446D4B634402EB9C +:1017500071511C4485EA02034B401C44694B3344DD +:1017600001EB3444019E1D4482EA010363402B4493 +:10177000654D04EB73233544069E154463EA01026C +:1017800062402A44614D03EBB2624D4462EA0409AF +:1017900029445F4D89EA0309454449442C445D4D81 +:1017A00002EBB1513544049E61EA03081D4488EA06 +:1017B0000208444401EB744464EA02034B402B44A6 +:1017C000554D04EBF323754463EA010E15448EEA8C +:1017D000040E0EEB0502514D03EBB262354462EA92 +:1017E000040E29440A9D8EEA030EA5F580164C4D81 +:1017F0007144A6F6833602EBB151264461EA030434 +:1018000054403444029E01EB7444354464EA0206B9 +:101810001D444E407319089E424D04EBF32335449A +:1018200063EA01061544664072193F4D03EBB2624C +:10183000654462EA040629443C4D5E403144079EFB +:1018400002EBB151354461EA03062C44384D564051 +:101850003D443444059E1D4401EB744464EA020394 +:101860004B402B44334D04EBF32335440B9E15447E +:1018700063EA010262402A442F4D03EBB262354411 +:10188000039E0D4462EA0401594029442B4D02EBAA +:10189000B15135442A4E254461EA030454402C4496 +:1018A000099D01EB74442E4464EA02051E4485EA56 +:1018B00001039D1903681A440AEB040303EBF523A3 +:1018C0000260436083681C44C36819448460C1603B +:1018D0000DB0BDE8F08F00BF44EABEA4A9CFDE4B37 +:1018E000604BBBF670BCBFBEC67E9B28FA27A1EA40 +:1018F0008530EFD4051D880439D0D4D9E599DBE6CD +:10190000F87CA21F6556ACC4442229F497FF2A43F1 +:10191000A72394AB39A093FCC3595B6592CC0C8F81 +:10192000D15D84854F7EA86FE0E62CFE144301A3B1 +:10193000A111084E827E53F735F23ABDBBD2D72AA9 +:1019400091D386EB094B036003F18833436003F1C5 +:101950002943A3F59613A3F68B638360A3F1883321 +:10196000C3600023C0E90433704700BF012345670B +:101970002DE9F8431446026905460E46E300C2F31A +:10198000C50800F118079B18036122BF43690133A2 +:10199000436112F4FC7F436903EB5473436114D039 +:1019A000C8F1400907EB08004C4504D22246BDE8C7 +:1019B000F84307F019BC403C4A464E4407F014FC7B +:1019C000444439462846FFF76FFCA04606EB04095D +:1019D000B8F13F0FA9EB08010AD94022384607F0B9 +:1019E00003FC39462846A8F14008FFF75DFCEFE705 +:1019F000A1096FF03F02384602FB014206EB81115C +:101A0000D5E7000070B50B6901F1180506460C46D4 +:101A1000C3F3C503EA18501C8022EA54C3F13F0205 +:101A2000072A1FD8002100F099F929462046FFF720 +:101A30003BFC38220021284600F090F92369294612 +:101A40002046236563696365FFF72EFC214610225B +:101A5000304607F0C9FB204658220021BDE87040FF +:101A600000F07CB9C3F137020021E5E72DE9F84F1A +:101A70004FF47A7306460D46002402FB03F7DFF8A5 +:101A80005080DFF8509098F900305FFA84FA5A1CC1 +:101A900001D0A34210D159F824002A4631460368E8 +:101AA000D3F820B03B46D847854205D1074B0120EB +:101AB00083F800A0BDE8F88F0134042CE3D14FF483 +:101AC000FA7004F0DBFD0020F4E700BFE4330020EF +:101AD0001022002014220020002307B50246012115 +:101AE0000DF107008DF80730FFF7C0FF20B19DF81A +:101AF000070003B05DF804FB4FF0FF30F9E700008A +:101B00000A46042108B5FFF7B1FF80F00100C0B21A +:101B1000404208BD074B0A4630B41978064B53F8CB +:101B20002140014623682046DD69044BAC4630BCA9 +:101B3000604700BFE433002014220020A08601008B +:101B400070B50A4E00240A4D05F01AFA3080286854 +:101B50003388834208D905F00FFA2B680444013317 +:101B6000B4F5003F2B60F2D370BD00BFE633002018 +:101B7000A033002005F0D2BA00F1006000F500307B +:101B80000068704700F10060920000F5003005F039 +:101B900053BA0000054B1A68054B1B889B1A8342F9 +:101BA00002D9104405F0E8B900207047A0330020A6 +:101BB000E633002038B50446074D29B12868204493 +:101BC000BDE8384005F0F0B92868204405F0DAF99E +:101BD0000028F3D038BD00BFA0330020002070479C +:101BE00000F1FF5000F58F10D0F80008704700009A +:101BF000064991F8243033B100230822086A81F89D +:101C00002430FFF7BFBF0120704700BFA43300207E +:101C1000014B1868704700BF0010005C194B013879 +:101C20000322084470B51D68174BC5F30B042D0C37 +:101C30001E88A6420BD15C680A46013C82421346CC +:101C40000FD214F9016F4EB102F8016BF6E7013AB9 +:101C500003F10803ECD181420B4602D22C2203F897 +:101C6000012B0424094A1688AE4204D1984284BF4D +:101C7000967803F8016B013C02F10402F3D1581A83 +:101C800070BD00BF0010005C2422002034930008C7 +:101C9000022804D1054B4FF400129A6170470128C5 +:101CA000FCD1024B4FF48012F7E700BF001002583E +:101CB000022803D1044B20229A6170470128FCD1ED +:101CC000014B1022F8E700BF00100258022805D18E +:101CD000064A536983F02003536170470128FCD101 +:101CE000024A536983F01003F6E700BF0010025860 +:101CF00070B504464FF47A764CB1412C254628BF86 +:101D0000412506FB05F0641B04F0B8FCF4E770BD48 +:101D1000002310B5934203D0CC5CC4540133F9E7DF +:101D200010BD0000013810B510F9013F3BB191F929 +:101D300000409C4203D11AB10131013AF4E71AB1D3 +:101D400091F90020981A10BD1046FCE703460246A0 +:101D5000D01A12F9011B0029FAD170470244034638 +:101D6000934202D003F8011BFAE770472DE9F843CC +:101D70001F4D14460746884695F8242052BBDFF8CD +:101D800070909CB395F824302BB92022FF2148464F +:101D90002F62FFF7E3FF95F824004146C0F10802E7 +:101DA00005EB8000A24228BF2246D6B29200FFF780 +:101DB000AFFF95F82430A41B17441E449044E4B2AE +:101DC000F6B2082E85F82460DBD1FFF711FF00285A +:101DD000D7D108E02B6A03EB82038342CFD0FFF711 +:101DE00007FF0028CBD10020BDE8F8830120FBE7E6 +:101DF000A4330020024B1A78024B1A70704700BFC0 +:101E0000E433002010220020F8B5194C194803F0E3 +:101E10003DFF2146174803F065FF24684FF47A70B0 +:101E2000154ED4F89020154DD2F80438114F43F0D8 +:101E30000203C2F80438FFF75BFF2046104904F0A4 +:101E40005FF8D4F890200424D2F8043823F0020379 +:101E5000C2F804384FF4E133336055F8040BB8424C +:101E600002D0314603F070FE013CF6D1F8BD00BF50 +:101E7000309B000818490020CC3300201422002099 +:101E8000389B00080C4B70B50C4D04461E780C4B6B +:101E900055F826209A420DD00A4B002118221846E8 +:101EA000FFF75CFF0460014655F82600BDE870406E +:101EB00003F04ABE70BD00BFE433002014220020AE +:101EC00018490020CC330020F0B5A1B071B6002332 +:101ED0000120002480261A46194602F087FD4FF49F +:101EE000D067214A3D25136923BBD2F810310BBBC3 +:101EF000036804F1006199600368C3F80CD00368BB +:101F00005E6003681F6001680B6843F001030B60AB +:101F100001680B6823F01E030B6001680B68DB0788 +:101F2000FCD4037B8034416805FA03F3B4F5001F49 +:101F30000B60D8D102F09AFDB4F5001F11D0002437 +:101F40000A4E0B4D012005F0E9F83388A34205D96C +:101F500028682044013405F027F8F6E7002005F052 +:101F6000DDF861B621B0F0BD00200052E63300205C +:101F7000A033002030B50A44084D91420DD011F82D +:101F8000013B5840082340F30004013B2C4013F070 +:101F9000FF0384EA5000F6D1EFE730BD2083B8EDAF +:101FA0000121884238BF084605F090B908B105F014 +:101FB00091B9704710B5084C01220849002001F082 +:101FC000B3FE23783BB1064803F036FD044803F026 +:101FD00069FD0023237010BDE833002044930008FE +:101FE000C83600201D482DE9F041036D2BB90122B0 +:101FF0004FF48051503005F0D1FA194E33780BB1BF +:10200000FFF7D8FF0324174F4FF00008134D154971 +:102010002846C7F8048003F037FD284603F070FB1C +:1020200048B1013C284603F03DFD14F0FF04EED119 +:10203000204634700FE00C4901220C4801F074FE78 +:10204000014618B1284603F0F7FCEAE7084800F01B +:1020500011F801203070BDE8F08100BFC8360020C3 +:10206000E83300203C22002044930008EC33002099 +:10207000489300080FB4002004B070470068704710 +:1020800003460068596870470B0A017043700B0CD7 +:10209000090E8370C1707047110A0270037141709C +:1020A000110C120E8170C2701A0A42711A0C1B0EAA +:1020B0008271C37170470000C36A0239023B8B42D0 +:1020C00083BF4389006C01FB0300002070470000C0 +:1020D000C2F307238A76CB760378032B01BF120C59 +:1020E0000A75120A4A75704700F10B010022D301EC +:1020F00043EA520310F8012B52FA83F38842DAB212 +:10210000F5D110467047000010B541780446002014 +:10211000013102464901022A16BFA35C032203EBE8 +:10212000C03302F101021EBF9BB203EB500398B211 +:102130009142F0D810BD000002684AB1134613F86E +:10214000011B1F290DD93A29F9D1911C8B4202D0CC +:102150004FF0FF3070471278302AF9D10360002029 +:102160007047014B187870473836002038B50D4657 +:10217000044618B9092000232B6038BD0368002BE2 +:10218000F8D01A78002AF5D08188DA889142F1D106 +:10219000587804F00FFC10F00100EBD12368EBE756 +:1021A00038B50D4640F25231144602F0B9F9FF2815 +:1021B00007D9012C0BD9030A022468702B70204622 +:1021C00038BD30B1002CFAD001242870F7E7002484 +:1021D000F5E70446F3E700002DE9F8430026D0F8C0 +:1021E000008005460C468E76836B002B4AD098F80B +:1021F0000030042B4BD133463546402720E0B7F55D +:10220000187F80F0C480F90606F1010608BF023786 +:10221000D05B02372BB900F5205292B2B2F5006FB5 +:102220000DD305F11A01C5F1FF0240EA0340214434 +:10223000FFF7B6FF002800F0AA80054400200346FF +:10224000D8F8102092F82310B142D8D8002B40F0D3 +:102250009E80002D00F09B8000232544AB766373A5 +:10226000D8F81020137903F03701DB0621730BD463 +:1022700002F13800FFF704FFC4E90001938963818C +:10228000D3892381BDE8F88300200146F4E7C36CBD +:1022900001335ED1EA6B00232E26551E184615F831 +:1022A000011F013020290CD0052908BFE52109288C +:1022B00004D10B2B9EBFE71801337E73E71801335F +:1022C00079730B28EBD1E11800204873A17E002917 +:1022D0004BD1002B40D06FF00C0604F10D00082507 +:1022E000361B331810F8011B002938D02E298BB269 +:1022F0004AD0A3F14101192903D8117B0D4200D026 +:1023000020330373EDE7B9F1000F05D100F5205339 +:102310009BB2B3F5006F0BD307F11A01C7F1FF02AF +:1023200040EA09402144FFF73BFF48B10744002041 +:1023300002368146D8F80C30985B0028E3D1384645 +:10234000B9F1000F4FF0000218BF002023189A7651 +:10235000A0E7B1463746EDE73F23A37601232344A8 +:1023600000219976137B03B96373D37A02F11C00C1 +:1023700003F03F0323730023FFF780FE20606360B8 +:10238000D38A6381138B7CE710250B46B9E73F2383 +:102390000125A37660E7000038B50546002435F82E +:1023A000020B08B9204638BD02F0EEF86308C2B24D +:1023B00003EBC43312FA83F39AB2C0F3072303EB9F +:1023C000520303EBC2339CB2E9E7000037B5C37890 +:1023D00004461BB90025284603B030BD00F14C016E +:1023E000826C01234078019104F00AFB054680B914 +:1023F000A36BE070A06C226BC31A9342EAD2A3785D +:102400000199022BE6D102440123607804F0F8FA26 +:10241000E1E70125DFE7000038B5836C05460C468F +:102420008B4210D0FFF7D2FF60B92246012305F19D +:102430004C01687804F0C0FA00281CBF4FF0FF344C +:102440000120AC6438BD0020FCE7000038B5002353 +:102450000446C3704FF0FF338364FFF7DDFF0028AD +:102460004BD1B4F84A524AF655239D4207D10B226C +:10247000254904F14C0006F0A7FE00283FD094F84F +:102480004C30EB2B03D01833DBB2012B2ED84AF69D +:1024900055239D4206D108221C4904F19E0006F0F6 +:1024A00093FE48B3B4F85730B3F5007F1ED194F8CB +:1024B0005930DBB15A1E1A4218D1B4F85A30ABB1B8 +:1024C00094F85C30013B012B10D8B4F85D306BB14F +:1024D000B4F85F307F2B06D804F16C00FFF7CEFD17 +:1024E000B0F5803F02D3B4F8623053B94AF65520B4 +:1024F00085420CBF0220032038BD0420FCE70120E8 +:10250000FAE70020F8E700BF749300088093000802 +:1025100002392DE9F04701F007044FF0010A466C3B +:1025200005460AFA04F41746984606EB1136C1F33D +:10253000C809E4B2314628460136FFF76DFF18B1ED +:102540000120BDE8F087994605EB090292F84C306E +:10255000234214BF01210021414513D06340013FB4 +:1025600082F84C3085F803A0EBD0640014F0FF042F +:10257000EAD109F1010301244FF00009B3F5007F0E +:10258000E1D1D7E70220DCE701290246F8B50C4685 +:1025900040F28C800668F36A8B4240F28780337881 +:1025A000013B032B00F28280DFE803F00229384B65 +:1025B00004EB5405B16B304601EB5521FFF72CFFBE +:1025C00010B14FF0FF30F8BD6F1CC5F30805B16BBB +:1025D0003046354401EB572195F84C50FFF71CFF6E +:1025E0000028EED1C7F30807E3073E4496F84C00F5 +:1025F00045EA00204CBF0009C0F30B00E3E7B16BD4 +:10260000304601EB1421FFF707FF0028D9D1640001 +:1026100004F4FF742644B6F84C00D4E7B16B30469E +:1026200001EBD411FFF7F8FE0028CAD1A40006F18F +:102630004C0004F4FE742044FFF720FD20F07040AD +:10264000C1E7D0E90430D57953EA000101D091689F +:1026500001B95DBB9168022DA4EB01010DD1013BD5 +:10266000728940F1FF305B0A43EAC053B3FBF2F3D7 +:1026700099421BD81CD0601CA5E7032D02D1936999 +:102680008B42F8D8D3699BB9B16B304601EBD411BA +:10269000FFF7C2FE002894D1A0004C3600F4FE7073 +:1026A0003044FFF7EBFC20F000408CE701208AE784 +:1026B0006FF0004087E70000F8B5066804460D4655 +:1026C0003378042B0CBF4FF080524FF400128A4233 +:1026D00001D80220F8BDCA06FBD182680163D2B9D5 +:1026E000022B13D83389B3EB551FF2D9F36BA363D5 +:1026F000A36B6263002BECD003EB55234C36C5F380 +:1027000008050020A3633544E563E3E7F36BC2717A +:10271000002BE7D01A4677897F02BD42114604D2CA +:102720003046FFF7C9FCA063E2E72046FFF72CFF25 +:10273000431C024606D00128CBD9F36A8342C8D98C +:10274000ED1BEAE70120C5E701292DE9F047064620 +:102750000C46174608D9C36A8B4205D90378022B69 +:1027600062D003D8012B22D0022552E0033B012B7B +:10277000FAD8816B01EBD411FFF74EFE0546002815 +:1027800047D1A40006F14C0304F4FE741C443378D2 +:10279000042B07D0204627F07047FFF76FFC00F0AE +:1027A0007040074339462046FFF76EFC2FE001EBEF +:1027B0005108816B01EB5821FFF72EFE054640BB07 +:1027C00014F0010406F14C0908F1010AC8F30808E5 +:1027D00008BFFBB230461FBF19F8083003F00F02E4 +:1027E0003B0103F0F00318BF134309F8083001233D +:1027F000B16BF37001EB5A21FFF70EFE054640B9AD +:10280000CAF3080A44B1C7F3071709F80A7001238D +:10281000F3702846BDE8F08719F80A30C7F303279C +:1028200023F00F031F43F0E7816B01EB1421FFF747 +:10283000F3FD05460028ECD1640006F14C0304F4D6 +:10284000FF741F551919C7F307274F70DFE7000002 +:10285000F8B504460E461746E3690BB91846F8BDAD +:10286000012BA6EB0305206814BFAA1C3A46691C7D +:10287000FFF76AFF0028F2D1E369013BE361EBE770 +:1028800001292DE9F84306460C461746056802D88B +:102890000220BDE8F883EB6A8B42F9D97AB9A146E8 +:1028A00021463046A046FFF76FFE0446B0B92B78AC +:1028B000042B02D1002F43D1F7710020E9E72B78D8 +:1028C000042B02D1C379022BE9D04FF0FF323946F5 +:1028D0002846FFF739FF0028E1D0DAE70128D7D0F2 +:1028E000421C01D10120D4E72B78042B19D1EA6ACC +:1028F000AB69023A93421CD308F10102A2420CD008 +:102900002B78042B08D10023A2EB09024946284664 +:10291000FFF7FEFD0028BCD1A146EB6AA342BFD859 +:10292000C5E7002241462846FFF70EFF0028DED00B +:10293000AFE70133AB612B7943F001032B71DBE788 +:10294000F3798BB9B468BC4202D10223F371B4E7C6 +:1029500021463046FFF718FE012899D9431CC1D003 +:1029600001348442EFD0A8E7032BA6D1B368BB4261 +:10297000A3D8B2691344BB429FD3E6E770B5C379CD +:102980000446032B06D181688369CD18A94203D17F +:102990000023E371002070BD4E1C20683246FFF713 +:1029A000D3FE0028F7D13146F0E700002DE9F743C8 +:1029B00005460191FFF70AFD0446002849D105F1BB +:1029C0004C09019928464FF40072FFF775FB214628 +:1029D000A86407464846FFF7C1F96C896402B4F55C +:1029E000004F28BF4FF40044B4F5007F2FD9204694 +:1029F00004F074FC804630B122460021640A0026AF +:102A0000FFF7ACF909E06408EEE72346BA1941463E +:102A1000687803F0F5FF18B926446B899E42F4D319 +:102A2000404604F06BFC6889801B18BF012003B08E +:102A3000BDE8F08301366B899E42F4D20123BA19B6 +:102A40004946687803F0DCFF0028F3D0EBE7002666 +:102A5000F1E70120EBE70000F8B50446FFF7B6FC0C +:102A60000546002842D12378032B37D12779012F3F +:102A700034D104F14C0601464FF400723046FFF7A2 +:102A80006DF955234122722184F84A32AA2304F5B4 +:102A90000D7084F84F2084F84B32522384F83012A2 +:102AA00084F84C3084F84D30612384F8311284F876 +:102AB0004E3084F83332A16984F83222FFF7E4FA09 +:102AC000616904F50E70FFF7DFFA626B3B46314631 +:102AD00001326078A26403F093FF257100226078D0 +:102AE000114603F0B1FF003818BF0120F8BD000007 +:102AF00000232DE9F0430B6085B00F461546FFF724 +:102B00001BFB061EC0F2B281804B53F82640002CFE +:102B100000F0AE813C6005F0FE0523786BB1607873 +:102B200003F048FFC70708D41DB110F0040500D01A +:102B30000A25284605B0BDE8F0830023F0B22370D3 +:102B4000607003F023FFC10700F194810DB14207CB +:102B5000EED400212046FFF779FC022840F099804E +:102B60006E4604F2122304F2522132461846103304 +:102B7000FFF784FA42F8040B8B42F7D1002556F890 +:102B8000041B00297DD02046FFF760FC012879D87E +:102B90000128A26C40F0C08004F1570304F18C01BD +:102BA00013F8015B002D7BD18B42F9D1B4F8B4301E +:102BB000B3F5807F74D194F8B830092B70D104F14B +:102BC0009400FFF75DFA4FF0FF33171841F1000151 +:102BD000BB4275EB010363D304F1A000FFF74EFA8B +:102BE00094F8BA302063012BA37059D194F8B990AE +:102BF00003FA09F91FFA89F36381002B50D0444B83 +:102C000004F1A800FFF73AFA0646984248D8831C18 +:102C1000626304F1A400E362FFF730FA00EB0208FC +:102C200004F19C00C4F84080FFF728FA10441FFA12 +:102C300089F2A06306FB02F313EB080345EB0502E0 +:102C40009F4271EB02032BD32E4604F1AC00FFF739 +:102C500015FAE06365B96389B34221D9E16B204677 +:102C6000FFF72AFA81192046FFF7D6FB98B90136FB +:102C7000631993F84C30812B14D02035C5F3080527 +:102C8000E8E703200135042D7FF479AF042807D14C +:102C900001E0042801D101254BE701287FF678AF38 +:102CA0000D2546E705F1140004F14C063044FFF70A +:102CB000E5F901280546F3D9E36A8342F0D9618931 +:102CC000821E236C02FB01336364A16B204601EB7F +:102CD000D511FFF7A1FB0028DDD105F07F0006EB41 +:102CE0008000FFF7CBF9431C03D00135A842ECD09C +:102CF000D6E70425C4E90500064A25700025138897 +:102D0000E56101339BB21380E38012E73C3600207B +:102D1000FDFFFF7F40360020B4F85730B3F5007F49 +:102D2000BED1B4F8626026B904F17000FFF7A6F9CD +:102D3000064694F85C302663591EA3700129AFD86B +:102D400094F859506581002DAAD0691E2942A7D157 +:102D5000B4F85D8018F00F0FA4F80880A0D1B4F883 +:102D60005F0018B904F16C00FFF788F9B4F85A1045 +:102D7000002995D006FB03FE01EB181CF4446045C6 +:102D80008ED3A0EB0C00A842B0FBF5F388D33E48ED +:102D9000834285D84FF6F57083426DD903259F1C79 +:102DA000114402EB0C03032DE7626263A163236409 +:102DB0004CD1B4F8763053EA08037FF471AFBB000E +:102DC00004F17800FFF75AF9E06303F2FF13B6EB62 +:102DD000532FFFF465AF4FF0FF33032DC4E90533E4 +:102DE0004FF08003237187D1B4F87C30012B83D15D +:102DF000511C2046FFF710FB00287FF47DAFB4F88C +:102E00004A224AF6552320719A427FF475AF1F4B30 +:102E100004F14C00FFF732F998427FF46DAF03F1F3 +:102E2000FF5304F50C70FFF729F903F50053203325 +:102E300098427FF461AF04F50D70FFF71FF9A061B0 +:102E400004F50E70FFF71AF9606155E7B8F1000F4D +:102E50003FF426AF7144022D4FEA4703E1631EBFE2 +:102E6000D91907F0010303EB5103AEE70B2560E628 +:102E70000C255EE603255CE640F6F575AB428CBF9B +:102E8000022501258BE700BFF5FFFF0F525261417C +:102E90002DE9F84F07460568884649B96E69C6B1FD +:102EA000EB6AB34298BF0126AB69A3B9002405E0E1 +:102EB000FFF76AFB0128044603D801242046BDE839 +:102EC000F88F421C00F0D280EB6A8342F6D8464667 +:102ED000EAE70126E8E72A78EB6A042A40F08380D3 +:102EE000A6F1020A023B4FF0010B9A4528BF4FF0B2 +:102EF000000AD146696C284601EB1931FFF78CFABC +:102F000000283BD109F00703EA6AC9F3C8010BFAAC +:102F100003F3901EDBB26A184C4609F1010992F8DE +:102F20004C20814502EA030233BF5B0000234FF4CB +:102F30000071DBB228BF9946B2B90234631E033375 +:102F4000BCD80123214628461A46FFF7E1FA022899 +:102F5000B3D0012800F08A80B8F1000F13D102230A +:102F6000FB710028A9D130E0CA450AD0002BD2D18C +:102F70000131B1F5007FBDD20123CCE74FF0FF3422 +:102F8000DCE70024DAE7FB79022B07D1731CA342AC +:102F9000E7D0BB68F31ABB610323FB7108F10102A0 +:102FA000FB69A24205D113B10133FB61D9E70223CA +:102FB000FBE70BB90123FB61224641463846FFF788 +:102FC00047FC00284FD10123FB61EA6AAB69023A52 +:102FD0006C6193429CBF03F1FF33AB612B7943F0EB +:102FE00001032B716AE7464514D1741C3846A3428D +:102FF00098BF02242146FFF7C7FA01283FF45DAFCE +:10300000431C33D0E0B16B69012B03D9EA6A9342C8 +:1030100038BF1E4634460134EB6AA34203D8012E62 +:103020007FF644AF022421463846FFF7ADFA48B197 +:1030300001283FF442AF013018D0B442EBD135E75C +:10304000002CE7D04FF0FF3221462846FFF77CFBEB +:1030500048B9B8F1000FB8D0224641462846FFF7DC +:1030600073FB0028B1D001287FF427AF4FF0FF3465 +:1030700024E700002DE9F84306680446076B8946FB +:1030800033782037042B0CBF4FF080534FF40013DC +:10309000BB429CBF00238363836B73B1C7F30808F3 +:1030A000B8F1000F3CD10133416B836339B93389E7 +:1030B000B3EB571F34D80023A36304200AE07389BD +:1030C000013B13EA57232BD1FFF75EFA012805468F +:1030D00002D80220BDE8F883421C01D10120F9E7A3 +:1030E000F36A834216D8B9F1000FE4D0616B204631 +:1030F000FFF7CEFE0546C8B10128EAD0431CEDD04B +:1031000001463046FFF752FC0028E7D1E37943F04F +:103110000403E371294630466563FEF7CDFFA063E3 +:103120004C36002027634644E663D3E70720D1E707 +:10313000F8B50E46002104460768FFF7BDFA98B9B6 +:103140000546A16B3846FFF767F968B93A78E36B33 +:10315000042A1B780CD11B060ED5054601212046FA +:10316000FFF788FF0028ECD0042808BF072006E0FE +:10317000E52B01D0002BF0D10135B542EED1F8BDE1 +:10318000C16C4B1C2DE9F04104460568066B1FD14C +:10319000E5274FF00108A16B2846FFF73DF998B9E4 +:1031A0002A78E36B042A09BF1A781F7002F07F02A5 +:1031B0001A7085F80380236BB3420DD2002120469C +:1031C000FFF758FF0028E6D0042808BF022003E0DC +:1031D000FFF772FA0028DBD0BDE8F0812DE9F0415D +:1031E00005460068A96B0669FFF716F9044620B981 +:1031F000EB6B1A78852A03D002242046BDE8F081C3 +:10320000324603F1200153F8040B8B4242F8040BC1 +:10321000F9D1777801377F01A7F16003B3F5007F1B +:10322000EAD800212846FFF725FF04280446E3D00A +:103230000028E2D1A96B2868FFF7EEF804460028C1 +:10324000DBD1EB6B1A78C02AD6D106F1200203F14C +:10325000200153F8040B8B4242F8040BF9D196F885 +:1032600023300F222C33B3FBF2F3B7EB431FC3D34E +:103270004FF0400800212846FFF7FCFE04280446D2 +:10328000BAD00028B9D1A96B2868FFF7C5F8044661 +:103290000028B2D1EB6B1A78C12AADD1B8F5187FEE +:1032A00009D206EB080203F1200153F8040B8B420C +:1032B00042F8040BF9D108F120084745DAD8B8F5EF +:1032C000187F9AD83046FEF71FFF7388834294D048 +:1032D00092E700000B68002210B5036004460B6AF9 +:1032E00083604B6AC261C37123F0FF03896AC0E93E +:1032F0000432C164FFF7E0F920B92046BDE8104070 +:10330000FFF76CBF10BD0000F8B503680546012744 +:103310001C692046FEF7F8FEA070000A6678E0708F +:103320002846E96CFFF7C8F920B1022828BF02201F +:10333000C0B2F8BDA96B2868FFF76EF80028F4D179 +:10334000EB6B04F1200254F8041B944243F8041B75 +:10335000F9D12B68DF70002EE7D000212846013E0E +:10336000FFF788FEE0E700002DE9F8434FF0FF0883 +:1033700006460768042445464FF6FF79B16B11B93C +:10338000002C73D063E03846FFF746F80446002867 +:103390005DD1F06B0378002B6ED03A78042A11D1FE +:1033A000852B4DD1336B3046F364FFF717FF04468E +:1033B00000284CD13B691B7903F03F03B3712046D1 +:1033C000BDE8F883C27AE52B02F03F02B27143D028 +:1033D0002E2B41D022F0200108293DD00F2A40D1C8 +:1033E000590637D503F0BF05336B90F80D80F364B1 +:1033F000437B434530D1428B72BB03780D21FC687F +:1034000023F04003DFF874E0013B4B4301211EF839 +:1034100001CB30F80CC009B3FF2B1DD824F813C022 +:103420006146013301320D2AF1D10278520605D5E9 +:1034300021B1FF2B10D8002224F81320013DEDB25A +:1034400000213046FFF716FE0446002896D00023E0 +:10345000B363B4E7AB42CBD0FF25F1E7CC45E1D075 +:10346000FAE72DB9FEF740FE404501D10024A6E75A +:103470004FF0FF33F364A2E70424E8E71C9400084C +:103480002DE9F04F002187B00446D0F80090FFF7F7 +:1034900013F9804670B999F80030042B33D1D9F86C +:1034A0000C00FEF779FF07462046FFF75DFF054653 +:1034B00020B18046404607B0BDE8F08FD9F8103003 +:1034C0009A8CBA42F0D193F823B040265D4506D1DC +:1034D000D9F80C3033F81530002BE5D1EAE7F106C6 +:1034E000D9F8103008BF0236985B01F04DF8D9F8D2 +:1034F0000C30824633F8150001F046F88245D3D1EE +:1035000002360135E2E74FF0FF0A4FF0FF3B554628 +:10351000C4F84CB0A16B4846FEF77EFF00285CD192 +:10352000E66B3778002F77D0F27AE52F02F03F0371 +:10353000A37103D0120704D50F2B04D0C4F84CB0EC +:103540004FE00F2B54D194F84B3058063FD47906F6 +:1035500045D5236B07F0BF0796F80DA0E364737B96 +:1035600053453ED1738B002B3BD135780121D9F8DF +:103570000C3005F03F0501930D23013D5D43284BC1 +:1035800013F8012BB25A71B3FF2D059329D81046B9 +:10359000049200F0F9FF6B1C03900293019B33F837 +:1035A000150000F0F1FF039981421AD1049A029D9F +:1035B0001146059B1B4A9342E2D133785A0604D543 +:1035C00019B1019B33F815305BB97D1EEDB20021B6 +:1035D0002046FFF74FFD00289CD080466AE7BD4299 +:1035E000BDD0FF25F3E74FF6FF708242E2D0F8E747 +:1035F0002DB93046FEF778FD50453FF45BAF94F8A7 +:103600004B30DB079AD40B2204F14001304605F021 +:10361000DBFD002892D14DE74FF004084AE700BFD8 +:103620001C940008299400082DE9F04F90F84BB045 +:1036300099B004461BF0A00540F068810668F26866 +:1036400032F81530002B4AD13378042B40F08780B4 +:103650000F230E352046B5FBF3F5A91CFFF768FDD7 +:103660008146002877D1236B0135A3EB4515E3791B +:103670005A07E56435D523F004032046E371FFF7CC +:103680007DF950BB4FF0FF32616B2046FFF7E0F849 +:1036900018BBA3682BB3214604A8FFF71BFEE0B9B3 +:1036A00070894FF40071D4E90423E0FB01233069F1 +:1036B000C4E904233830FEF7EFFC3069D4E9042371 +:1036C0002830FEF7E9FCE379326904A843F00103EE +:1036D00082F82130FFF718FE18B181463BE0013532 +:1036E000AEE7D6E90354402200212046FEF736FB20 +:1036F0008523012140222370C0234FF0C10C04EB2D +:10370000010884F8203000231E469E46571C04F80A +:1037100002C0F0B2023204F807E021B135F813100C +:1037200009B10133DBB20F0AA15408F8027002326A +:10373000D706F2D135F813700136002FE6D184F8A0 +:103740002330831C28466370FEF726FE84F824008D +:10375000000A84F82500484619B0BDE8F08F04F14E +:1037600040070DF1100A1BF0010F97E807008AE8E7 +:10377000070000F0D38040234FF0010884F84B305D +:10378000BC46F368B8F1050F9AE80700ACE80300FF +:103790002CF8022B4FEA12428CF8002059D9981EBF +:1037A000424630F8021F002942D10DF10F0C0721CB +:1037B00002F00F0E914612090EF13000392888BF31 +:1037C0000EF1370001390CF8010902D0B9F10F0FE1 +:1037D000EED818AB7E205A1802F8580C3846002252 +:1037E000914206D010F801CB02F1010EBCF1200F7E +:1037F00031D104F13F0C072902F1010297BF18AB48 +:1038000020205818013198BF10F8580C072A0CF8DE +:103810000200F0D92046FFF733FE8146002878D118 +:1038200008F10108B8F1640FAAD14FF0070992E737 +:103830004FF0100C01F0010E49080EEB4202D303C9 +:1038400044BF82F4883282F02102BCF1010CF1D134 +:10385000A7E74246A9E77246C2E7216B2046A1EBE3 +:103860004511FEF729FF814600287FF474AF4FF61B +:10387000FF783846FEF738FC0190A16B3046FEF722 +:10388000CBFD814600287FF466AFE36BE9B2019A75 +:103890004FF00D0CD6F80CE05A734FF00F02DFF822 +:1038A000E0A0DA724A1E18730CFB02F28446987686 +:1038B000D87640451AF8019B0CF1010C18BF3EF870 +:1038C000120003EB090B18BF013203F809004FEA9D +:1038D0001029002808BF4046BCF10D0F8BF801905D +:1038E000E7D1404502D03EF812200AB941F040012C +:1038F0001970012300212046F370FFF7BBFB8146BE +:1039000000287FF428AF013DB7D11BE04FF0060936 +:1039100021E704287FF41FAF84F84BB01BF0020F9F +:1039200020461BBF0C350D210125B5FBF1F518BF55 +:1039300001352946FFF7FCFB814600287FF40BAFD9 +:10394000013D8AD1A16B3046FEF766FD8146002815 +:103950007FF401AF01462022E06BFEF7FFF9E36B35 +:1039600003CF18605960BA7839889A72198194F82F +:103970004B30E26B03F0180313730123F370EAE694 +:103980001C94000810B504460A463430FEF776FB56 +:10399000886004F13800FEF773FBC2E9040194F873 +:1039A000213003F00203D3710023D36110BD000066 +:1039B00003284B8B04BF8A8A43EA024318467047A8 +:1039C0002DE9F04F0B7899B0044689462F2BD0F89B +:1039D00000B001D05C2B09D14A46137891460132E0 +:1039E0002F2BFAD05C2BF8D0002301E0DBF81C3041 +:1039F000A3600023E3619BF80030042B1ED1A36871 +:103A0000E3B1DBF82030214604A82362DBF8243040 +:103A10006362DBF82830A362FFF75CFC03460028F2 +:103A200054D1DBF8102002F13800FEF727FBC4E97F +:103A3000040392F8213003F00203E37199F8003097 +:103A40001F2B00F2358180230021204684F84B3063 +:103A500019B0BDE8F04FFEF72FBE49460B788946F6 +:103A600001312F2BFAD05C2BF8D01F2B8CBF0025F7 +:103A70000425012F2FD113882E2B31D1002322F8BA +:103A8000173004F140029F428CBF2E2120210133C8 +:103A90000B2B02F8011BF6D145F02005204684F8D7 +:103AA0004B50FFF7EDFC94F84B30002800F0E78016 +:103AB00004280BD1990603F0040240F1DC80002AAF +:103AC00000F0F6808023002084F84B3019B0BDE868 +:103AD000F08F0425CDE7022F02D153882E2BCAD0B8 +:103AE000911E87BB002322F81730002F00F01181B0 +:103AF00032F81300194601332028F9D009B92E28CD +:103B000001D145F00305901E30F817302E2B01D05F +:103B1000013FF9D14FF020334FF0000A6364D046E3 +:103B20002364C4F847300823481C32F81160009021 +:103B3000F6B1202E03D02E2E0DD1B84210D045F074 +:103B400003050099F0E731F81730202B01D02E2B18 +:103B5000C8D1013FC5E79A4505D20099B9423BD18A +:103B60000B2B30D101E00B2B27D145F003050B23A4 +:103B700094F84020E52A04BF052284F84020082B51 +:103B800004BF4FEA88085FFA88F808F00C030C2B92 +:103B900003D008F00303032B01D145F00205A80769 +:103BA0003FF57CAF18F0010F18BF45F0100518F075 +:103BB000040F18BF45F0080570E70099B94202D01C +:103BC00045F00305D4D84FEA88080B234FF0080AC4 +:103BD00000975FFA88F8B4E77F2E15D9304640F297 +:103BE0005231CDE9022345F00203019300F098FC25 +:103BF00010F0800F0646DDE9022316D000F07F06A4 +:103C000046498E5D019D46B331464548CDE90123C5 +:103C100005F004FBDDE90123F8B9A6F1410189B201 +:103C200019291ED848F0020810E0FF28EAD9591EC9 +:103C30008A4503D345F003059A4682E704EB0A015F +:103C4000000A0AF1010A019D81F8400004EB0A0113 +:103C50000AF1010A81F8406073E745F003055F2629 +:103C6000F4E7A6F1610189B219299EBF203E48F010 +:103C70000108B6B2EAE7002A08BF052026E75A077E +:103C80003FF524AFA379DB0645D59BF80000042857 +:103C900035D1A3682146E27923622369DBF810005D +:103CA00023F0FF0313436362E36CA362FFF76AFE32 +:103CB00023680027DA6819F8010B00283FF409AFE0 +:103CC00040F25231009200F04BFC054608B31F2829 +:103CD000009A7FF6FEAE2F283FF4BFAE5C283FF47B +:103CE000BCAE7F2805D801460E4805F097FA009A29 +:103CF00078B9FF2F0DD022F817500137DBE7216B81 +:103D00000BF14C03C1F308011944FFF751FEA06009 +:103D1000CEE70620DAE60520D8E600BF9C9300082F +:103D2000959300088C9300081FB5CDE9001003A8F7 +:103D300014460391FEF700FA002815DB0B4A52F8EF +:103D400020300BB100211970019B0BB100211970BB +:103D500042F820302CB1002201A96846FEF7C8FEC7 +:103D60000446204604B010BD0B24FAE73C36002080 +:103D70002DE9F04798B0904605460191002800F0E3 +:103D8000528102F03F0603A901A83246FEF7B0FEB9 +:103D9000002840F04681039B4FF48C60049303F0AD +:103DA0009DFA0746002800F04081039B00F5007251 +:103DB0000199D86004A81A61FFF702FE044620B9F1 +:103DC0009DF95B30002BB8BF062418F01C0F00F0E3 +:103DD000CD80002C4CD0042C40D104A8FFF724FC4B +:103DE000044600283AD146F00806039B1A78042AB4 +:103DF00040F08380186929462B60FFF7C3FD039BC1 +:103E00001E22002118690230FDF7A8FF039C002143 +:103E10001A2220692630FDF7A1FF236920221A719A +:103E2000246903F095FA0146012208342046FEF782 +:103E30002BF9039B04A81B6983F82120FFF764FA80 +:103E4000044658B9A96801B302462846FEF718FD92 +:103E5000AB68039A0446013B5361B4B1384603F0A2 +:103E60004DFA0CB100232B60204618B0BDE8F08756 +:103E70009DF8163013F0110F40F0848018F0040FF5 +:103E800040F0C98018F0080FAFD1039A3107139999 +:103E9000936C48BF46F04006E964AB641078042890 +:103EA00072D1069B9DF817102B62089B106923F0B6 +:103EB000FF030B4329466B62179BAB62FFF762FD62 +:103EC000DDF80CA00024002205F15008BAF80630F5 +:103ED00021464046C5F800A0AB80002385F830603D +:103EE00085F831406C64C5E90E234FF40072FDF78C +:103EF00035FFB20653D40024B0E703F029FA014697 +:103F0000009013980E30FEF7BFF813980099163002 +:103F1000FEF7BAF8039C13992078FFF749FD202398 +:103F200000228046CB7220461399FEF7D1F8139BEE +:103F3000002201211A775A779A77DA77039BD97092 +:103F4000B8F1000FA1D0414604A8D3F84890FEF77D +:103F500097FC0446002881D149460398FEF75CFA95 +:103F6000039B044608F1FF30586176E7002C7FF48C +:103F700075AF9DF81630DC064FD418F0020F84D0D0 +:103F8000D80782D5072469E7FFF712FD0023A86050 +:103F900001F11C00FEF772F86B61286190E7D5E92A +:103FA000046956EA0903A6D0BAF80AA0A9684FEA3C +:103FB0004A2AC5E90E69B24574EB09031BD30024F4 +:103FC0002964002C7FF44AAFC6F30803002B92D07B +:103FD000039C2046FEF770F808B3760A0123414699 +:103FE00046EAC95682196A64607802F0E5FC041E4C +:103FF00018BF012432E72846FEF7C6FAB6EB0A06D8 +:10400000014669F10009012803D9431CD3D10124D9 +:10401000D6E70224D4E7082420E704241EE702247C +:104020001CE704461EE709241EE711241CE70000D4 +:104030002DE9F04F994685B00023884603A9044630 +:10404000C9F800301646FEF791F8054680BB94F893 +:1040500031506DBB94F8303013F00103009300F041 +:10406000A68004F1500AD4E90432D4E90E011B1AE7 +:1040700062EB0102B34272F1000238BF1E46BEB1CC +:10408000D4E90E10C1F30803002B40F08280039B9B +:104090005A894B0A013A43EAC0531A401BD151EAEC +:1040A000000309D1A06801280DD8022584F83150F9 +:1040B000284605B0BDE8F08F216C20460192FEF73E +:1040C00063FA019AEFE7431C04D10123009D84F8B1 +:1040D0003130EDE72064DDF80CB0216C5846FDF777 +:1040E000EBFF0028E1D0B6F5007F02EB000731D3EB +:1040F000BBF80A1002EB5620730A88429BF80100B5 +:1041000088BF8B1A3A464146019302F055FC0028BD +:10411000DBD194F93020019B002A0BDA606CC01BC4 +:10412000984207D24FF40072514608EB4020FDF749 +:10413000EFFD019B5F02D9F80030F61BB8443B4409 +:10414000C9F80030D4E90E32DB1942F10002C4E9AB +:104150000E3294E7626CBA421AD094F93030002BD8 +:104160000DDA012351469BF8010002F049FC0028BA +:10417000ABD194F8303003F07F0384F830300398EB +:1041800001233A465146407802F016FC00289CD1A3 +:104190006764A16B4046C1F30801C1F50077514443 +:1041A000B74228BF37463A46FDF7B2FDC3E70725B9 +:1041B0007EE7000070B596B00E460022019002A97D +:1041C00001A8FEF795FC0446E0B94FF48C6003F0BB +:1041D00085F80546D8B1029B00F500720199D860B8 +:1041E00002A81A61FFF7ECFB044640B99DF9533071 +:1041F000002B0ADB1EB1314602A8FDF7EDFF284671 +:1042000003F07CF8204616B070BD0624F7E71124B1 +:10421000F8E7000070B5B8B00222019003A901A828 +:10422000FEF766FC044608BB039B4FF48C601093BA +:1042300003F054F80546002866D0039B00F5007291 +:104240000199D86010A81A61FFF7BAFB044650B96B +:104250009DF88B30980655D4190653D49DF84630F6 +:10426000DA0706D50724284603F048F8204638B078 +:1042700070BD039B04931878042814D104A918690D +:10428000FFF780FB069E9DF84630DB0610D410A891 +:10429000FEF776FF04460028E5D156BB0398FEF7EB +:1042A000DBFB0446DFE71F99FFF782FB0646EAE7E0 +:1042B000039BDA69B242D5D024930021269624A824 +:1042C0001B78042B01BFDDE90823CDE928239DF8E5 +:1042D00017308DF89730FEF7EFF904460028C2D169 +:1042E00024A8FFF741F804460028BBD00428BAD11F +:1042F000CDE70246314604A8FEF7C2FA044600287C +:10430000B1D1CBE70624AEE71124AFE7F0B5BDB0DD +:10431000CDE900106846FDF70FFF022203A901A8AE +:10432000FEF7E6FB0446002841D1039B4FF48C6066 +:10433000149302F0D3FF0546002800F0EE80039BA3 +:1043400000F5007214AE0199D8601A613046FFF78B +:1043500037FB044640BB9DF89B3013F0A00F40F0A4 +:10436000D880039B009F1A78042A68D11B6904AC8B +:1043700003F1400C1868083353F8041C22466345C7 +:1043800003C21446F6D15022314628A8FDF7C0FCDE +:10439000394628A8FFF714FB044600284CD12A9A76 +:1043A000169B9A4206D00824284602F0A7FF204612 +:1043B0003DB0F0BD349A209B9A42F4D128A8FFF773 +:1043C00033F904460028EFD1039B04AF1B6993F82F +:1043D00001E093F823C09C8C3A46083303CAB242EA +:1043E00043F8080C43F8041C1746F5D1039B28A892 +:1043F0001B6983F801E0039B1A6982F823C01A69DC +:1044000082F82440240A82F825401A691379D906D3 +:104410005CBF43F020031371FEF776FF04460028CB +:10442000C2D13046FEF7ACFE04460028BCD103984A +:10443000FEF712FB0446B7E70428B5D1BEE7239A7E +:1044400004AB02F1200C1068083252F8041C1C4620 +:10445000624503C42346F6D15022314628A8FDF711 +:1044600057FC394628A8FFF7ABFA044600284CD180 +:104470002A9A169B9A4296D1349A209B9A4292D1BC +:1044800028A8FFF7D1F8044600288DD137990DF1FF +:104490001D030DF12D0001F10D0253F8044B834271 +:1044A00042F8044BF9D11888012710809B7893704B +:1044B0009DF81B30039CDA0658BF43F02003CB72F3 +:1044C000E770CB7ADB06ACD5169A2A9B9A42A8D025 +:1044D0002078FFF76DFA01462046FDF7EDFD014615 +:1044E000C8B12046FDF798FF044600287FF45CAF72 +:1044F000039890F86D302E2B93D12A9A00F16C011D +:10450000FDF7E6FD039BDF708BE704287FF44CAFDB +:10451000B6E7062448E7022446E7112447E70000EF +:104520007F2810B501D880B210BDB0F5803F13D2FE +:1045300040F2523399420FD10849002231F8024B20 +:1045400093B2844203D103F18000C0B2ECE70132A0 +:10455000802AF3D11346F6E70020E5E7DC96000851 +:104560007F280DD940F25233994208D1FF2806D84E +:1045700000F10040034B803833F8100070470020F2 +:10458000704700BFDC960008B0F5803FF0B522D23E +:104590001F4A83B21F49B0F5805F28BF0A46141D29 +:1045A00034F8042C2146AAB1934213D334F8025CA8 +:1045B0002E0AEFB252FA85F5A84222DA082E09D85F +:1045C000DFE806F0050A10121416181A1C00801AEB +:1045D00034F810301846F0BD981A00F001001B1A8C +:1045E0009BB2F7E7103BFBE7203BF9E7303BF7E7EF +:1045F0001A3BF5E70833F3E7503BF1E7A3F5E35344 +:10460000EEE70434002ECBD101EB4702C7E700BF31 +:104610002C9400082096000808B5074B074A196833 +:1046200001F03D01996053680BB190689847BDE86F +:10463000084003F0A3B800BF000002404436002049 +:1046400008B5084B1968890901F03D018A019A6093 +:10465000054AD3680BB110699847BDE8084003F0DC +:104660008DB800BF000002404436002008B5084B5A +:104670001968090C01F03D010A049A60054A536962 +:104680000BB190699847BDE8084003F077B800BFC8 +:10469000000002404436002008B5084B1968890D17 +:1046A00001F03D018A059A60054AD3690BB1106A91 +:1046B0009847BDE8084003F061B800BF0000024021 +:1046C0004436002008B5074B074A596801F03D0100 +:1046D000D960536A0BB1906A9847BDE8084003F06F +:1046E0004DB800BF000002404436002008B5084B1A +:1046F0005968890901F03D018A01DA60054AD36AE7 +:104700000BB1106B9847BDE8084003F037B800BF05 +:10471000000002404436002008B5084B5968090CD7 +:1047200001F03D010A04DA60054A536B0BB1906B4E +:104730009847BDE8084003F021B800BF00000240E0 +:104740004436002008B5084B5968890D01F03D0139 +:104750008A05DA60054AD36B0BB1106C9847BDE847 +:10476000084003F00BB800BF0000024044360020B0 +:1047700008B5074B074A196801F03D019960536C71 +:104780000BB1906C9847BDE8084002F0F7BF00BF3E +:10479000000402404436002008B5084B1968890916 +:1047A00001F03D018A019A60054AD36C0BB1106D8E +:1047B0009847BDE8084002F0E1BF00BF0004024096 +:1047C0004436002008B5084B1968090C01F03D017A +:1047D0000A049A60054A536D0BB1906D9847BDE885 +:1047E000084002F0CBBF00BF000402404436002066 +:1047F00008B5084B1968890D01F03D018A059A60DA +:10480000054AD36D0BB1106E9847BDE8084002F021 +:10481000B5BF00BF000402404436002008B5074B76 +:10482000074A596801F03D01D960536E0BB1906E93 +:104830009847BDE8084002F0A1BF00BF0004024055 +:104840004436002008B5084B5968890901F03D013C +:104850008A01DA60054AD36E0BB1106F9847BDE844 +:10486000084002F08BBF00BF000402404436002025 +:1048700008B5084B5968090C01F03D010A04DA60DB +:10488000054A536F0BB1906F9847BDE8084002F09E +:1048900075BF00BF000402404436002008B5084B35 +:1048A0005968890D01F03D018A05DA60054AD36F28 +:1048B00013B1D2F880009847BDE8084002F05EBF0F +:1048C000000402404436002000230C4910B51A466B +:1048D0000B4C0B6054F82300026001EB43000433DF +:1048E0004260402BF6D1074A4FF0FF339360D3600C +:1048F000C2F80834C2F80C3410BD00BF44360020A2 +:10490000DC970008000002400F28F8B510D91028E5 +:1049100010D0112811D0122808D10F240720DFF859 +:10492000C8E00126DEF80050A04208D9002653E076 +:104930000446F4E70F240020F1E70724FBE706FA1A +:1049400000F73D424AD1264C4FEA001C3D4304EBA0 +:1049500000160EEBC000CEF80050C0E90123FBB2F8 +:1049600073B12048D0F8D83043F00103C0F8D830F4 +:10497000D0F8003143F00103C0F80031D0F8003125 +:1049800017F47F4F0ED01748D0F8D83043F0020309 +:10499000C0F8D830D0F8003143F00203C0F800313D +:1049A000D0F8003154F80C00036823F01F030360B3 +:1049B000056815F00105FBD104EB0C033D2493F8C9 +:1049C0000CC05F6804FA0CF43C602124056044616B +:1049D00012B1987B00F0CAFC3046F8BD0130A3E765 +:1049E000DC970008004402584436002010B53024FB +:1049F00084F31188FFF788FF002383F3118810BD2B +:104A000010B50446807B00F0C7FC01231549627B8A +:104A100003FA02F20B6823EA0203DAB20B6072B9FE +:104A2000114AD2F8D81021F00101C2F8D810D2F8FA +:104A3000001121F00101C2F80011D2F8002113F495 +:104A40007F4F0ED1084BD3F8D82022F00202C3F8D2 +:104A5000D820D3F8002122F00202C3F80021D3F8B5 +:104A6000003110BD443600200044025808B5302300 +:104A700083F31188FFF7C4FF002383F3118808BD77 +:104A8000836CC26A8B42506810B506D95A1E4C001E +:104A900002EB4103B3FBF4F3184410BD01F0010332 +:104AA0008A0748BF43F002034A0748BF43F00803A0 +:104AB0000A0748BF43F00403CA0648BF43F0100387 +:104AC0008A06426B48BF43F0200313434363704799 +:104AD00010B5074C204600F0CBFF064B0022C4E97E +:104AE0001023054BA364054BE363054BE36410BD42 +:104AF000C83600200024024800B4C4041C3700203B +:104B00001C390020C36A0BB9104BC3620379012B17 +:104B100011D10F4B98420ED10E4BD3F8DC2042F44A +:104B20000072C3F8DC20D3F8042142F40072C3F809 +:104B30000421D3F80431436C00221A65DA621A604A +:104B40005A605A624FF0FF329A637047DC9800084F +:104B5000C8360020004402580379012B1BD0436C57 +:104B600000221A65DA621A605A605A624FF0FF3208 +:104B70009A63094B98420ED1084BD3F8DC2022F4FB +:104B80000072C3F8DC20D3F8042122F40072C3F8C9 +:104B90000421D3F804317047C8360020004402587D +:104BA00010B5446C0649FFF76BFF6060236842F262 +:104BB000107043F003032360BDE8104001F05EBDB8 +:104BC000801A06000129F8B5466C0B4F09D17568AB +:104BD0000A493D40FFF754FF054345F48055756091 +:104BE000F8BD746806493C40FFF74AFF044344F4AB +:104BF00080547460F4E700BF00ECFFFF80F0FA021D +:104C000040787D01436C00225A601A6070470000B2 +:104C1000426C0129536823F4404304D0022905D093 +:104C200001B95360704743F48043FAE743F400430B +:104C3000F7E70000436C41F480519A60D9605A6BE9 +:104C40001206FCD580229A637047000010B541F42B +:104C50008851446CA260E160616B11F04502FBD0A9 +:104C6000A26311F0040203D0FFF718FF012010BD6A +:104C7000616910461960FAE710B541F48851446C37 +:104C8000A260E160616B11F04502FBD0A26311F0FC +:104C9000050203D0FFF702FF012010BD6169104635 +:104CA0001960FAE773B5134604460E46302282F3C4 +:104CB0001188426CD26B32B14FF0FF31403001931A +:104CC00001F0E8FC019B606C00220265C263C262D5 +:104CD000456B15F4807504D185F31188012002B06D +:104CE00070BD4FF0FF31816382F31188012E06D928 +:104CF0000C21204602B0BDE87040FFF7BDBF104652 +:104D0000EDE7000073B5446C0E4600250192616B1F +:104D1000A1632565E562FFF7C1FE012E07D9019B5E +:104D20002A460C2102B0BDE87040FFF7A5BF02B0D3 +:104D300070BD000010B541F49851446CA260E16070 +:104D4000616B11F04502FBD0A26311F03F0203D06A +:104D5000FFF7A4FE012010BD216A10461960E16929 +:104D60005960A16999606169D960F4E72DE9F74359 +:104D700004460191006D01A91746984602F026FCF1 +:104D8000064600284AD0626C2046DDF8049055683B +:104D9000C5F3090501356B00A56CB5FBF3F54FF4C0 +:104DA0007A73B5FBF3F55D43556200F027FE50BB07 +:104DB000636C4FF0FF3201254146C3F858902046FE +:104DC0001D659A634FF49572DA6342F207029F629F +:104DD000DA62E36C0A9AFFF74FFFA0B9E26C104B5E +:104DE00011680B407BB929462046FFF75BFF05465B +:104DF00048B92E463A460199206D02F01FFC304614 +:104E000003B0BDE8F0833A460199206D02F016FC2C +:104E1000E26C01212046FFF775FFF0E70126EEE77F +:104E200008E0FFFD2DE9F7431F46436C01924FF464 +:104E30007A725D6804468846C5F3090501356E003F +:104E4000856CB5FBF6F5B5FBF2F555435D6200F0F8 +:104E5000D5FD20B10125284603B0BDE8F0837E02D0 +:104E600001A9206D324602F0B1FB05460028F1D0C1 +:104E7000636C019AD4F84C909A6501221A654FF040 +:104E8000FF329A634FF49572DA639E62236BDB06FE +:104E90004B4658BF4FEA4828012F42461BD91221E2 +:104EA0002046FFF7E9FEC0B9D9F80020104B1340A7 +:104EB0009BB9636C42F2930239462046DA62E26C97 +:104EC000FFF7F0FE804640B932460199206D454615 +:104ED00002F0B4FBBFE71121E2E732460199206DF1 +:104EE00002F0ACFBE26C39462046FFF70BFFB2E75D +:104EF00008E0FFFD2DE9F3411F46436C01924FF49A +:104F00007A725D6804468846C5F3090501356E006E +:104F1000856CB5FBF6F5B5FBF2F555435D6200F027 +:104F20006DFD20B10125284602B0BDE8F0817E026A +:104F300001A9206D324602F08FFB05460028F1D012 +:104F4000636C019A9A6501221A654FF0FF329A63E9 +:104F50004FF48D72DA639E62236BE66CDB06334698 +:104F600058BF4FEA4828012F424619D91921204637 +:104F7000FFF782FEB0B932680F4B134093B9636CF0 +:104F800042F2910239462046DA62E26CFFF78AFE6D +:104F9000064638B901993546206D02F099FBC2E703 +:104FA0001821E4E70199206D02F092FBE26C39468A +:104FB0002046FFF7A7FEB6E708E0FFFD12F0030F5B +:104FC0002DE9F04107460C4615461E4617D00E4403 +:104FD000B44202D10020BDE8F0810123FA6B2146E2 +:104FE0003846FFF71FFF0028F5D128464FF400721E +:104FF000F96B05F500750134FCF78AFEE8E7BDE8BA +:10500000F041FFF70FBF000012F0030F2DE9F04150 +:1050100007460C4615461E4617D00E44B44202D130 +:105020000020BDE8F08129464FF40072F86B05F5C9 +:105030000075FCF76DFE0123FA6B21463846FFF739 +:1050400059FF0028EDD10134E8E7BDE8F041FFF752 +:1050500051BF000000207047302310B583F3118842 +:105060000024436C40302146DC6301F021FB84F3D3 +:10507000118810BD026843681143016003B11847ED +:1050800070470000024A136843F0C0031360704782 +:1050900000480040024A136843F0C00313607047A1 +:1050A00000500040024A136843F0C0031360704789 +:1050B00000780040064BD3F8E8200243C3F8E8200C +:1050C000D3F810211043C3F81001D3F81031704702 +:1050D0000044025837B5274C274D204600F028FDE4 +:1050E00004F11400009400234FF40072234900F0EF +:1050F000C3F94FF40072224904F138000094214BA7 +:1051000000F03CFA204BC4E91735204C204600F053 +:105110000FFD04F11400009400234FF400721C49A9 +:1051200000F0AAF94FF400721A4904F13800009413 +:10513000194B00F023FA194BC4E91735184C2046D7 +:1051400000F0F6FC04F1140000234FF4007215493E +:10515000009400F091F9144B4FF40072134904F1DC +:105160003800009400F00AFA114BC4E9173503B077 +:1051700030BD00BF2039002000E1F505643A002071 +:105180006440002085500008004800408C39002011 +:10519000643C00206442002095500008005000400C +:1051A000F8390020643E0020A55000086444002027 +:1051B0000078004038B5264D0446037C002918BF0E +:1051C0000D46012B06D1234B984230D14FF480205D +:1051D000FFF770FF2A68236EE16D03EB5203A566AB +:1051E000B3FBF2F36A68100442BF23F0070003F038 +:1051F000070343EA4003CB60AB6843F040034B60D6 +:10520000EB6843F001038B6042F4967343F00103B3 +:105210000B604FF0FF330B62510505D512F01022E1 +:1052200011D0B2F1805F10D084F8643038BD0A4BE1 +:10523000984205D0094B9842CCD14FF08040C7E747 +:105240004FF48010C4E77F23EEE73F23ECE700BF75 +:10525000E4980008203900208C390020F83900201B +:105260002DE9F047C66D05463768F46921073462B9 +:105270001AD014F0080118BF4FF48071E20748BF3C +:1052800041F02001A3074FF0300348BF41F0400137 +:10529000600748BF41F0800183F31188281DFFF7A4 +:1052A000E9FE002383F31188E2050AD5302383F356 +:1052B00011884FF48061281DFFF7DCFE002383F383 +:1052C00011884FF030094FF0000A14F0200838D14F +:1052D0003B0616D54FF0300905F1380A200610D5E7 +:1052E00089F31188504600F07DF9002836DA08214C +:1052F000281DFFF7BFFE27F080033360002383F3F0 +:105300001188790614D5620612D5302383F31188EB +:10531000D5E913239A4208D12B6C33B127F040070B +:105320001021281DFFF7A6FE3760002383F31188A4 +:10533000E30618D5AA6E1369ABB15069BDE8F04712 +:10534000184789F31188736A284695F86410194044 +:1053500000F008FC8AF31188F469B6E7B06288F3BC +:105360001188F469BAE7BDE8F0870000090100F18F +:105370006043012203F56143C9B283F8001300F0D2 +:105380001F039A4043099B0003F1604303F5614307 +:10539000C3F880211A60704700F01F0301229A4071 +:1053A000430900F160409B0000F5614003F1604358 +:1053B00003F56143C3F88020C3F88021002380F8FF +:1053C00000337047F8B51546826804460B46AA427A +:1053D00000D28568A1692669761AB5420BD21846B3 +:1053E0002A46FCF795FCA3692B44A3612846A368D1 +:1053F0005B1BA360F8BD0CD9AF1B18463246FCF707 +:1054000087FC3A46E1683044FCF782FCE3683B44A1 +:10541000EBE718462A46FCF77BFCE368E5E700006B +:1054200083689342F7B50446154600D28568D4E9EF +:105430000460361AB5420BD22A46FCF769FC636950 +:105440002B4463612846A3685B1BA36003B0F0BDD7 +:105450000DD93246AF1B0191FCF75AFC01993A462F +:10546000E0683144FCF754FCE3683B44E9E72A4632 +:10547000FCF74EFCE368E4E710B50A440024C3617E +:10548000029B8460C16002610362C0E90000C0E960 +:10549000051110BD08B5D0E90532934201D18268EB +:1054A00082B98268013282605A1C426119700021FF +:1054B000D0E904329A4224BFC368436101F012F973 +:1054C000002008BD4FF0FF30FBE7000070B530232F +:1054D00004460E4683F31188A568A5B1A368A269A6 +:1054E000013BA360531CA36115782269934224BF3A +:1054F000E368A361E3690BB120469847002383F377 +:105500001188284607E03146204601F0DBF80028E4 +:10551000E2DA85F3118870BD2DE9F74F04460E4697 +:1055200017469846D0F81C904FF0300A8AF311883D +:105530004FF0000B154665B12A4631462046FFF76D +:1055400041FF034660B94146204601F0BBF8002800 +:10555000F1D0002383F31188781B03B0BDE8F08FEE +:10556000B9F1000F03D001902046C847019B8BF38F +:105570001188ED1A1E448AF31188DCE7C160C3610B +:10558000009B82600362C0E905111144C0E900007C +:1055900001617047F8B504460D461646302383F383 +:1055A0001188A768A7B1A368013BA36063695A1C6F +:1055B00062611D70D4E904329A4224BFE3686361DA +:1055C000E3690BB120469847002080F3118807E07B +:1055D0003146204601F076F80028E2DA87F3118898 +:1055E000F8BD0000D0E9052310B59A4201D18268C8 +:1055F0007AB982680021013282605A1C82611C786B +:1056000003699A4224BFC368836101F06BF82046A6 +:1056100010BD4FF0FF30FBE72DE9F74F04460E4673 +:1056200017469846D0F81C904FF0300A8AF311883C +:105630004FF0000B154665B12A4631462046FFF76C +:10564000EFFE034660B94146204601F03BF80028D2 +:10565000F1D0002383F31188781B03B0BDE8F08FED +:10566000B9F1000F03D001902046C847019B8BF38E +:105670001188ED1A1E448AF31188DCE70379052BA3 +:1056800005BF836A002001204B6004BF4FF4007304 +:105690000B60704770B55D1E866A04460D44B542C6 +:1056A00005D9436B43F080034363012070BD062599 +:1056B0000571FFF783FC05232371F7E770B55D1EC5 +:1056C000866A04460D44B54205D9436B43F0800316 +:1056D0004363012070BD07250571FFF795FC052385 +:1056E0002371F7E738B505790446052D05D1082360 +:1056F0000371FFF7AFFC257138BD0120FCE7000006 +:105700000323F0B5037185B00446FFF749FA002280 +:1057100020461146FFF78EFA4FF4D57203AB0821ED +:105720002046FFF7A9FA0246B8B901232363039B79 +:10573000C3F30323012B09D103AB37212046FFF725 +:105740009BFA18B9A44B039A1340ABB1204601252C +:10575000FFF758FA0223237137E103AB0022372108 +:105760002046FFF789FA28B99B4A039B1A40002A72 +:1057700000F0A78002232363236B03F00F03022BA7 +:1057800040F0A9806425954E42F2107000F076FF3B +:1057900003AB324601212046FFF758FA0028D5D145 +:1057A000039B002B80F293805A0003D5236B43F0B8 +:1057B00010032363002204F1080302212046FFF7AF +:1057C000B9FA02460028C1D104F13803032120466A +:1057D000FFF752FA0028B9D104F11805A26B09218C +:1057E00020462B46FFF7A6FA0028AFD102ABA26BEA +:1057F00007212046FFF740FA06460028A6D1236B72 +:1058000003F00F03022B40F08F807E227F21284679 +:1058100003F022FA012840F28780E76B42F2107011 +:1058200000F02CFF08234FF4007239462046009602 +:10583000FFF79CFA002889D1384603F05BFA236B06 +:10584000A06203F00F03022B72D103AB644A06215E +:105850002046FFF711FA002871D15F49039B1940D8 +:10586000B1FA81F149092046FFF7ACF902AB4FF4D8 +:10587000007210212046FFF7FFF9054600287FF44B +:1058800065AF554E029B33427FF460AF236B13F03C +:105890000E0F03F00F0273D0022A7FF457AFE36AB2 +:1058A0001978012900F09480022900F093800029E2 +:1058B00000F089804B4F2046FFF7AAF903AB3A4628 +:1058C00076E0114620462263FFF7B4F954E7013D24 +:1058D0007FF45AAF3AE7444D6426444A3E4F012BC9 +:1058E00018BF154603AB002237212046FFF7C4F945 +:1058F00000287FF42BAF039B3B427FF427AF03AB21 +:105900002A4629212046FFF7A1F900287FF41EAF7F +:10591000039B002BFFF648AF013E3FF417AF42F266 +:10592000107000F0ABFEDDE7284603F0B7F986E71C +:105930007E227F212846E66B03F08EF908B900210C +:1059400091E7002340223146204600930623FFF7CB +:105950000DFA0028F3D1B3895BBA9B07EFD5244B2E +:1059600040223146204600930623FFF7FFF9002826 +:10597000E5D1317C01F00F010F3918BF012172E729 +:10598000E36A1978F9B101297FF4E0AE2046FFF708 +:105990003FF903ABA26B37212046FFF76DF90028D2 +:1059A0007FF4D4AE039B33427FF4D0AE03AB02222C +:1059B00006212046FFF760F900287FF4C7AE039B5D +:1059C00033427FF4C3AE05232371284605B0F0BDF2 +:1059D000084F70E7084F6EE708E0FFFD0080FFC04A +:1059E0000001B9030000B7030080FF5000001080E1 +:1059F000F1FFFF800001B7030002B70337B504468B +:105A00000C4D01ABA26B0D212046FFF735F978B99B +:105A1000019B2B420BD1C3F34323042B08D0053B3E +:105A2000022B04D84FF47A7000F028FEE9E7012039 +:105A300003B030BD08E0FFFD70B53023054683F3A9 +:105A4000118803790024022B03D184F311882046A6 +:105A500070BD0423037184F311880226FFF7CEFF83 +:105A600004462846FFF7CEF82E71F0E7FFF730B86E +:105A7000044B03600123037100234363C0E90A332D +:105A8000704700BFFC98000810B53023044683F32C +:105A90001188C162FFF736F802230020237180F3DA +:105AA000118810BD10B53023044683F31188FFF729 +:105AB00053F800230122E362227183F3118810BDA1 +:105AC000026843681143016003B118477047000042 +:105AD0001430FFF721BD00004FF0FF331430FFF703 +:105AE0001BBD00003830FFF797BD00004FF0FF33BB +:105AF0003830FFF791BD00001430FFF7E7BC00001D +:105B00004FF0FF311430FFF7E1BC00003830FFF7F1 +:105B100041BD00004FF0FF323830FFF73BBD0000C1 +:105B2000012914BF6FF0130000207047FFF7D2BAAD +:105B3000044B036000234360C0E902330123037474 +:105B4000704700BF2099000810B53023044683F346 +:105B50001188FFF72FFB02230020237480F31188A4 +:105B600010BD000038B5C36904460D461BB90421B9 +:105B70000844FFF7A5FF294604F11400FFF78AFC4B +:105B8000002806DA201D4FF40061BDE83840FFF719 +:105B900097BF38BD026843681143016003B11847DD +:105BA0007047000013B5406B00F58054D4F8A4385A +:105BB0001A681178042914D1017C022911D11979AC +:105BC000012312898B4013420BD101A94C3002F002 +:105BD00071FFD4F8A4480246019B2179206800F0A7 +:105BE000DFF902B010BD0000143002F0F3BE000077 +:105BF0004FF0FF33143002F0EDBE00004C3002F0E5 +:105C0000C5BF00004FF0FF334C3002F0BFBF0000B3 +:105C1000143002F0C1BE00004FF0FF31143002F02A +:105C2000BBBE00004C3002F091BF00004FF0FF32CD +:105C30004C3002F08BBF00000020704710B500F51B +:105C40008054D4F8A4381A681178042917D1017C3B +:105C5000022914D15979012352898B4013420ED164 +:105C6000143002F053FE024648B1D4F8A4484FF471 +:105C7000407361792068BDE8104000F07FB910BD25 +:105C8000406BFFF7DBBF0000704700007FB5124B91 +:105C900001250426044603600023057400F18402F4 +:105CA00043602946C0E902330C4B02901430019343 +:105CB0004FF44073009602F005FE094B04F694423F +:105CC000294604F14C000294CDE900634FF440737F +:105CD00002F0CCFE04B070BD48990008815C000859 +:105CE000A55B00080A68302383F311880B790B3316 +:105CF00042F823004B79133342F823008B7913B118 +:105D00000B3342F8230000F58053C3F8A418022394 +:105D10000374002080F311887047000038B5037FBA +:105D2000044613B190F85430ABB90125201D02216F +:105D3000FFF730FF04F114006FF00101257700F048 +:105D4000DDFC04F14C0084F854506FF00101BDE813 +:105D5000384000F0D3BC38BD10B5012104460430F2 +:105D6000FFF718FF0023237784F8543010BD00009C +:105D700038B504460025143002F0BCFD04F14C0097 +:105D8000257702F08BFE201D84F854500121FFF787 +:105D900001FF2046BDE83840FFF750BF90F8803043 +:105DA00003F06003202B06D190F881200023212AE4 +:105DB00003D81F2A06D800207047222AFBD1C0E949 +:105DC0001D3303E0034A426707228267C36701204D +:105DD000704700BF4422002037B500F58055D5F844 +:105DE000A4381A68117804291AD1017C022917D124 +:105DF0001979012312898B40134211D100F14C040F +:105E0000204602F00BFF58B101A9204602F052FED5 +:105E1000D5F8A4480246019B2179206800F0C0F81B +:105E200003B030BD01F10B03F0B550F8236085B02D +:105E300004460D46FEB1302383F3118804EB850739 +:105E4000301D0821FFF7A6FEFB6806F14C005B69D8 +:105E50001B681BB1019002F03BFE019803A902F000 +:105E600029FE024648B1039B2946204600F098F8D7 +:105E7000002383F3118805B0F0BDFB685A691268EE +:105E8000002AF5D01B8A013B1340F1D104F18002B6 +:105E9000EAE70000133138B550F82140ECB1302367 +:105EA00083F3118804F58053D3F8A428136852793A +:105EB00003EB8203DB689B695D6845B104216018D0 +:105EC000FFF768FE294604F1140002F029FD204680 +:105ED000FFF7B4FE002383F3118838BD704700003C +:105EE00001F04ABD01234022002110B5044600F80C +:105EF000303BFBF733FF0023C4E9013310BD000042 +:105F000010B53023044683F3118824224160002118 +:105F10000C30FBF723FF204601F050FD0223002048 +:105F2000237080F3118810BD70B500EB8103054626 +:105F300050690E461446DA6018B110220021FBF7B2 +:105F40000DFFA06918B110220021FBF707FF3146B1 +:105F50002846BDE8704001F037BE0000836820226B +:105F6000002103F0011310B5044683601030FBF7E5 +:105F7000F5FE2046BDE8104001F0B2BEF0B40125A8 +:105F800000EB810447898D40E4683D43A469458165 +:105F900023600023A2606360F0BC01F0CFBE00006C +:105FA000F0B4012500EB810407898D40E4683D438E +:105FB0006469058123600023A2606360F0BC01F086 +:105FC00045BF000070B5022300250446242203705B +:105FD0002946C0F888500C3040F8045CFBF7BEFE40 +:105FE000204684F8705001F083FD63681B6823B17C +:105FF00029462046BDE87040184770BD0378052B40 +:1060000010B504460AD080F88C300523037043682D +:106010001B680BB1042198470023A36010BD00004A +:106020000178052906D190F88C20436802701B681E +:1060300003B118477047000070B590F870300446FF +:1060400013B1002380F8703004F18002204601F083 +:106050006BFE63689B68B3B994F8803013F06005F9 +:1060600035D00021204602F05DF90021204602F0E3 +:106070004DF963681B6813B1062120469847062333 +:1060800084F8703070BD204698470028E4D0B4F8FA +:106090008630A26F9A4288BFA36794F98030A56FBB +:1060A000002B4FF0300380F20381002D00F0F280CE +:1060B000092284F8702083F3118800212046D4E956 +:1060C0001D23FFF76DFF002383F31188DAE794F8AF +:1060D000812003F07F0343EA022340F2023293421D +:1060E00000F0C58021D8B3F5807F48D00DD8012BB2 +:1060F0003FD0022B00F09380002BB2D104F1880234 +:1061000062670222A267E367C1E7B3F5817F00F00F +:106110009B80B3F5407FA4D194F88230012BA0D1AD +:10612000B4F8883043F0020332E0B3F5006F4DD08D +:1061300017D8B3F5A06F31D0A3F5C063012B90D869 +:106140006368204694F882205E6894F88310B4F85F +:106150008430B047002884D0436863670368A3672E +:106160001AE0B3F5106F36D040F6024293427FF446 +:1061700078AF5C4B63670223A3670023C3E794F8FF +:106180008230012B7FF46DAFB4F8883023F0020326 +:10619000A4F88830C4E91D55E56778E7B4F8803085 +:1061A000B3F5A06F0ED194F88230204684F88A307F +:1061B00001F0FCFC63681B6813B10121204698477D +:1061C000032323700023C4E91D339CE704F18B03F0 +:1061D00063670123C3E72378042B10D1302383F3B3 +:1061E00011882046FFF7BAFE85F311880321636802 +:1061F00084F88B5021701B680BB12046984794F8A7 +:106200008230002BDED084F88B3004232370636847 +:106210001B68002BD6D0022120469847D2E794F87D +:10622000843020461D0603F00F010AD501F06EFDF3 +:10623000012804D002287FF414AF2B4B9AE72B4B94 +:1062400098E701F055FDF3E794F88230002B7FF4D6 +:1062500008AF94F8843013F00F01B3D01A0620462B +:1062600002D502F077F8ADE702F068F8AAE794F8F3 +:106270008230002B7FF4F5AE94F8843013F00F01D8 +:10628000A0D01B06204602D502F04CF89AE702F097 +:106290003DF897E7142284F8702083F311882B4689 +:1062A0002A4629462046FFF769FE85F31188E9E66C +:1062B0005DB1152284F8702083F3118800212046F7 +:1062C000D4E91D23FFF75AFEFDE60B2284F8702067 +:1062D00083F311882B462A4629462046FFF760FEA5 +:1062E000E3E700BF789900087099000874990008E6 +:1062F00038B590F870300446002B3ED0063BDAB239 +:106300000F2A34D80F2B32D8DFE803F037313108A9 +:10631000223231313131313131313737856FB0F897 +:1063200086309D4214D2C3681B8AB5FBF3F203FB8F +:1063300012556DB9302383F311882B462A4629461E +:10634000FFF72EFE85F311880A2384F870300EE0E3 +:10635000142384F87030302383F3118800232046FF +:106360001A461946FFF70AFE002383F3118838BD49 +:10637000C36F03B198470023E7E70021204601F0EF +:10638000D1FF0021204601F0C1FF63681B6813B1F3 +:106390000621204698470623D7E7000010B590F85D +:1063A00070300446142B29D017D8062B05D001D8FD +:1063B0001BB110BD093B022BFBD80021204601F088 +:1063C000B1FF0021204601F0A1FF63681B6813B1F3 +:1063D000062120469847062319E0152BE9D10B2307 +:1063E00080F87030302383F3118800231A46194651 +:1063F000FFF7D6FD002383F31188DAE7C3689B69B2 +:106400005B68002BD5D1C36F03B19847002384F894 +:106410007030CEE70023826880F8243083691B68DF +:1064200099689142FBD25A680360426010605860DC +:10643000704700000023826880F8243083691B685D +:1064400099689142FBD85A680360426010605860B6 +:106450007047000008B50846302383F3118891F88F +:106460002430032B05D0042B0DD02BB983F31188D6 +:1064700008BD8B6A00221A604FF0FF338362FFF77A +:10648000C9FF0023F2E7D1E9003213605A60F3E755 +:10649000034610B51B68984203D09C688A689442F2 +:1064A000F8D25A680B604A601160596010BD000054 +:1064B000FFF7B0BF064BD96881F82400186802685E +:1064C00053601A600122D86080F82420F9F79ABF3F +:1064D000684600200C4B30B5DD684B1C87B0044685 +:1064E0000FD02B46094A684600F09CF92046FFF77A +:1064F000E1FF009B13B1684600F09EF9A86A07B05F +:1065000030BDFFF7D7FFF9E7684600205564000863 +:10651000044B1A68DB6890689B68984294BF00201F +:106520000120704768460020094B10B51C68D868E8 +:10653000226853601A600122DC6084F82420FFF78F +:1065400079FF01462046BDE81040F9F75BBF00BF68 +:1065500068460020044B1A68DB6892689B689A4280 +:1065600001D9FFF7E1BF70476846002038B5012325 +:10657000084C00252370656002F0EAFB02F010FC75 +:106580000549064802F0E6FC0223237085F31188D2 +:1065900038BD00BF104900208099000868460020DF +:1065A00000F080B9034A516853685B1A9842FBD8DF +:1065B000704700BF001000E08B604B630023CA618E +:1065C00000F128020B6302230A618B840123886196 +:1065D00081F8263001F11003C26A4A611360C36278 +:1065E00001F12C030846CB6270470000D0E901316D +:1065F000026841F8183CA1F19C033839CB6003696B +:1066000041F8243C436941F8203C034B41F8043CE9 +:10661000C3680248FFF7D0BF1D0400086846002089 +:1066200008B5FFF7E3FFBDE80840FFF741BF0000F2 +:1066300038B50E4BDC6804F12C05A062E06AA84274 +:106640000FD194F826303BB994F825309B0702BF50 +:10665000D4E9043213605A600F20BDE83840FFF7D8 +:1066600029BF0368E362FFF723FFE7E768460020DE +:10667000302383F31188FFF7DBBF000008B5014624 +:10668000302383F311880820FFF724FF002383F3CE +:10669000118808BD054BDB6821B1036098620320B7 +:1066A000FFF718BF4FF0FF30704700BF684600206B +:1066B00003682BB10022026018469962FFF7F8BE0A +:1066C00070470000064BDB6839B1426818605A60B9 +:1066D000136043600420FFF7FDBE4FF0FF307047AA +:1066E000684600200368984206D01A68026050602D +:1066F00018469962FFF7DCBE7047000038B50446C3 +:106700000D462068844200D138BD036823605C6078 +:106710008562FFF7CDFEF4E7036810B59C68A242DE +:106720000CD85C688A600B604C6021605960996885 +:106730008A1A9A604FF0FF33836010BD121B1B68EA +:10674000ECE700000A2938BF0A2170B504460D465F +:106750000A26601902F0F6FA02F0DEFA041BA542DE +:1067600003D8751C04462E46F3E70A2E04D90120EF +:10677000BDE8704002F0B6BC70BD0000F8B5144B27 +:106780000D460A2A4FF00A07D96103F11001826011 +:1067900038BF0A22416019691446016048601861D7 +:1067A000A81802F0BFFA02F0B7FA431B0646A3424C +:1067B00006D37C1C28192746354602F0C3FAF2E7B7 +:1067C0000A2F04D90120BDE8F84002F08BBCF8BDC7 +:1067D00068460020F8B506460D4602F09DFA0F4ABD +:1067E000134653F8107F9F4206D12A460146304691 +:1067F000BDE8F840FFF7C2BFD169BB68441A2C1945 +:1068000028BF2C46A34202D92946FFF79BFF224608 +:1068100031460348BDE8F840FFF77EBF68460020D8 +:1068200078460020C0E90323002310B45DF8044B30 +:106830004361FFF7CFBF000010B5194C23699842A0 +:106840000DD08168D0E9003213605A609A680A441A +:106850009A60002303604FF0FF33A36110BD02680C +:10686000234643F8102F53600022026022699A42A7 +:1068700003D1BDE8104002F05FBA936881680B4411 +:10688000936002F049FA2269E1699268441AA242CF +:10689000E4D91144BDE81040091AFFF753BF00BF07 +:1068A000684600202DE9F047DFF8BC8008F11007AA +:1068B0002C4ED8F8105002F02FFAD8F81C40AA68D5 +:1068C000031B9A423ED814444FF00009D5E9003228 +:1068D000C8F81C4013605A60C5F80090D8F8103012 +:1068E000B34201D102F028FA89F31188D5E90331C6 +:1068F00028469847302383F311886B69002BD8D042 +:1069000002F00AFA6A69A0EB040982464A450DD2F0 +:10691000022002F0E7FB0022D8F81030B34208D181 +:1069200051462846BDE8F047FFF728BF121A224417 +:10693000F2E712EB09092946384638BF4A46FFF705 +:10694000EBFEB5E7D8F81030B34208D01444C8F8CD +:106950001C00211AA960BDE8F047FFF7F3BEBDE8AF +:10696000F08700BF784600206846002038B502F066 +:10697000D3F9054AD2E90845031B181945F100016E +:10698000C2E9080138BD00BF6846002010B560B9F3 +:10699000074804790368053C9B6818BF01249847A1 +:1069A00008B144F00404204610BD0124FBE700BFF9 +:1069B000C8360020FFF7EABF2DE9F04788461746A2 +:1069C0009A460446B0B90D4E3579052D05D00324FD +:1069D0000DE0013D15F0FF050ED0326853463946F3 +:1069E0003046D2F814904246C8470028F1D12046DC +:1069F000BDE8F0870424FAE70124F8E7C836002050 +:106A00002DE9F047884617469A460446B0B90D4E20 +:106A10003579052D05D003240DE0013D15F0FF0566 +:106A20000ED03268534639463046D2F81890424666 +:106A3000C8470028F1D12046BDE8F0870424FAE7D2 +:106A40000124F8E7C836002037B50C46154670B962 +:106A500051B101290BD10748694603681B6A984761 +:106A600010B9019B04462B60204603B030BD0424BE +:106A7000FAE700BFC836002000207047FEE700009C +:106A8000704700004FF0FF30704700004B684360D4 +:106A90008B688360CB68C3600B6943614B69036299 +:106AA0008B6943620B6803607047000008B53C4B7C +:106AB00040F2FF713B48D3F888200A43C3F888208E +:106AC000D3F8882022F4FF6222F00702C3F888205E +:106AD000D3F88820D3F8E0200A43C3F8E020D3F8A5 +:106AE00008210A43C3F808212F4AD3F80831114678 +:106AF000FFF7CCFF00F5806002F11C01FFF7C6FF35 +:106B000000F5806002F13801FFF7C0FF00F58060FA +:106B100002F15401FFF7BAFF00F5806002F1700145 +:106B2000FFF7B4FF00F5806002F18C01FFF7AEFFC4 +:106B300000F5806002F1A801FFF7A8FF00F5806072 +:106B400002F1C401FFF7A2FF00F5806002F1E0014D +:106B5000FFF79CFF00F5806002F1FC01FFF796FF54 +:106B600002F58C7100F58060FFF790FF01F08AFC60 +:106B70000E4BD3F8902242F00102C3F89022D3F8D2 +:106B8000942242F00102C3F894220522C3F898200F +:106B90004FF06052C3F89C20054AC3F8A02008BDFE +:106BA00000440258000002589499000800ED00E0EB +:106BB0001F00080308B501F071FEFFF7D7FC104B6A +:106BC000D3F8DC2042F04002C3F8DC20D3F80421E3 +:106BD00022F04002C3F80421D3F80431094B1A68AB +:106BE00042F008021A601A6842F004021A6000F0CB +:106BF00077FD00F037FBBDE8084000F0B5B800BFF6 +:106C000000440258001802480120704700207047D5 +:106C10007047000002290CD0032904D0012907483D +:106C200018BF00207047032A05D8054800EBC200B2 +:106C30007047044870470020704700BF949B0008CD +:106C400054220020489B000870B59AB005460846BB +:106C5000144601A900F0C2F801A8FBF777F8431C1D +:106C60000022C6B25B001046C5E90034237003233E +:106C7000023404F8013C01ABD1B202348E4201D897 +:106C80001AB070BD13F8011B013204F8010C04F8AE +:106C9000021CF1E708B5302383F311880348FFF79E +:106CA0009BF8002383F3118808BD00BF184900201A +:106CB00090F8803003F01F02012A07D190F881205C +:106CC0000B2A03D10023C0E91D3315E003F0600354 +:106CD000202B08D1B0F884302BB990F88120212ADC +:106CE00003D81F2A04D8FFF759B8222AEBD0FAE7B5 +:106CF000034A426707228267C3670120704700BFCB +:106D00004B22002007B5052917D8DFE801F0191636 +:106D100003191920302383F31188104A01210190AF +:106D2000FFF702F9019802210D4AFFF7FDF80D481F +:106D3000FFF71EF8002383F3118803B05DF804FB0E +:106D4000302383F311880748FEF7E8FFF2E730238A +:106D500083F311880348FEF7FFFFEBE7E89A00088A +:106D60000C9B00081849002038B50C4D0C4C2A46E5 +:106D70000C4904F10800FFF767FF05F1CA0204F1AE +:106D800010000949FFF760FF05F5CA7204F1180009 +:106D90000649BDE83840FFF757BF00BFF06100204B +:106DA00054220020C89A0008D29A0008DD9A0008F0 +:106DB00070B5044608460D46FAF7C8FFC6B220462D +:106DC000013403780BB9184670BD32462946FAF7EC +:106DD000A9FF0028F3D10120F6E700002DE9F84FC4 +:106DE00005460C46FAF7B2FF2D49C6B22846FFF712 +:106DF000DFFF08B10436F6B22A492846FFF7D8FF6C +:106E000008B11036F6B2632E0DD8DFF89490DFF893 +:106E100094A0DFF89C80DFF89CB0234F2E7846B911 +:106E20002670BDE8F88F29462046BDE8F84F02F0ED +:106E300003BA252E30D1072249462846FAF772FFB9 +:106E400070B93B6807350B3444F80B3C7B6844F859 +:106E5000073C3B8924F8033CBB7A04F8013CDDE79E +:106E6000082251462846FAF75DFFA8B9A21C0F4B2D +:106E700019780232090911F8081002F8041C13F8F5 +:106E8000011B01F00F015B4511F8081002F8031C0B +:106E9000EED118340835C1E7013504F8016BBDE7C0 +:106EA000B49B0008DD9A0008BC9B000800E8F11FB5 +:106EB000C89B00080CE8F11FBFF34F8F044B1A6901 +:106EC0005107FCD1D3F810215207F8D1704700BF09 +:106ED0000020005208B50D4B1B78ABB9FFF7ECFF53 +:106EE0000B4BDA68D10704D50A4A5A6002F188329E +:106EF0005A60D3F80C21D20706D5064AC3F80421FC +:106F000002F18832C3F8042108BD00BF4E6400209E +:106F1000002000522301674508B5114B1B78F3B9D7 +:106F2000104B1A69510703D5DA6842F04002DA6063 +:106F3000D3F81021520705D5D3F80C2142F04002B6 +:106F4000C3F80C21FFF7B8FF064BDA6842F00102E4 +:106F5000DA60D3F80C2142F00102C3F80C2108BD1D +:106F60004E640020002000520F289ABF00F5806078 +:106F700040040020704700004FF4003070470000CC +:106F8000102070470F2808B50BD8FFF7EDFF00F56C +:106F900000330268013204D104308342F9D1012068 +:106FA00008BD0020FCE700000F2838B505463FD893 +:106FB000FFF782FF1F4CFFF78DFF4FF0FF330728CD +:106FC0006361C4F814311DD82361FFF775FF030214 +:106FD00043F02403E360E36843F08003E360236944 +:106FE0005A07FCD42846FFF767FFFFF7BDFF4FF4B1 +:106FF000003100F0AFFA2846FFF78EFFBDE83840B9 +:10700000FFF7C0BFC4F81031FFF756FFA0F1080327 +:107010001B0243F02403C4F80C31D4F80C3143F0C4 +:107020008003C4F80C31D4F810315B07FBD4D9E7E6 +:10703000002038BD002000522DE9F84F05460C46CF +:10704000104645EA0203DE0602D00020BDE8F88FB4 +:1070500020F01F00DFF8BCB0DFF8BCA0FFF73AFF5C +:1070600004EB0008444503D10120FFF755FFEDE78D +:1070700020222946204602F0A7F810B920352034F6 +:10708000F0E72B4605F120021F68791CDDD104339F +:107090009A42F9D105F178431B481C4EB3F5801F85 +:1070A0001B4B38BF184603F1F80332BFD946D1460F +:1070B0001E46FFF701FF0760A5EB040C336804F1DF +:1070C0001C0143F002033360231FD9F8007017F04E +:1070D0000507FAD153F8042F8B424CF80320F4D162 +:1070E000BFF34F8FFFF7E8FE4FF0FF33202221461A +:1070F00003602846336823F00203336002F064F82B +:107100000028BBD03846B0E7142100520C200052B2 +:1071100014200052102000521021005210B5084CCB +:10712000237828B11BB9FFF7D5FE0123237010BDCA +:10713000002BFCD02070BDE81040FFF7EDBE00BF73 +:107140004E6400202DE9F04F0D4685B0814658B1C0 +:1071500011F00D0614BF2022082211F0080301933C +:1071600004D0431E034269D0002435E0002E37D0FE +:1071700009F11F0121F01F094FF00108314F05F0FF +:107180000403DFF8CCA005EA080BBBF1000F32D0F6 +:107190007869C0072FD408F101080C37B8F1060F41 +:1071A000F3D19EB9284D4946A819019201F01AFE63 +:1071B0000446002839D12036019AA02EF3D1494641 +:1071C00001F010FE044600282FD1019A49461F48BD +:1071D00001F008FE044660BB204605B0BDE8F08F14 +:1071E0000029C9D101462846029201F0FBFD044660 +:1071F000D8B9029AC0E713B178694107CBD5AC077B +:1072000002D578698007C6D5019911B1786901075F +:10721000C1D549460AEB4810CDE9022301F0E2FD51 +:107220000446DDE902230028B5D04A460021204665 +:1072300001E04A460021FAF791FDCDE70246002E13 +:1072400096D199E7DC9B00089064002050640020F0 +:10725000706400200021FFF775BF00000121FFF7D7 +:1072600071BF000070B5144D0124144E40F2FF327E +:1072700000210120FAF772FD06EB441001342A695F +:1072800055F80C1F01F09AFD062CF5D137254FF467 +:10729000C0542046FFF7E2FF014628B122460848C5 +:1072A000BDE8704001F08ABDC4EBC404013D4FEA63 +:1072B000D404EED170BD00BFDC9B000870640020D8 +:1072C000506400200421FFF73DBF00004843FFF752 +:1072D000C1BF000008B101F0F7BD7047B0F5805F95 +:1072E00010B5044607D8FFF7EDFF28B92046BDE8E2 +:1072F0001040FFF7AFBF002010BD0000FFF7EABF4E +:1073000070B5AAB140EA010313F01F030FD1094C75 +:107310000144A5686D0706D52568A84203D36668B1 +:107320003544A94204D903330C34122BF1D1002285 +:10733000104670BDDC9B000808B501F0DBFE034A77 +:10734000D2E90032C01842EB010108BD30650020CF +:10735000434BD3E900232DE9F34113437CD0FFF7DE +:10736000EBFF404A00230027F9F766F806460D4672 +:107370003D4A0023F9F760F80023144630462946B9 +:10738000394AF9F759F84FF461613C23ADF80170BF +:10739000B4FBF1F5B4FBF3F601FB154103FB164614 +:1073A0004624B1FBF3F1314BF6B28DF80040984220 +:1073B0003CD84FF0640C4FF4C87EA30704F26C7203 +:1073C00025D1B2FBFCF30CFB132313BBB2FBFEF382 +:1073D0000EFB1322B2FA82F35B0903F26D18621CF2 +:1073E0008045D2B217D90FB18DF800400022204C51 +:1073F0004FF00C0C17460CFB0343D4B2013213F8C8 +:1074000004C084450CD8A0EB0C000127F5E700234D +:10741000E3E70123E1E7A0EB080014460127CCE7EE +:107420000FB18DF80140431C8DF802309DF801002A +:10743000431C9DF800005038400640EA43509DF838 +:10744000023040EA034040EA560040EAC52040EAE4 +:10745000411002B0BDE8F0814FF40410F9E700BF1D +:107460003065002040420F008051010090230B0046 +:10747000249C00080244074BD2B210B5904200D1C0 +:1074800010BD441C00B253F8200041F8040BE0B2D8 +:10749000F4E700BF504000580E4B30B51C6F240479 +:1074A00005D41C6F1C671C6F44F400441C670A4C15 +:1074B00002442368D2B243F480732360074B9042A6 +:1074C00000D130BD441C51F8045B00B243F8205099 +:1074D000E0B2F4E700440258004802585040005817 +:1074E00007B5012201A90020FFF7C4FF019803B0EE +:1074F0005DF804FB13B50446FFF7F2FFA04205D088 +:10750000012201A900200194FFF7C6FF02B010BDBF +:1075100010B56424013C4FF47A70FFF7AFF814F013 +:10752000FF04F7D10A4B102214249A614FF48062B1 +:107530009A61A3F580534FF400529A61013C4FF4D5 +:107540007A70FFF79BF814F0FF04F7D110BD00BF6D +:10755000001802580144BFF34F8F064B884204D3F2 +:10756000BFF34F8FBFF36F8F7047C3F85C022030BB +:10757000F4E700BF00ED00E00144BFF34F8F064B7E +:10758000884204D3BFF34F8FBFF36F8F7047C3F8A8 +:1075900070022030F4E700BF00ED00E070B5054652 +:1075A00016460C4601201021FFF790FE2860467316 +:1075B0003CB1204636B1FFF785FE2B68186000B15C +:1075C0009C6070BDFFF74AFEF7E70000F8B50F4674 +:1075D0001546044648B905F11F010126386821F017 +:1075E0001F01FFF7B7FF3046F8BD427B29463868D8 +:1075F000FFF786FE06460028EDD13B686360A3686E +:10760000AB4210D213B12068FFF764FE637B2846BB +:107610002BB1FFF757FE206020B9A060E3E7FFF72A +:107620001DFEF8E7A560206805F11F01012621F085 +:107630001F013860FFF78EFF2673D4E710B50446AC +:1076400040B10068884205D1606808B1FAF760FB74 +:107650000023237310BD0000F8B50F4614460546FD +:1076600048B904F11F010126386821F01F01FFF716 +:1076700083FF3046F8BD427B21463868FFF740FE65 +:1076800006460028EDD1AB68A34210D213B128689A +:10769000FFF720FE6B7B20462BB1FFF713FE28601F +:1076A00020B9A860E5E7FFF7D9FDF8E7AC603968D5 +:1076B00019B122462868FAF72BFB286804F11F014C +:1076C000012621F01F013860FFF756FF2E73D0E727 +:1076D00020B103688B4204BF00230373704700008E +:1076E000034B1A681AB9034AD2F8D0241A607047BB +:1076F000386500200040025808B5FFF7F1FF024B43 +:107700001868C0F3806008BD38650020EFF3098376 +:10771000054968334A6B22F001024A6383F3098802 +:10772000002383F31188704700EF00E0302080F3DE +:10773000118862B60D4B0E4AD96821F4E061090444 +:10774000090C0A430B49DA60D3F8FC2042F080723E +:10775000C3F8FC20084AC2F8B01F116841F00101CB +:1077600011602022DA7783F82200704700ED00E0F4 +:107770000003FA0555CEACC5001000E0302310B56B +:1077800083F311880E4B5B6813F4006314D0F1EEA1 +:10779000103AEFF309844FF08073683CE361094BC2 +:1077A000DB6B236684F30988FEF7B2FE10B1064B4B +:1077B000A36110BD054BFBE783F31188F9E700BF18 +:1077C00000ED00E000EF00E02F04000832040008A4 +:1077D00070B5BFF34F8FBFF36F8F1A4A0021C2F805 +:1077E0005012BFF34F8FBFF36F8F536943F40033D1 +:1077F0005361BFF34F8FBFF36F8FC2F88410BFF395 +:107800004F8FD2F8803043F6E074C3F3C900C3F35E +:107810004E335B0103EA0406014646EA81750139ED +:10782000C2F86052F9D2203B13F1200FF2D1BFF31E +:107830004F8F536943F480335361BFF34F8FBFF3CE +:107840006F8F70BD00ED00E0FEE70000214B224885 +:10785000224A70B5904237D3214BC11EDA1C121A4E +:1078600022F003028B4238BF00220021FAF776FA99 +:107870001C4A0023C2F88430BFF34F8FD2F8803007 +:1078800043F6E074C3F3C900C3F34E335B0103EA6C +:107890000406014646EA81750139C2F86C52F9D2F4 +:1078A000203B13F1200FF2D1BFF34F8FBFF36F8F47 +:1078B000BFF34F8FBFF36F8F0023C2F85032BFF377 +:1078C0004F8FBFF36F8F70BD53F8041B40F8041B3C +:1078D000C0E700BF2C9D00083467002034670020FB +:1078E0003467002000ED00E0074BD3F8D81021EA00 +:1078F0000001C3F8D810D3F8002122EA0002C3F82F +:107900000021D3F8003170470044025870B5D0E927 +:10791000244300224FF0FF359E6804EB42135101CF +:10792000D3F80009002805DAD3F8000940F08040B8 +:10793000C3F80009D3F8000B002805DAD3F8000BD0 +:1079400040F08040C3F8000B013263189642C3F840 +:107950000859C3F8085BE0D24FF00113C4F81C3893 +:1079600070BD0000890141F02001016103699B069F +:10797000FCD41220FEF716BE10B50A4C2046FEF7C6 +:10798000B1FA094BC4F89030084BC4F89430084C55 +:107990002046FEF7A7FA074BC4F89030064BC4F810 +:1079A000943010BD3C65002000000840609C000839 +:1079B000D8650020000004406C9C000870B5037876 +:1079C0000546012B5CD1434BD0F89040984258D1EA +:1079D000414B0E216520D3F8D82042F00062C3F855 +:1079E000D820D3F8002142F00062C3F80021D3F878 +:1079F0000021D3F8802042F00062C3F88020D3F841 +:107A0000802022F00062C3F88020D3F88030FDF798 +:107A1000ADFC324BE360324BC4F800380023D5F89C +:107A20009060C4F8003EC02323604FF40413A363A6 +:107A30003369002BFCDA01230C203361FEF7B2FD21 +:107A40003369DB07FCD41220FEF7ACFD3369002B51 +:107A5000FCDA00262846A660FFF758FF6B68C4F8DA +:107A60001068DB68C4F81468C4F81C6883BB1D4B3D +:107A7000A3614FF0FF336361A36843F00103A36088 +:107A800070BD194B9842C9D1134B4FF08060D3F8A9 +:107A9000D82042F00072C3F8D820D3F8002142F079 +:107AA0000072C3F80021D3F80021D3F8802042F0FF +:107AB0000072C3F88020D3F8802022F00072C3F84F +:107AC0008020D3F88030FFF70FFF0E214D209EE776 +:107AD000064BCDE73C6500200044025840140040AE +:107AE00003002002003C30C0D8650020083C30C0B4 +:107AF000F8B5D0F89040054600214FF000662046CA +:107B0000FFF730FFD5F8941000234FF001128F6873 +:107B10004FF0FF30C4F83438C4F81C2804EB43128B +:107B200001339F42C2F80069C2F8006BC2F808092D +:107B3000C2F8080BF2D20B68D5F89020C5F898303F +:107B4000636210231361166916F01006FBD1122030 +:107B5000FEF728FDD4F8003823F4FE63C4F800389B +:107B6000A36943F4402343F01003A3610923C4F83D +:107B70001038C4F814380B4BEB604FF0C043C4F816 +:107B8000103B094BC4F8003BC4F81069C4F8003935 +:107B9000D5F8983003F1100243F48013C5F898200B +:107BA000A362F8BD3C9C000840800010D0F89020F3 +:107BB00090F88A10D2F8003823F4FE6343EA0113E8 +:107BC000C2F80038704700002DE9F84300EB81034C +:107BD000D0F890500C468046DA680FFA81F94801D7 +:107BE000166806F00306731E022B05EB41134FF0D7 +:107BF000000194BFB604384EC3F8101B4FF00101CA +:107C000004F1100398BF06F1805601FA03F391695D +:107C100098BF06F5004600293AD0578A04F158016A +:107C2000374349016F50D5F81C180B430021C5F8A4 +:107C30001C382B180127C3F81019A7405369611E7F +:107C40009BB3138A928B9B08012A88BF5343D8F8B1 +:107C50009820981842EA034301F140022146C8F8EF +:107C60009800284605EB82025360FFF77BFE08EB85 +:107C70008900C3681B8A43EA845348341E43640165 +:107C80002E51D5F81C381F43C5F81C78BDE8F88381 +:107C900005EB4917D7F8001B21F40041C7F8001B7A +:107CA000D5F81C1821EA0303C0E704F13F030B4A8F +:107CB0002846214605EB83035A60FFF753FE05EB88 +:107CC0004910D0F8003923F40043C0F80039D5F842 +:107CD0001C3823EA0707D7E70080001000040002E1 +:107CE000D0F894201268C0F89820FFF70FBE00006B +:107CF0005831D0F8903049015B5813F4004004D05B +:107D000013F4001F0CBF0220012070474831D0F847 +:107D1000903049015B5813F4004004D013F4001F65 +:107D20000CBF02200120704700EB8101CB68196A6B +:107D30000B6813604B6853607047000000EB8103D1 +:107D400030B5DD68AA691368D36019B9402B84BFC8 +:107D5000402313606B8A1468D0F890201C4402EB17 +:107D60004110013C09B2B4FBF3F46343033323F045 +:107D7000030343EAC44343F0C043C0F8103B2B68FD +:107D800003F00303012B0ED1D2F8083802EB4110A7 +:107D900013F4807FD0F8003B14BF43F0805343F0CE +:107DA0000053C0F8003B02EB4112D2F8003B43F015 +:107DB0000443C2F8003B30BD2DE9F041D0F890609B +:107DC00005460C4606EB4113D3F8087B3A07C3F887 +:107DD000087B08D5D6F814381B0704D500EB8103BF +:107DE000DB685B689847FA071FD5D6F81438DB07BD +:107DF0001BD505EB8403D968CCB98B69488A5A68CE +:107E0000B2FBF0F600FB16228AB91868DA689042D5 +:107E10000DD2121AC3E90024302383F311882146BE +:107E20002846FFF78BFF84F31188BDE8F08101231A +:107E300003FA04F26B8923EA02036B81CB68002BFF +:107E4000F3D021462846BDE8F041184700EB8103F6 +:107E50004A0170B5DD68D0F890306C692668E6603C +:107E600056BB1A444FF40020C2F810092A6802F0E9 +:107E70000302012A0AB20ED1D3F8080803EB421418 +:107E800010F4807FD4F8000914BF40F0805040F017 +:107E90000050C4F8000903EB4212D2F8000940F088 +:107EA0000440C2F800090122D3F8340802FA01F1B3 +:107EB0000143C3F8341870BD19B9402E84BF402067 +:107EC000206020681A442E8A8419013CB4FBF6F421 +:107ED00040EAC44040F00050C6E700002DE9F843F6 +:107EE000D0F8906005460C464F0106EB4113D3F8DD +:107EF000088918F0010FC3F808891CD0D6F810388B +:107F0000DB0718D500EB8103D3F80CC0DCF8143084 +:107F1000D3F800E0DA68964530D2A2EB0E024FF0BB +:107F200000091A60C3F80490302383F31188FFF727 +:107F30008DFF89F3118818F0800F1DD0D6F83438E2 +:107F40000126A640334217D005EB84030134D5F84F +:107F50009050D3F80CC0E4B22F44DCF8142005EBA9 +:107F60000434D2F800E05168714514D3D5F83438A0 +:107F700023EA0606C5F83468BDE8F883012303FA4E +:107F800001F2038923EA02030381DCF80830002BA5 +:107F9000D1D09847CFE7AEEB0103BCF81000834285 +:107FA00028BF0346D7F8180980B2B3EB800FE3D897 +:107FB0009068A0F1040959F8048FC4F80080A0EB80 +:107FC00009089844B8F1040FF5D818440B449060A0 +:107FD0005360C8E72DE9F84FD0F8905004466E6919 +:107FE000AB691E4016F480586E6103D0BDE8F84FAF +:107FF000FDF7E8BF002E12DAD5F8003E9B0705D04A +:10800000D5F8003E23F00303C5F8003ED5F8043848 +:10801000204623F00103C5F80438FEF701F83705C0 +:1080200005D52046FFF772FC2046FDF7E7FFB004B8 +:108030000CD5D5F8083813F0060FEB6823F470530D +:108040000CBF43F4105343F4A053EB6031071BD52E +:108050006368DB681BB9AB6923F00803AB61237865 +:10806000052B0CD1D5F8003E9A0705D0D5F8003E77 +:1080700023F00303C5F8003E2046FDF7D1FF6368F7 +:10808000DB680BB120469847F30200F1BA80B702D3 +:1080900026D5D4F8909000274FF0010A09EB47123B +:1080A000D2F8003B03F44023B3F5802F11D1D2F86E +:1080B000003B002B0DDA62890AFA07F322EA030378 +:1080C000638104EB8703DB68DB6813B13946204624 +:1080D00098470137D4F89430FFB29B689F42DDD9AE +:1080E000F00619D5D4F89000026AC2F30A1702F01C +:1080F0000F0302F4F012B2F5802F00F0CA80B2F53F +:10810000402F09D104EB8303002200F58050DB6887 +:108110001B6A974240F0B0803003D5F8185835D527 +:10812000E90303D500212046FFF746FEAA0303D545 +:1081300001212046FFF740FE6B0303D502212046B4 +:10814000FFF73AFE2F0303D503212046FFF734FE45 +:10815000E80203D504212046FFF72EFEA90203D52D +:1081600005212046FFF728FE6A0203D50621204696 +:10817000FFF722FE2B0203D507212046FFF71CFE46 +:10818000EF0103D508212046FFF716FE700340F1EA +:10819000A780E90703D500212046FFF79FFEAA0725 +:1081A00003D501212046FFF799FE6B0703D5022175 +:1081B0002046FFF793FE2F0703D503212046FFF744 +:1081C0008DFEEE0603D504212046FFF787FEA806A4 +:1081D00003D505212046FFF781FE690603D5062158 +:1081E0002046FFF77BFE2A0603D507212046FFF72E +:1081F00075FEEB0574D520460821BDE8F84FFFF762 +:108200006DBED4F890904FF0000B4FF0010AD4F8F7 +:1082100094305FFA8BF79B689F423FF638AF09EBCB +:108220004713D3F8002902F44022B2F5802F20D161 +:10823000D3F80029002A1CDAD3F8002942F0904232 +:10824000C3F80029D3F80029002AFBDB3946D4F80B +:108250009000FFF787FB22890AFA07F322EA03035B +:10826000238104EB8703DB689B6813B13946204602 +:1082700098470BF1010BCAE7910701D1D0F80080B4 +:10828000072A02F101029CBF03F8018B4FEA18286C +:108290003FE704EB830300F58050DA68D2F818C09A +:1082A000DCF80820DCE9001CA1EB0C0C00218F425B +:1082B00008D1DB689B699A683A449A605A683A44E4 +:1082C0005A6029E711F0030F01D1D0F800808C45E6 +:1082D00001F1010184BF02F8018B4FEA1828E6E79B +:1082E000BDE8F88F08B50348FFF774FEBDE8084005 +:1082F000FFF744BA3C65002008B50348FFF76AFE63 +:10830000BDE80840FFF73ABAD8650020D0F89030B1 +:1083100003EB4111D1F8003B43F40013C1F8003BDB +:1083200070470000D0F8903003EB4111D1F80039CC +:1083300043F40013C1F8003970470000D0F89030C2 +:1083400003EB4111D1F8003B23F40013C1F8003BCB +:1083500070470000D0F8903003EB4111D1F800399C +:1083600023F40013C1F8003970470000064BD3F81E +:10837000DC200243C3F8DC20D3F804211043C3F807 +:108380000401D3F8043170470044025808B53C4B4F +:108390004FF0FF31D3F8802062F00042C3F8802014 +:1083A000D3F8802002F00042C3F88020D3F8802068 +:1083B000D3F88420C3F88410D3F884200022C3F8B3 +:1083C0008420D3F88400D86F40F0FF4040F4FF00D1 +:1083D00040F4DF4040F07F00D867D86F20F0FF40C6 +:1083E00020F4FF0020F4DF4020F07F00D867D86F32 +:1083F000D3F888006FEA40506FEA5050C3F8880005 +:10840000D3F88800C0F30A00C3F88800D3F88800C6 +:10841000D3F89000C3F89010D3F89000C3F89020E0 +:10842000D3F89000D3F89400C3F89410D3F89400D4 +:10843000C3F89420D3F89400D3F89800C3F89810A8 +:10844000D3F89800C3F89820D3F89800D3F88C009C +:10845000C3F88C10D3F88C00C3F88C20D3F88C00B0 +:10846000D3F89C00C3F89C10D3F89C10C3F89C2050 +:10847000D3F89C30FCF728FABDE8084000F0D6B9E4 +:108480000044025808B50122534BC3F80821534B4E +:10849000D3F8F42042F00202C3F8F420D3F81C21F0 +:1084A00042F00202C3F81C210222D3F81C314C4BCB +:1084B000DA605A689104FCD54A4A1A6001229A602F +:1084C000494ADA6000221A614FF440429A61444BF3 +:1084D0009A699204FCD51A6842F480721A603F4B84 +:1084E0001A6F12F4407F04D04FF480321A670022D2 +:1084F0001A671A6842F001021A60384B1A6850076E +:10850000FCD500221A611A6912F03802FBD1012150 +:1085100019604FF0804159605A67344ADA62344A30 +:108520001A611A6842F480321A602C4B1A6891035F +:10853000FCD51A6842F480521A601A689204FCD57D +:108540002C4A2D499A6200225A63196301F57C0175 +:10855000DA6301F2E71199635A64284A1A64284AD7 +:10856000DA621A6842F0A8521A601C4B1A6802F0CC +:108570002852B2F1285FF9D148229A614FF48862FB +:10858000DA6140221A621F4ADA641F4A1A651F4ADA +:108590005A651F4A9A6532231E4A1360136803F016 +:1085A0000F03022BFAD10D4A136943F00303136141 +:1085B000136903F03803182BFAD14FF00050FFF77E +:1085C000D5FE4FF08040FFF7D1FE4FF00040BDE8F0 +:1085D0000840FFF7CBBE00BF0080005100440258A6 +:1085E0000048025800C000F0020000010000FF0136 +:1085F000008890082220400063020901470E050808 +:10860000DD0BBF0120000020000001100910E00078 +:1086100000010110002000524FF0B04208B5D2F81E +:10862000883003F00103C2F8883023B1044A13688C +:108630000BB150689847BDE80840FFF79FB800BFEE +:10864000B46600204FF0B04208B5D2F8883003F08D +:108650000203C2F8883023B1044A93680BB1D06892 +:108660009847BDE80840FFF789B800BFB46600200E +:108670004FF0B04208B5D2F8883003F00403C2F8D6 +:10868000883023B1044A13690BB150699847BDE89B +:108690000840FFF773B800BFB46600204FF0B04247 +:1086A00008B5D2F8883003F00803C2F8883023B147 +:1086B000044A93690BB1D0699847BDE80840FFF7B9 +:1086C0005DB800BFB46600204FF0B04208B5D2F8E4 +:1086D000883003F01003C2F8883023B1044A136ACB +:1086E0000BB1506A9847BDE80840FFF747B800BF94 +:1086F000B46600204FF0B04310B5D3F8884004F4BE +:108700007872C3F88820A30604D5124A936A0BB185 +:10871000D06A9847600604D50E4A136B0BB1506BB4 +:108720009847210604D50B4A936B0BB1D06B984741 +:10873000E20504D5074A136C0BB1506C9847A305AA +:1087400004D5044A936C0BB1D06C9847BDE8104037 +:10875000FFF714B8B46600204FF0B04310B5D3F85B +:10876000884004F47C42C3F88820620504D5164A88 +:10877000136D0BB1506D9847230504D5124A936DC4 +:108780000BB1D06D9847E00404D50F4A136E0BB1BE +:10879000506E9847A10404D50B4A936E0BB1D06E6E +:1087A0009847620404D5084A136F0BB1506F98477D +:1087B000230404D5044A936F0BB1D06F9847BDE8EA +:1087C0001040FEF7DBBF00BFB466002008B50348C9 +:1087D000FCF742FCBDE80840FEF7D0BFC8360020D9 +:1087E00008B50348FCF73CFDBDE80840FEF7C6BFEE +:1087F0002039002008B50348FCF732FDBDE80840E9 +:10880000FEF7BCBF8C39002008B50348FCF728FDF3 +:10881000BDE80840FEF7B2BFF839002008B500F007 +:10882000BDFCBDE80840FEF7A9BF0000062108B561 +:108830000846FCF79BFD06210720FCF797FD062163 +:108840000820FCF793FD06210920FCF78FFD062187 +:108850000A20FCF78BFD06211720FCF787FD062177 +:108860002820FCF783FD09217A20FCF77FFD0921F0 +:108870007C20FCF77BFD07213220FCF777FD0C21E3 +:108880002720FCF773FD0C213520FCF76FFD0C2130 +:108890005220BDE80840FCF769BD000008B5FFF7AD +:1088A00075FD00F043FCFDF741F9FDF7DFF8FDF73A +:1088B00017FBFDF7E9F9FEF7ABF9BDE8084000F05A +:1088C00029BA000030B50433039C0172002104FB77 +:1088D0000325C160C0E90653049B0363059BC0E9FF +:1088E0000000C0E90422C0E90842C0E90A1143635C +:1088F00030BD00000022416AC260C0E90411C0E935 +:108900000A226FF00101FDF7F9BE0000D0E9043240 +:10891000934201D1C2680AB9181D70470020704700 +:10892000036919600021C2680132C260C269134440 +:1089300082699342036124BF436A0361FDF7D2BE9B +:1089400038B504460D46E3683BB162690020131D4B +:108950001268A3621344E36207E0237A33B929461D +:108960002046FDF7AFFE0028EDDA38BD6FF00100BC +:10897000FBE70000C368C269013BC360436913445D +:1089800082699342436124BF436A43610023836247 +:10899000036B03B11847704770B53023044683F367 +:1089A0001188866A3EB9FFF7CBFF054618B186F3FA +:1089B0001188284670BDA36AE26A13F8015B9342EE +:1089C000A36202D32046FFF7D5FF002383F311886B +:1089D000EFE700002DE9F84F04460E46174698468B +:1089E0004FF0300989F311880025AA46D4F828B041 +:1089F000BBF1000F09D141462046FFF7A1FF20B18E +:108A00008BF311882846BDE8F88FD4E90A12A7EB4A +:108A1000050B521A934528BF9346BBF1400F1BD953 +:108A2000334601F1400251F8040B914243F8040B24 +:108A3000F9D1A36A403640354033A362D4E90A2312 +:108A40009A4202D32046FFF795FF8AF31188BD4270 +:108A5000D8D289F31188C9E730465A46F9F758F950 +:108A6000A36A5E445D445B44A362E7E710B5029CE1 +:108A70000433017203FB0421C460C0E90613002320 +:108A8000C0E90A33039B0363049BC0E90000C0E90B +:108A90000422C0E90842436310BD0000026A6FF07F +:108AA0000101C260426AC0E904220022C0E90A2230 +:108AB000FDF724BED0E904239A4201D1C26822B94D +:108AC000184650F8043B0B60704700231846FAE73D +:108AD000C3680021C2690133C360436913448269DA +:108AE0009342436124BF436A4361FDF7FBBD00002D +:108AF00038B504460D46E3683BB1236900201A1DD2 +:108B0000A262E2691344E36207E0237A33B929469B +:108B10002046FDF7D7FD0028EDDA38BD6FF00100E3 +:108B2000FBE7000003691960C268013AC260C269CC +:108B3000134482699342036124BF436A03610023A3 +:108B40008362036B03B118477047000070B5302390 +:108B50000D460446114683F31188866A2EB9FFF745 +:108B6000C7FF10B186F3118870BDA36A1D70A36A98 +:108B7000E26A01339342A36204D3E16920460439D7 +:108B8000FFF7D0FF002080F31188EDE72DE9F84FC3 +:108B900004460D46904699464FF0300A8AF31188F4 +:108BA0000026B346A76A4FB949462046FFF7A0FF03 +:108BB00020B187F311883046BDE8F88FD4E90A0761 +:108BC0003A1AA8EB0607974228BF1746402F1BD931 +:108BD00005F1400355F8042B9D4240F8042BF9D1D0 +:108BE000A36A40364033A362D4E90A239A4204D3ED +:108BF000E16920460439FFF795FF8BF3118846455C +:108C0000D9D28AF31188CDE729463A46F9F780F898 +:108C1000A36A3D443E443B44A362E5E7D0E9042314 +:108C20009A4217D1C3689BB1836A8BB1043B9B1AEC +:108C30000ED01360C368013BC360C3691A448369E3 +:108C40009A42026124BF436A0361002383620123C5 +:108C5000184670470023FBE701F01F03F0B502F050 +:108C60001F0456095A1C0123B6EB511F50F8265019 +:108C700003FA02F34FEA511703F1FF333DBF50F8F7 +:108C80002720C4F12000134003EA05003BBF03FA8C +:108C900000F225FA04F0E0401043F0BD70B57E22EA +:108CA0007F210546FFF7D8FF18B1012819D0002011 +:108CB00070BD3E2249212846FFF7CEFF2F220446F1 +:108CC00031212846FFF7C8FF0646013450220236FC +:108CD00053212846B440FFF7BFFF093804FA00F0DB +:108CE000E6E7302245212846FFF7B6FF0130800233 +:108CF000DEE7000090F8D63090F8D7201B0403EB95 +:108D0000026390F8D42090F8D500134403EB0020C0 +:108D10007047000000F018BA014B586A704700BF56 +:108D2000000C0040034B002258631A610222DA60F3 +:108D3000704700BF000C0040014B0022DA60704712 +:108D4000000C0040014B5863704700BF000C00400E +:108D5000024B034A1A60034A5A6070478C6600202F +:108D60003867002000000220074B494210B55C68BC +:108D7000201A08401968821A8A4203D3A24201D8F5 +:108D80005A6010BD0020FCE78C66002008B5302337 +:108D900083F31188FFF7E8FF002383F3118808BDF0 +:108DA0000448054B03600023C0E901330C3000F098 +:108DB00017B900BF946600208D8D0008CB1D083ABE +:108DC00023F00703591A521A10B4D2080024C0E93C +:108DD000004384600C301C605A605DF8044B00F066 +:108DE000FFB800002DE9F74F364FCD1D884600280B +:108DF00018BF0746082A4FEAD50538BF082207F1F1 +:108E00000C003C1D9146019000F02CF9019809F1ED +:108E10000701C9F1000E2246246864B900F02CF95C +:108E20003B68CBB308224946E8009847044698B30C +:108E300040E9027830E004EB010CD4F804A00CEA1D +:108E40000E0C0AF10106ACF1080304EBC6069E42C3 +:108E5000E1D9A6EB0C0CB5EBEC0F4FEAEC0BDAD832 +:108E60009C421DD204F10802AB45A3EB02024FEA7B +:108E7000E202626009D9691CED4303EBC1025D4463 +:108E80005560256843F8315022601C46C3F80480C1 +:108E900044F8087B00F0F0F8204603B0BDE8F08FFE +:108EA000AA45216802D111602346EEE7013504EBA3 +:108EB000C50344F8351003F10801761AF6105E6018 +:108EC0001360F1E79466002073B50446A0F108052D +:108ED00050F8080C54F8043C061D0C300733019080 +:108EE000DB0844F8043C00F0BDF8334601989E428C +:108EF0001A6801D0AB4228D20AB1954225D244F873 +:108F0000082C54F8042C1D60013254F8081C05EBA1 +:108F1000C206B14206D14E68324444F8042C0A68B5 +:108F200044F8082C5E68711C03EBC1018D4207D127 +:108F300054F8042C013232445A6054F8082C1A6058 +:108F400002B0BDE8704000F097B81346CFE70000CC +:108F5000FEE7000070B51E4B0025044686B0586041 +:108F60000E4605638163FEF7E1FB04F12803A5606B +:108F70006563C4E90A3304F11003C4E904334FF014 +:108F8000FF33C4E90044C4E90635FFF7C5FE2B46AC +:108F9000024604F13C012046C4E9082380230D4A1F +:108FA0006567FDF709FB7368E0600B4A0362012304 +:108FB000009280F824306846F26801923269CDE967 +:108FC0000223064BCDE90435FDF72AFB06B070BD40 +:108FD00010490020809C0008789C0008518F0008F0 +:108FE0000023C0E9000083600361704770B51C4B2B +:108FF00005468468DE685CB3B44213D1036901336B +:10900000036170BDA36094F8243083B1062B15D1A1 +:10901000A06A2146D4E9003213605A60FDF738FA9D +:10902000A36A9C68B368A2689A42EBD306E0D4E9CD +:109030000032204613605A60FDF73AFA284631465E +:10904000FDF726FAB5620620BDE87040FDF732BA9A +:109050000369866001330361336BC3603063D0E71B +:109060006846002008B5302383F31188FFF7BEFF60 +:10907000002383F3118808BD194BD96883688B429C +:1090800010B520D1302383F311880269013A0261BF +:10909000B2B90468C368A0420B631ED04A6B9BB987 +:1090A00001238A60036103681A68026050601A6BCA +:1090B0008360C26018631846FDF7FAF9FDF74AFAB3 +:1090C000002383F3118810BD1C68A34203D0A46859 +:1090D000A24238BF2246DB68E1E78260F0E700BFCA +:1090E00068460020024A536B18435063704700BF24 +:1090F0006846002038B5EFF311859DB9EFF305847C +:10910000C4F30804302334B183F31188FDF72EFC37 +:1091100085F3118838BD83F31188FDF727FC84F3AC +:10912000118838BDBDE83840FDF720BC0023054A52 +:1091300019460133102BC2E9001102F10802F8D1DF +:10914000704700BFB4660020114BD3F8E82042F00E +:109150000802C3F8E820D3F8102142F00802C3F84F +:1091600010210C4AD3F81031D36B43F00803D363BA +:10917000C722094B9A624FF0FF32DA6200229A61ED +:109180005A63DA605A6001225A611A60704700BF60 +:10919000004402580010005C000C0040094A08B569 +:1091A0001169D3680B40D9B29B076FEA01011161C5 +:1091B00007D5302383F31188FDF7F2F9002383F3F9 +:1091C000118808BD000C004010B5013902449042DE +:1091D00001D1002005E0037811F8014FA34201D02E +:1091E000181B10BD0130F2E7884210B501EB0204F4 +:1091F00002D98442234607D8431EA14208D011F861 +:10920000012B03F8012FF8E7024401468A4200D1FE +:1092100010BD13F8014D02F8014DF7E7C9B203463E +:1092200010F8012B1AB18A42F9D11846704700296B +:1092300018BF0023F9E70000034611F8012B03F8DB +:10924000012B002AF9D1704710B50139034632B11C +:1092500011F8014F03F8014B013A002CF7D11A44E1 +:109260000021934200D110BD03F8011BF9E7000073 +:109270004D4435002D2D0A002F6172647570696CA4 +:109280006F742E6162696E002F6172647570696C13 +:109290006F742D7665726966792E6162696E002F32 +:1092A0006172647570696C6F742D666C6173682E81 +:1092B0006162696E002F6172647570696C6F742DE4 +:1092C000666C61736865642E6162696E00000000FF +:1092D0000000000000000000050F0008A10F0008BA +:1092E00051110008D90F0008990F00080000000074 +:1092F00000000000010F0008AD0F000889110008F0 +:10930000FD0E0008090F000853544D333248373F13 +:109310003F3F0053544D3332483733782F3732783C +:109320000053544D3332483734332F3735332F37CA +:109330003530000001105A00031059000120580078 +:10934000032056002F000000537563636573736636 +:10935000756C6C79206D6F756E7465642053444331 +:109360006172642028736C6F77646F776E3D25752A +:10937000290A0000EB7690455846415420202000F1 +:109380004641543332202020000000002A3A3C3E5F +:109390007C223F7F002B2C3B3D5B5D0043554541CC +:1093A0004141414345454549494941414592924FD3 +:1093B0004F4F5555594F554F9C4F9E9F41494F55C3 +:1093C000A5A5A6A7A8A9AAABACADAEAFB0B1B2B3E4 +:1093D000B4414141B8B9BABBBCBDBEBFC0C1C2C334 +:1093E000C4C54141C8C9CACBCCCDCECFD1D14545EA +:1093F0004549494949D9DADBDCDD49DF4FE14F4FC7 +:109400004F4FE6E8E85555555959EEEFF0F1F2F3B4 +:10941000F4F5F6F7F8F9FAFBFCFDFEFF010305078A +:10942000090E10121416181C1E00000061001A0309 +:10943000E0001703F8000703FF00010078010001B6 +:10944000300132010601390110014A012E01790172 +:10945000060180014D0043028101820182018401E5 +:10946000840186018701870189018A018B018B01B3 +:109470008D018E018F019001910191019301940161 +:10948000F60196019701980198013D029B019C010C +:109490009D0120029F01A001A001A201A201A4013F +:1094A000A401A601A701A701A901AA01AB01AC0172 +:1094B000AC01AE01AF01AF01B101B201B301B30123 +:1094C000B501B501B701B801B801BA01BB01BC01D2 +:1094D000BC01BE01F701C001C101C201C301C40149 +:1094E000C501C401C701C801C701CA01CB01CA0136 +:1094F000CD011001DD0101008E01DE011201F30139 +:109500000300F101F401F401F80128012202120123 +:109510003A020900652C3B023B023D02662C3F02E9 +:1095200040024102410246020A0153024000810109 +:109530008601550289018A0158028F015A02900161 +:109540005C025D025E025F029301610262029401AD +:109550006402650266026702970196016A02622C44 +:109560006C026D026E029C01700271029D01730219 +:1095700074029F0176027702780279027A027B02F6 +:109580007C02642C7E027F02A60181028202A90174 +:109590008402850286028702AE014402B101B20153 +:1095A00045028D028E028F0290029102B7017B0369 +:1095B0000300FD03FE03FF03AC03040086038803DE +:1095C00089038A03B1031103C2030200A303A303A7 +:1095D000C4030803CC0303008C038E038F03D8035A +:1095E0001801F2030A00F903F303F403F503F60389 +:1095F000F703F703F903FA03FA03300420035004D6 +:109600001007600422018A043601C1040E01CF0450 +:109610000100C004D00444016105260400000000DC +:109620007D1D0100632C001E9601A01E5A01001F23 +:109630000806101F0606201F0806301F0806401FD8 +:109640000606511F0700591F521F5B1F541F5D1F45 +:10965000561F5F1F601F0806701F0E00BA1FBB1F3A +:10966000C81FC91FCA1FCB1FDA1FDB1FF81FF91F36 +:10967000EA1FEB1FFA1FFB1F801F0806901F08063A +:10968000A01F0806B01F0400B81FB91FB21FBC1FDF +:10969000CC1F0100C31FD01F0206E01F0206E51FFA +:1096A0000100EC1FF31F0100FC1F4E2101003221BD +:1096B00070211002842101008321D0241A05302C4E +:1096C0002F04602C0201672C0601752C0201802CEE +:1096D0006401002D260841FF1A030000C700FC00AA +:1096E000E900E200E400E000E500E700EA00EB004A +:1096F000E800EF00EE00EC00C400C500C900E60081 +:10970000C600F400F600F200FB00F900FF00D600EE +:10971000DC00F800A300D800D7009201E100ED00C2 +:10972000F300FA00F100D100AA00BA00BF00AE00B9 +:10973000AC00BD00BC00A100AB00BB009125922590 +:10974000932502252425C100C200C000A90063257D +:10975000512557255D25A200A50010251425342587 +:109760002C251C2500253C25E300C3005A25542543 +:1097700069256625602550256C25A400F000D000E1 +:10978000CA00CB00C8003101CD00CE00CF001825A3 +:109790000C2588258425A600CC008025D300DF0079 +:1097A000D400D200F500D500B500FE00DE00DA00DE +:1097B000DB00D900FD00DD00AF00B400AD00B1005A +:1097C0001720BE00B600A700F700B800B000A80040 +:1097D000B700B900B300B200A025A00010000240FD +:1097E000080002400008024000000B002800024070 +:1097F000080002400408024006010C00400002403C +:10980000080002400808024010020D005800024003 +:10981000080002400C08024016030E0070000240CF +:109820000C0002401008024000040F0088000240B3 +:109830000C0002401408024006051000A00002407F +:109840000C0002401808024010061100B800024047 +:109850000C0002401C08024016072F0010040240B2 +:109860000804024020080240000838002804024092 +:10987000080402402408024006093900400402405E +:109880000804024028080240100A3A005804024026 +:10989000080402402C080240160B3B0070040240F2 +:1098A0000C04024030080240000C3C0088040240D6 +:1098B0000C04024034080240060D4400A00402409B +:1098C0000C04024038080240100E4500B804024063 +:1098D0000C0402403C080240160F46000100000044 +:1098E00000000000009600000000000000000000E2 +:1098F0000000000000000000000000000000000068 +:10990000096C00080D6C000801570008395A00085E +:1099100095560008BD560008E55600087D5600081B +:1099200000000000ED5A0008D95A0008155B000835 +:10993000015B00080D5B0008F95A0008E55A0008B1 +:10994000D15A0008215B000800000000055C0008F7 +:10995000F15B00082D5C0008195C0008255C00081C +:10996000115C0008FD5B0008E95B0008395C000839 +:109970000000000001000000000000006330000053 +:109980007C9900080000000000000000E046002074 +:10999000104900200000802A00020000AAAAAAAAFA +:1099A00000000024FFFF00000000000000A00A00EB +:1099B000000200A000000000AAAAAAAA000100500C +:1099C000FFFF0000000009000000009900000006F1 +:1099D00000000000AAAAAAAA00000001FFFF0000E0 +:1099E000000000000000080020A00A400000000065 +:1099F000AAAAAAAA10400540FFFF0000000800BB69 +:109A0000770000004045020000000000AAAAAAAAB0 +:109A100000000100C7FF0000000000000700000078 +:109A20000020000000000000AAAAAAAA001000005E +:109A3000FFFF000000000007000000001041900040 +:109A400000000000AAAAAAAA10404000FFFF0000E0 +:109A50000000000000A00000000400000000000062 +:109A6000AAAAAAAA00040000FFFF0000000000004C +:109A7000000000000000040000000000AAAAAAAA3A +:109A800000000400FFFF00000000000000000000D4 +:109A90000000000000000000AAAAAAAA000000001E +:109AA000FFFF0000000000000000000000000000B8 +:109AB00000000000AAAAAAAA00000000FFFF000000 +:109AC00000000000000000004172647550696C6F76 +:109AD000740025424F415244252D424C00255345E8 +:109AE0005249414C25000000020000000000000027 +:109AF000255E0008955E000840004000C06100201F +:109B0000D0610020020000000000000003000000FF +:109B100000000000DD5E00080000000010000000F2 +:109B2000E0610020000000000100000000000000D3 +:109B30003C65002001010200056D0008156C00085D +:109B4000B16C0008956C000843000000509B0008B1 +:109B500009024300020100C03209040000010202B0 +:109B60000100052400100105240100010424020263 +:109B70000524060001070582030800FF090401000F +:109B8000020A0000000705010240000007058102EB +:109B900040000000120000009C9B00081201100110 +:109BA00002000040091241570002010203010000B7 +:109BB0000403090425424F415244250043554156B0 +:109BC0002D372D4E616E6F003031323334353637DC +:109BD000383941424344454600000000000000205F +:109BE000000002000200000000000030000004003D +:109BF000080000000000002400000800040000002D +:109C00000004000000FC000002000000000004301E +:109C10000080000008000000000000380000010083 +:109C2000010000001F1C1F1E1F1E1F1F1E1F1E1FC6 +:109C30001F1D1F1E1F1E1F1F1E1F1E1F00000000B6 +:109C400039600008F16200089D6300084000400090 +:109C50007466002074660020010000008466002005 +:109C6000800000004001000008000000000100002A +:109C7000001000000800000069646C65000000002E +:109C80006D61696E002C0438040438080C10141C33 +:109C900020242526000000000000640401000400C8 +:109CA00000000000000C001028303400BC67FF7F6B +:109CB0000100000000000000581B00000000000030 +:109CC00000001E0000000000FF00000018490020F6 +:109CD000F83900208C3900202039002000000000D5 +:109CE0000893000883040000139300085004000048 +:109CF0002193000801000000000000000096000011 +:109D000000000800960000000008000004000000A9 +:109D1000B09B0008000000000000000000000000F0 +:0C9D200000000000000000000000000037 +:00000001FF diff --git a/Tools/bootloaders/2RAWH743_bl.bin b/Tools/bootloaders/IFLIGHT_2RAW_H7_bl.bin similarity index 100% rename from Tools/bootloaders/2RAWH743_bl.bin rename to Tools/bootloaders/IFLIGHT_2RAW_H7_bl.bin diff --git a/Tools/bootloaders/2RAWH743_bl.hex b/Tools/bootloaders/IFLIGHT_2RAW_H7_bl.hex similarity index 100% rename from Tools/bootloaders/2RAWH743_bl.hex rename to Tools/bootloaders/IFLIGHT_2RAW_H7_bl.hex diff --git a/Tools/environment_install/install-prereqs-ubuntu.sh b/Tools/environment_install/install-prereqs-ubuntu.sh index 76ea0c3098543c..5dece4504fb242 100755 --- a/Tools/environment_install/install-prereqs-ubuntu.sh +++ b/Tools/environment_install/install-prereqs-ubuntu.sh @@ -400,7 +400,11 @@ if [ -n "$PYTHON_VENV_PACKAGE" ]; then fi # try update packaging, setuptools and wheel before installing pip package that may need compilation -$PIP install $PIP_USER_ARGUMENT -U pip packaging setuptools wheel +SETUPTOOLS="setuptools" +if [ ${RELEASE_CODENAME} == 'focal' ]; then + SETUPTOOLS=setuptools==70.3.0 +fi +$PIP install $PIP_USER_ARGUMENT -U pip packaging $SETUPTOOLS wheel if [ "$GITHUB_ACTIONS" == "true" ]; then PIP_USER_ARGUMENT+=" --progress-bar off" @@ -414,7 +418,11 @@ if [ ${RELEASE_CODENAME} == 'bookworm' ] || $PIP install $PIP_USER_ARGUMENT -U attrdict3 fi -$PIP install $PIP_USER_ARGUMENT -U $PYTHON_PKGS +# install Python packages one-at-a-time so it is clear which package +# is causing problems: +for PACKAGE in $PYTHON_PKGS; do + $PIP install $PIP_USER_ARGUMENT -U $PACKAGE +done if [[ -z "${DO_AP_STM_ENV}" ]] && maybe_prompt_user "Install ArduPilot STM32 toolchain [N/y]?" ; then DO_AP_STM_ENV=1 diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index bb19e6b4c69bf2..aa53a8c10ad7da 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -73,6 +73,7 @@ def __init__(self, Feature('Telemetry', 'FrSky D', 'AP_FRSKY_D_TELEM_ENABLED', 'Enable FrSkyD Telemetry', 0, 'FrSky'), Feature('Telemetry', 'FrSky SPort', 'AP_FRSKY_SPORT_TELEM_ENABLED', 'Enable FrSkySPort Telemetry', 0, 'FrSky'), # noqa Feature('Telemetry', 'FrSky SPort PassThrough', 'AP_FRSKY_SPORT_PASSTHROUGH_ENABLED', 'Enable FrSkySPort PassThrough Telemetry', 0, 'FrSky SPort,FrSky'), # noqa + Feature('Telemetry', 'Bidirectional FrSky Telemetry', 'HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL', 'Enable Bidirectional FrSky telemetry', 0, 'FrSky SPort'), # noqa Feature('Telemetry', 'GHST', 'AP_GHST_TELEM_ENABLED', 'Enable Ghost Telemetry', 0, "RC_GHST"), # noqa Feature('Notify', 'PLAY_TUNE', 'AP_NOTIFY_MAVLINK_PLAY_TUNE_SUPPORT_ENABLED', 'Enable MAVLink Play Tune', 0, None), # noqa @@ -191,6 +192,9 @@ def __init__(self, Feature('Payload', 'SPRAYER', 'HAL_SPRAYER_ENABLED', 'Enable Sprayer', 0, None), Feature('Payload', 'LANDING_GEAR', 'AP_LANDINGGEAR_ENABLED', 'Enable Landing Gear', 0, None), Feature('Payload', 'WINCH', 'AP_WINCH_ENABLED', 'Enable Winch', 0, None), + Feature('Payload', 'WINCH_DAIWA', 'AP_WINCH_DAIWA_ENABLED', 'Enable DAIWA Winch support', 0, 'WINCH'), + Feature('Payload', 'WINCH_PWM', 'AP_WINCH_PWM_ENABLED', 'Enable PWM Winch support', 0, 'WINCH'), + Feature('Payload', 'RELAY', 'AP_RELAY_ENABLED', 'Enable Relay support', 0, None), Feature('Payload', 'SERVORELAY_EVENTS', 'AP_SERVORELAYEVENTS_ENABLED', 'Enable Servo/Relay Event support', 0, None), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index c1f3aa86b6cb5f..d1a3d8192dfa5a 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -121,6 +121,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_FRSKY_D_TELEM_ENABLED', 'AP_Frsky_D::send',), ('AP_FRSKY_SPORT_TELEM_ENABLED', 'AP_Frsky_SPort::send_sport_frame',), ('AP_FRSKY_SPORT_PASSTHROUGH_ENABLED', 'AP::frsky_passthrough_telem',), + ('HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL', 'AP_Frsky_SPort_Passthrough::set_telem_data'), ('MODE_{type}_ENABLED', r'Mode(?P.+)::init',), ('MODE_GUIDED_NOGPS_ENABLED', r'ModeGuidedNoGPS::init',), @@ -170,6 +171,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('HAL_SPRAYER_ENABLED', 'AC_Sprayer::AC_Sprayer',), ('AP_LANDINGGEAR_ENABLED', r'AP_LandingGear::init\b',), ('AP_WINCH_ENABLED', 'AP_Winch::AP_Winch',), + ('AP_WINCH_{type}_ENABLED', r'AP_Winch_(?P.*)::update\b',), ('AP_RELAY_ENABLED', 'AP_Relay::init',), ('AP_SERVORELAYEVENTS_ENABLED', 'AP_ServoRelayEvents::update_events',), diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 0465ece670ec68..14d3a1fc7f32d3 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -150,6 +150,27 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = { // @User: Standard AP_GROUPINFO("INPUT_TC", 20, AC_AttitudeControl, _input_tc, AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT), + // @Param: LAND_R_MULT + // @DisplayName: Landed roll gain multiplier + // @Description: Roll gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the roll axis. + // @Range: 0.25 1.0 + // @User: Advanced + AP_GROUPINFO("LAND_R_MULT", 21, AC_AttitudeControl, _land_roll_mult, 1.0), + + // @Param: LAND_P_MULT + // @DisplayName: Landed pitch gain multiplier + // @Description: Pitch gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the pitch axis. + // @Range: 0.25 1.0 + // @User: Advanced + AP_GROUPINFO("LAND_P_MULT", 22, AC_AttitudeControl, _land_pitch_mult, 1.0), + + // @Param: LAND_Y_MULT + // @DisplayName: Landed yaw gain multiplier + // @Description: Yaw gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the yaw axis. + // @Range: 0.25 1.0 + // @User: Advanced + AP_GROUPINFO("LAND_Y_MULT", 23, AC_AttitudeControl, _land_yaw_mult, 1.0), + AP_GROUPEND }; @@ -204,6 +225,25 @@ void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly() get_rate_yaw_pid().relax_integrator(0.0, _dt, AC_ATTITUDE_RATE_RELAX_TC); } +// Reduce attitude control gains while landed to stop ground resonance +void AC_AttitudeControl::landed_gain_reduction(bool landed) +{ + if (is_positive(_input_tc)) { + // use 2.0 x tc to match the response time to 86% commanded + const float spool_step = _dt / (2.0 * _input_tc); + if (landed) { + _landed_gain_ratio = MIN(1.0, _landed_gain_ratio + spool_step); + } else { + _landed_gain_ratio = MAX(0.0, _landed_gain_ratio - spool_step); + } + } else { + _landed_gain_ratio = landed ? 1.0 : 0.0; + } + Vector3f scale_mult = VECTORF_111 * (1.0 - _landed_gain_ratio) + Vector3f(_land_roll_mult, _land_pitch_mult, _land_yaw_mult) * _landed_gain_ratio; + set_PD_scale_mult(scale_mult); + set_angle_P_scale_mult(scale_mult); +} + // The attitude controller works around the concept of the desired attitude, target attitude // and measured attitude. The desired attitude is the attitude input into the attitude controller // that expresses where the higher level code would like the aircraft to move to. The target attitude is moved diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 1883546c5632d1..b3968282580d51 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -163,6 +163,9 @@ class AC_AttitudeControl { // reset rate controller I terms smoothly to zero in 0.5 seconds void reset_rate_controller_I_terms_smoothly(); + // Reduce attitude control gains while landed to stop ground resonance + void landed_gain_reduction(bool landed); + // Sets attitude target to vehicle attitude and sets all rates to zero // If reset_rate is false rates are not reset to allow the rate controllers to run void reset_target_and_rate(bool reset_rate = true); @@ -479,6 +482,11 @@ class AC_AttitudeControl { // rate controller input smoothing time constant AP_Float _input_tc; + // Controller gain multiplyer to be used when landed + AP_Float _land_roll_mult; + AP_Float _land_pitch_mult; + AP_Float _land_yaw_mult; + // Intersampling period in seconds float _dt; @@ -561,6 +569,9 @@ class AC_AttitudeControl { // PD scale used for last loop, used for logging Vector3f _pd_scale_used; + // ratio of normal gain to landed gain + float _landed_gain_ratio; + // References to external libraries const AP_AHRS_View& _ahrs; const AP_MultiCopter &_aparm; diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 6dac22cc742157..bf86386c69fd40 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -134,6 +134,9 @@ void AC_AutoTune::send_step_string() case UPDATE_GAINS: gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Updating Gains"); return; + case ABORT: + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Aborting Test"); + return; case TESTING: gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Testing"); return; @@ -340,13 +343,6 @@ void AC_AutoTune::control_attitude() step_start_time_ms = now; step_time_limit_ms = get_testing_step_timeout_ms(); // set gains to their to-be-tested values - twitch_first_iter = true; - test_rate_max = 0.0f; - test_rate_min = 0.0f; - test_angle_max = 0.0f; - test_angle_min = 0.0f; - rotation_rate_filt.reset(0.0f); - rate_max = 0.0f; load_gains(GAIN_TEST); } else { // when waiting for level we use the intra-test gains @@ -356,18 +352,15 @@ void AC_AutoTune::control_attitude() // Initialize test-specific variables switch (axis) { case ROLL: - angle_finish = target_angle_max_rp_cd(); start_rate = ToDeg(ahrs_view->get_gyro().x) * 100.0f; start_angle = ahrs_view->roll_sensor; break; case PITCH: - angle_finish = target_angle_max_rp_cd(); start_rate = ToDeg(ahrs_view->get_gyro().y) * 100.0f; start_angle = ahrs_view->pitch_sensor; break; case YAW: case YAW_D: - angle_finish = target_angle_max_y_cd(); start_rate = ToDeg(ahrs_view->get_gyro().z) * 100.0f; start_angle = ahrs_view->yaw_sensor; break; @@ -537,8 +530,11 @@ void AC_AutoTune::control_attitude() } } } + FALLTHROUGH; + case ABORT: if (axis == YAW || axis == YAW_D) { + // todo: check to make sure we need this attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs_view->yaw_sensor, false); } @@ -594,6 +590,7 @@ void AC_AutoTune::backup_gains_and_initialise() */ void AC_AutoTune::load_gains(enum GainType gain_type) { + // todo: add previous setting so gains are not loaded on each loop. switch (gain_type) { case GAIN_ORIGINAL: load_orig_gains(); diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index 510fd6c59fbb4b..68a8dcee507e75 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -198,6 +198,7 @@ class AC_AutoTune WAITING_FOR_LEVEL = 0, // autotune is waiting for vehicle to return to level before beginning the next twitch TESTING = 1, // autotune has begun a test and is watching the resulting vehicle movement UPDATE_GAINS = 2, // autotune has completed a test and is updating the gains based on the results + ABORT = 3 // load normal gains and return to WAITING_FOR_LEVEL }; // mini steps performed while in Tuning mode, Testing step @@ -259,24 +260,15 @@ class AC_AutoTune bool positive_direction; // false = tuning in negative direction (i.e. left for roll), true = positive direction (i.e. right for roll) StepType step; // see StepType for what steps are performed TuneType tune_type; // see TuneType - bool ignore_next; // true = ignore the next test bool twitch_first_iter; // true on first iteration of a twitch (used to signal we must step the attitude or rate target) uint8_t axes_completed; // bitmask of completed axes - float test_rate_min; // the minimum angular rate achieved during TESTING_RATE step-multi only - float test_rate_max; // the maximum angular rate achieved during TESTING_RATE step-multi only - float test_angle_min; // the minimum angle achieved during TESTING_ANGLE step-multi only - float test_angle_max; // the maximum angle achieved during TESTING_ANGLE step-multi only uint32_t step_start_time_ms; // start time of current tuning step (used for timeout checks) uint32_t step_time_limit_ms; // time limit of current autotune process uint32_t level_start_time_ms; // start time of waiting for level int8_t counter; // counter for tuning gains - float target_rate; // target rate-multi only - float target_angle; // target angle-multi only float start_angle; // start angle float start_rate; // start rate - parent and multi - float rate_max; // maximum rate variable - parent and multi float test_accel_max; // maximum acceleration variable - float angle_finish; // Angle that test is aborted- parent and multi float desired_yaw_cd; // yaw heading during tune - parent and Tradheli float step_scaler; // scaler to reduce maximum target step - parent and multi diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index d3ffdefe1acfb8..434f67671dcec3 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -60,15 +60,15 @@ #define AUTOTUNE_RP_MAX 2.0 // maximum Rate P value #define AUTOTUNE_SP_MAX 40.0 // maximum Stab P value #define AUTOTUNE_SP_MIN 0.5 // maximum Stab P value -#define AUTOTUNE_RP_ACCEL_MIN 4000.0 // Minimum acceleration for Roll and Pitch -#define AUTOTUNE_Y_ACCEL_MIN 1000.0 // Minimum acceleration for Yaw +#define AUTOTUNE_RP_ACCEL_MIN 4000.0 // Minimum acceleration for Roll and Pitch +#define AUTOTUNE_Y_ACCEL_MIN 1000.0 // Minimum acceleration for Yaw #define AUTOTUNE_Y_FILT_FREQ 10.0 // Autotune filter frequency when testing Yaw #define AUTOTUNE_D_UP_DOWN_MARGIN 0.2 // The margin below the target that we tune D in -#define AUTOTUNE_RD_BACKOFF 1.0 // Rate D gains are reduced to 50% of their maximum value discovered during tuning +#define AUTOTUNE_RD_BACKOFF 1.0 // Rate D gains are reduced to 50% of their maximum value discovered during tuning #define AUTOTUNE_RP_BACKOFF 1.0 // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning -#define AUTOTUNE_SP_BACKOFF 0.9f // Stab P gains are reduced to 90% of their maximum value discovered during tuning +#define AUTOTUNE_SP_BACKOFF 0.9 // Stab P gains are reduced to 90% of their maximum value discovered during tuning #define AUTOTUNE_ACCEL_RP_BACKOFF 1.0 // back off from maximum acceleration -#define AUTOTUNE_ACCEL_Y_BACKOFF 1.0 // back off from maximum acceleration +#define AUTOTUNE_ACCEL_Y_BACKOFF 1.0 // back off from maximum acceleration // roll and pitch axes #define AUTOTUNE_TARGET_RATE_RLLPIT_CDS 18000 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step @@ -573,7 +573,6 @@ void AC_AutoTune_Multi::twitching_test_rate(float angle, float rate, float rate_ // update min and max and test for end conditions void AC_AutoTune_Multi::twitching_abort_rate(float angle, float rate, float angle_max, float meas_rate_min, float angle_min) { - const uint32_t now = AP_HAL::millis(); if (angle >= angle_max) { if (is_equal(rate, meas_rate_min) || (angle_min > 0.95 * angle_max)) { // we have reached the angle limit before completing the measurement of maximum and minimum @@ -587,10 +586,7 @@ void AC_AutoTune_Multi::twitching_abort_rate(float angle, float rate, float angl LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED); } // ignore result and start test again - step = WAITING_FOR_LEVEL; - positive_direction = twitch_reverse_direction(); - step_start_time_ms = now; - level_start_time_ms = now; + step = ABORT; } else { step = UPDATE_GAINS; } @@ -653,11 +649,11 @@ void AC_AutoTune_Multi::twitching_test_angle(float angle, float rate, float angl } // twitching_measure_acceleration - measure rate of change of measurement -void AC_AutoTune_Multi::twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) const +void AC_AutoTune_Multi::twitching_measure_acceleration(float &accel_average, float rate, float rate_max) const { - if (rate_measurement_max < rate_measurement) { - rate_measurement_max = rate_measurement; - rate_of_change = (1000.0f*rate_measurement_max)/(AP_HAL::millis() - step_start_time_ms); + if (rate_max < rate) { + rate_max = rate; + accel_average = (1000.0 * rate_max) / (AP_HAL::millis() - step_start_time_ms); } } @@ -1189,6 +1185,7 @@ void AC_AutoTune_Multi::twitch_test_init() float target_max_rate; switch (axis) { case ROLL: { + angle_abort = target_angle_max_rp_cd(); target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler * AUTOTUNE_TARGET_RATE_RLLPIT_CDS); target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_roll()) * 100.0, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate); target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll()) * 100.0, target_angle_min_rp_cd(), target_angle_max_rp_cd()); @@ -1196,6 +1193,7 @@ void AC_AutoTune_Multi::twitch_test_init() break; } case PITCH: { + angle_abort = target_angle_max_rp_cd(); target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler * AUTOTUNE_TARGET_RATE_RLLPIT_CDS); target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch()) * 100.0, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate); target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch()) * 100.0, target_angle_min_rp_cd(), target_angle_max_rp_cd()); @@ -1204,6 +1202,7 @@ void AC_AutoTune_Multi::twitch_test_init() } case YAW: case YAW_D: { + angle_abort = target_angle_max_y_cd(); target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, step_scaler*AUTOTUNE_TARGET_RATE_YAW_CDS); target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw() * 0.75) * 100.0, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, target_max_rate); target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw() * 0.75) * 100.0, target_angle_min_y_cd(), target_angle_max_y_cd()); @@ -1217,11 +1216,16 @@ void AC_AutoTune_Multi::twitch_test_init() } if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { + // todo: consider if this should be done for other axis rotation_rate_filt.reset(start_rate); } else { - rotation_rate_filt.reset(0); + rotation_rate_filt.reset(0.0); } - + twitch_first_iter = true; + test_rate_max = 0.0; + test_rate_min = 0.0; + test_angle_max = 0.0; + test_angle_min = 0.0; } //run twitch test @@ -1312,18 +1316,18 @@ void AC_AutoTune_Multi::twitch_test_run(AxisType test_axis, const float dir_sign case RD_UP: case RD_DOWN: twitching_test_rate(lean_angle, rotation_rate, target_rate, test_rate_min, test_rate_max, test_angle_min); - twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max); - twitching_abort_rate(lean_angle, rotation_rate, angle_finish, test_rate_min, test_angle_min); + twitching_measure_acceleration(test_accel_max, rotation_rate, test_rate_max); + twitching_abort_rate(lean_angle, rotation_rate, angle_abort, test_rate_min, test_angle_min); break; case RP_UP: - twitching_test_rate(lean_angle, rotation_rate, target_rate*(1+0.5f*aggressiveness), test_rate_min, test_rate_max, test_angle_min); - twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max); - twitching_abort_rate(lean_angle, rotation_rate, angle_finish, test_rate_min, test_angle_min); + twitching_test_rate(lean_angle, rotation_rate, target_rate * (1 + 0.5 * aggressiveness), test_rate_min, test_rate_max, test_angle_min); + twitching_measure_acceleration(test_accel_max, rotation_rate, test_rate_max); + twitching_abort_rate(lean_angle, rotation_rate, angle_abort, test_rate_min, test_angle_min); break; case SP_DOWN: case SP_UP: - twitching_test_angle(lean_angle, rotation_rate, target_angle*(1+0.5f*aggressiveness), test_angle_min, test_angle_max, test_rate_min, test_rate_max); - twitching_measure_acceleration(test_accel_max, rotation_rate - dir_sign * start_rate, rate_max); + twitching_test_angle(lean_angle, rotation_rate, target_angle * (1 + 0.5 * aggressiveness), test_angle_min, test_angle_max, test_rate_min, test_rate_max); + twitching_measure_acceleration(test_accel_max, rotation_rate - dir_sign * start_rate, test_rate_max); break; case RFF_UP: case MAX_GAINS: diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.h b/libraries/AC_AutoTune/AC_AutoTune_Multi.h index 3308ae5d8afb44..e580e4afcbf778 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.h @@ -156,7 +156,7 @@ class AC_AutoTune_Multi : public AC_AutoTune void twitching_test_angle(float angle, float rate, float angle_target, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max); // measure acceleration during twitch test - void twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) const; + void twitching_measure_acceleration(float &accel_average, float rate, float rate_max) const; // updating_rate_d_up - increase D and adjust P to optimize the D term for a little bounce back // optimize D term while keeping the maximum just below the target by adjusting P @@ -185,6 +185,14 @@ class AC_AutoTune_Multi : public AC_AutoTune AP_Int8 axis_bitmask; // axes to be tuned AP_Float aggressiveness; // aircraft response aggressiveness to be tuned AP_Float min_d; // minimum rate d gain allowed during tuning + bool ignore_next; // ignore the results of the next test when true + float target_angle; // target angle for the test + float target_rate; // target rate for the test + float angle_abort; // Angle that test is aborted + float test_rate_min; // the minimum angular rate achieved during TESTING_RATE + float test_rate_max; // the maximum angular rate achieved during TESTING_RATE + float test_angle_min; // the minimum angle achieved during TESTING_ANGLE + float test_angle_max; // the maximum angle achieved during TESTING_ANGLE }; #endif // AC_AUTOTUNE_ENABLED diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp index 56f6b59ece6fb7..5f6d2cfefd5bef 100644 --- a/libraries/AP_CANManager/AP_CANManager.cpp +++ b/libraries/AP_CANManager/AP_CANManager.cpp @@ -199,8 +199,9 @@ void AP_CANManager::init() } // Allocate the set type of Driver + switch (drv_type[drv_num]) { #if HAL_ENABLE_DRONECAN_DRIVERS - if (drv_type[drv_num] == AP_CAN::Protocol::DroneCAN) { + case AP_CAN::Protocol::DroneCAN: _drivers[drv_num] = _drv_param[drv_num]._uavcan = NEW_NOTHROW AP_DroneCAN(drv_num); if (_drivers[drv_num] == nullptr) { @@ -209,10 +210,10 @@ void AP_CANManager::init() } AP_Param::load_object_from_eeprom((AP_DroneCAN*)_drivers[drv_num], AP_DroneCAN::var_info); - } else + break; #endif #if HAL_PICCOLO_CAN_ENABLE - if (drv_type[drv_num] == AP_CAN::Protocol::PiccoloCAN) { + case AP_CAN::Protocol::PiccoloCAN: _drivers[drv_num] = _drv_param[drv_num]._piccolocan = NEW_NOTHROW AP_PiccoloCAN; if (_drivers[drv_num] == nullptr) { @@ -221,9 +222,9 @@ void AP_CANManager::init() } AP_Param::load_object_from_eeprom((AP_PiccoloCAN*)_drivers[drv_num], AP_PiccoloCAN::var_info); - } else + break; #endif - { + default: continue; } diff --git a/libraries/AP_Camera/AP_Camera_Params.cpp b/libraries/AP_Camera/AP_Camera_Params.cpp index 7fb915082c991e..749d2985f0ab40 100644 --- a/libraries/AP_Camera/AP_Camera_Params.cpp +++ b/libraries/AP_Camera/AP_Camera_Params.cpp @@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Camera_Params::var_info[] = { // @Param: _TYPE // @DisplayName: Camera shutter (trigger) type // @Description: how to trigger the camera to take a picture - // @Values: 0:None, 1:Servo, 2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting + // @Values: 0:None, 1:Servo, 2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi/Topotek/Viewpro/Xacti), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Camera_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index aef7fe26528a97..aa358f8fbde636 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -38,136 +38,138 @@ extern const AP_HAL::HAL &hal; -/* - header for 50Hz INS data - assumes the following config for VN-300: - $VNWRG,75,3,8,34,072E,0106,0612*0C - - 0x34: Groups 3,5,6 - Group 3 (IMU): - 0x072E: - UncompMag - UncompAccel - UncompGyro - Pres - Mag - Accel - AngularRate - Group 5 (Attitude): - 0x0106: - YawPitchRoll - Quaternion - YprU - Group 6 (INS): - 0x0612: - PosLLa - VelNed - PosU - VelU +/* +TYPE::VN_AHRS configures 2 packets: high-rate IMU and mid-rate EKF +Header for IMU packet + $VNWRG,75,3,16,01,0721*D415 + Common group (Group 1) + TimeStartup + AngularRate + Accel + Imu + MagPres +Header for EKF packet + $VNWRG,76,3,16,11,0001,0106*B36B + Common group (Group 1) + TimeStartup + Attitude group (Group 4) + Ypr + Quaternion + YprU */ -static const uint8_t vn_ins_pkt1_header[] { 0x34, 0x2E, 0x07, 0x06, 0x01, 0x12, 0x06 }; -#define VN_INS_PKT1_LENGTH 170 // includes header and CRC -struct PACKED VN_INS_packet1 { - float uncompMag[3]; +struct PACKED VN_IMU_packet { + static constexpr uint8_t header[]{0x01, 0x21, 0x07}; + uint64_t timeStartup; + float gyro[3]; + float accel[3]; float uncompAccel[3]; float uncompAngRate[3]; - float pressure; float mag[3]; - float accel[3]; - float gyro[3]; + float temp; + float pressure; +}; +constexpr uint8_t VN_IMU_packet::header[]; +constexpr uint8_t VN_IMU_LENGTH = sizeof(VN_IMU_packet) + sizeof(VN_IMU_packet::header) + 1 + 2; // Includes sync byte and CRC + +struct PACKED VN_AHRS_ekf_packet { + static constexpr uint8_t header[]{0x11, 0x01, 0x00, 0x06, 0x01}; + uint64_t timeStartup; float ypr[3]; float quaternion[4]; float yprU[3]; - double positionLLA[3]; - float velNED[3]; - float posU; - float velU; }; - -// check packet size for 4 groups -static_assert(sizeof(VN_INS_packet1)+2+3*2+2 == VN_INS_PKT1_LENGTH, "incorrect VN_INS_packet1 length"); +constexpr uint8_t VN_AHRS_ekf_packet::header[]; +constexpr uint8_t VN_AHRS_EKF_LENGTH = sizeof(VN_AHRS_ekf_packet) + sizeof(VN_AHRS_ekf_packet::header) + 1 + 2; // Includes sync byte and CRC /* - header for 5Hz GNSS data - assumes the following VN-300 config: - $VNWRG,76,3,80,4E,0002,0010,20B8,0018*63 - - 0x4E: Groups 2,3,4,7 - Group 2 (Time): - 0x0002: - TimeGps - Group 3 (IMU): - 0x0010: - Temp - Group 4 (GPS1): - 0x20B8: - NumSats - Fix - PosLLa - VelNed - DOP - Group 7 (GPS2): - 0x0018: - NumSats - Fix +TYPE::VN_INS configures 3 packets: high-rate IMU, mid-rate EKF, and 5Hz GNSS +Header for IMU packet + $VNWRG,75,3,16,01,0721*D415 + Common group (Group 1) + TimeStartup + AngularRate + Accel + Imu + MagPres +Header for EKF packet + $VNWRG,76,3,16,31,0001,0106,0613*097A + Common group (Group 1) + TimeStartup + Attitude group (Group 4) + Ypr + Quaternion + YprU + Ins group (Group 5) + InsStatus + PosLla + VelNed + PosU + VelU +Header for GNSS packet + $VNWRG,77,1,160,49,0003,26B8,0018*4FD9 + Common group (Group 1) + TimeStartup + TimeGps + Gnss1 group (Group 3) + NumSats + GnssFix + GnssPosLla + GnssVelNed + PosU1 + VelU1 + GnssDop + Gnss2 group (Group 6) + NumSats + GnssFix */ -static const uint8_t vn_ins_pkt2_header[] { 0x4e, 0x02, 0x00, 0x10, 0x00, 0xb8, 0x20, 0x18, 0x00 }; -#define VN_INS_PKT2_LENGTH 92 // includes header and CRC -struct PACKED VN_INS_packet2 { - uint64_t timeGPS; - float temp; - uint8_t numGPS1Sats; - uint8_t GPS1Fix; - double GPS1posLLA[3]; - float GPS1velNED[3]; - float GPS1DOP[7]; - uint8_t numGPS2Sats; - uint8_t GPS2Fix; +union Ins_Status { + uint16_t _value; + struct { + uint16_t mode : 2; + uint16_t gnssFix : 1; + uint16_t resv1 : 2; + uint16_t imuErr : 1; + uint16_t magPresErr : 1; + uint16_t gnssErr : 1; + uint16_t resv2 : 1; + uint16_t gnssHeadingIns : 2; + }; }; -// check packet size for 4 groups -static_assert(sizeof(VN_INS_packet2)+2+4*2+2 == VN_INS_PKT2_LENGTH, "incorrect VN_INS_packet2 length"); - -/* -header for 50Hz IMU data, used in TYPE::VN_AHRS only - assumes the following VN-100 config: - $VNWRG,75,3,80,14,073E,0004*66 - - Alternate first packet for VN-100 - 0x14: Groups 3, 5 - Group 3 (IMU): - 0x073E: - UncompMag - UncompAccel - UncompGyro - Temp - Pres - Mag - Accel - Gyro - Group 5 (Attitude): - 0x0004: - Quaternion -*/ -static const uint8_t vn_ahrs_pkt1_header[] { 0x14, 0x3E, 0x07, 0x04, 0x00 }; -#define VN_AHRS_PKT1_LENGTH 104 // includes header and CRC - -struct PACKED VN_AHRS_packet1 { - float uncompMag[3]; - float uncompAccel[3]; - float uncompAngRate[3]; - float temp; - float pressure; - float mag[3]; - float accel[3]; - float gyro[3]; +struct PACKED VN_INS_ekf_packet { + static constexpr uint8_t header[]{0x31, 0x01, 0x00, 0x06, 0x01, 0x13, 0x06}; + uint64_t timeStartup; + float ypr[3]; float quaternion[4]; + float yprU[3]; + uint16_t insStatus; + double posLla[3]; + float velNed[3]; + float posU; + float velU; }; - -static_assert(sizeof(VN_AHRS_packet1)+2+2*2+2 == VN_AHRS_PKT1_LENGTH, "incorrect VN_AHRS_packet1 length"); +constexpr uint8_t VN_INS_ekf_packet::header[]; +constexpr uint8_t VN_INS_EKF_LENGTH = sizeof(VN_INS_ekf_packet) + sizeof(VN_INS_ekf_packet::header) + 1 + 2; // Includes sync byte and CRC + +struct PACKED VN_INS_gnss_packet { + static constexpr uint8_t header[]{0x49, 0x03, 0x00, 0xB8, 0x26, 0x18, 0x00}; + uint64_t timeStartup; + uint64_t timeGps; + uint8_t numSats1; + uint8_t fix1; + double posLla1[3]; + float velNed1[3]; + float posU1[3]; + float velU1; + float dop1[7]; + uint8_t numSats2; + uint8_t fix2; +}; +constexpr uint8_t VN_INS_gnss_packet::header[]; +constexpr uint8_t VN_INS_GNSS_LENGTH = sizeof(VN_INS_gnss_packet) + sizeof(VN_INS_gnss_packet::header) + 1 + 2; // Includes sync byte and CRC // constructor AP_ExternalAHRS_VectorNav::AP_ExternalAHRS_VectorNav(AP_ExternalAHRS *_frontend, @@ -183,12 +185,13 @@ AP_ExternalAHRS_VectorNav::AP_ExternalAHRS_VectorNav(AP_ExternalAHRS *_frontend, baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0); port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0); - bufsize = MAX(MAX(VN_INS_PKT1_LENGTH, VN_INS_PKT2_LENGTH), VN_AHRS_PKT1_LENGTH); + bufsize = MAX(MAX(MAX(VN_IMU_LENGTH, VN_INS_EKF_LENGTH), VN_INS_GNSS_LENGTH), VN_AHRS_EKF_LENGTH); + pktbuf = NEW_NOTHROW uint8_t[bufsize]; - last_ins_pkt1 = NEW_NOTHROW VN_INS_packet1; - last_ins_pkt2 = NEW_NOTHROW VN_INS_packet2; + latest_ins_ekf_packet = NEW_NOTHROW VN_INS_ekf_packet; + latest_ins_gnss_packet = NEW_NOTHROW VN_INS_gnss_packet; - if (!pktbuf || !last_ins_pkt1 || !last_ins_pkt2) { + if (!pktbuf || !latest_ins_ekf_packet) { AP_BoardConfig::allocation_error("VectorNav ExternalAHRS"); } @@ -226,48 +229,57 @@ bool AP_ExternalAHRS_VectorNav::check_uart() bool match_header1 = false; bool match_header2 = false; bool match_header3 = false; + bool match_header4 = false; if (pktbuf[0] != SYNC_BYTE) { goto reset; } - if (type == TYPE::VN_INS) { - match_header1 = (0 == memcmp(&pktbuf[1], vn_ins_pkt1_header, MIN(sizeof(vn_ins_pkt1_header), unsigned(pktoffset-1)))); - match_header2 = (0 == memcmp(&pktbuf[1], vn_ins_pkt2_header, MIN(sizeof(vn_ins_pkt2_header), unsigned(pktoffset-1)))); + + match_header1 = (0 == memcmp(&pktbuf[1], VN_IMU_packet::header, MIN(sizeof(VN_IMU_packet::header), unsigned(pktoffset - 1)))); + if (type == TYPE::VN_AHRS) { + match_header2 = (0 == memcmp(&pktbuf[1], VN_AHRS_ekf_packet::header, MIN(sizeof(VN_AHRS_ekf_packet::header), unsigned(pktoffset - 1)))); } else { - match_header3 = (0 == memcmp(&pktbuf[1], vn_ahrs_pkt1_header, MIN(sizeof(vn_ahrs_pkt1_header), unsigned(pktoffset-1)))); + match_header3 = (0 == memcmp(&pktbuf[1], VN_INS_ekf_packet::header, MIN(sizeof(VN_INS_ekf_packet::header), unsigned(pktoffset - 1)))); + match_header4 = (0 == memcmp(&pktbuf[1], VN_INS_gnss_packet::header, MIN(sizeof(VN_INS_gnss_packet::header), unsigned(pktoffset - 1)))); } - if (!match_header1 && !match_header2 && !match_header3) { + if (!match_header1 && !match_header2 && !match_header3 && !match_header4) { goto reset; } - if (match_header1 && pktoffset >= VN_INS_PKT1_LENGTH) { - uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_PKT1_LENGTH-1, 0); + if (match_header1 && pktoffset >= VN_IMU_LENGTH) { + uint16_t crc = crc16_ccitt(&pktbuf[1], VN_IMU_LENGTH - 1, 0); + if (crc == 0) { + process_imu_packet(&pktbuf[sizeof(VN_IMU_packet::header) + 1]); + memmove(&pktbuf[0], &pktbuf[VN_IMU_LENGTH], pktoffset - VN_IMU_LENGTH); + pktoffset -= VN_IMU_LENGTH; + } else { + goto reset; + } + } else if (match_header2 && pktoffset >= VN_AHRS_EKF_LENGTH) { + uint16_t crc = crc16_ccitt(&pktbuf[1], VN_AHRS_EKF_LENGTH - 1, 0); if (crc == 0) { - // got pkt1 - process_ins_packet1(&pktbuf[sizeof(vn_ins_pkt1_header)+1]); - memmove(&pktbuf[0], &pktbuf[VN_INS_PKT1_LENGTH], pktoffset-VN_INS_PKT1_LENGTH); - pktoffset -= VN_INS_PKT1_LENGTH; + process_ahrs_ekf_packet(&pktbuf[sizeof(VN_AHRS_ekf_packet::header) + 1]); + memmove(&pktbuf[0], &pktbuf[VN_AHRS_EKF_LENGTH], pktoffset - VN_AHRS_EKF_LENGTH); + pktoffset -= VN_AHRS_EKF_LENGTH; } else { goto reset; } - } else if (match_header2 && pktoffset >= VN_INS_PKT2_LENGTH) { - uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_PKT2_LENGTH-1, 0); + } else if (match_header3 && pktoffset >= VN_INS_EKF_LENGTH) { + uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_EKF_LENGTH - 1, 0); if (crc == 0) { - // got pkt2 - process_ins_packet2(&pktbuf[sizeof(vn_ins_pkt2_header)+1]); - memmove(&pktbuf[0], &pktbuf[VN_INS_PKT2_LENGTH], pktoffset-VN_INS_PKT2_LENGTH); - pktoffset -= VN_INS_PKT2_LENGTH; + process_ins_ekf_packet(&pktbuf[sizeof(VN_INS_ekf_packet::header) + 1]); + memmove(&pktbuf[0], &pktbuf[VN_INS_EKF_LENGTH], pktoffset - VN_INS_EKF_LENGTH); + pktoffset -= VN_INS_EKF_LENGTH; } else { goto reset; } - } else if (match_header3 && pktoffset >= VN_AHRS_PKT1_LENGTH) { - uint16_t crc = crc16_ccitt(&pktbuf[1], VN_AHRS_PKT1_LENGTH-1, 0); + } else if (match_header4 && pktoffset >= VN_INS_GNSS_LENGTH) { + uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_GNSS_LENGTH - 1, 0); if (crc == 0) { - // got AHRS pkt - process_ahrs_packet(&pktbuf[sizeof(vn_ahrs_pkt1_header)+1]); - memmove(&pktbuf[0], &pktbuf[VN_AHRS_PKT1_LENGTH], pktoffset-VN_AHRS_PKT1_LENGTH); - pktoffset -= VN_AHRS_PKT1_LENGTH; + process_ins_gnss_packet(&pktbuf[sizeof(VN_INS_gnss_packet::header) + 1]); + memmove(&pktbuf[0], &pktbuf[VN_INS_GNSS_LENGTH], pktoffset - VN_INS_GNSS_LENGTH); + pktoffset -= VN_INS_GNSS_LENGTH; } else { goto reset; } @@ -431,8 +443,9 @@ void AP_ExternalAHRS_VectorNav::initialize() { // VN-1X0 type = TYPE::VN_AHRS; - // This assumes unit is still configured at its default rate of 800hz - run_command("VNWRG,75,3,%u,14,073E,0004", unsigned(800 / get_rate())); + // These assumes unit is still configured at its default rate of 800hz + run_command("VNWRG,75,3,%u,01,0721", unsigned(800 / get_rate())); + run_command("VNWRG,76,3,16,11,0001,0106"); } else { // Default to setup for sensors other than VN-100 or VN-110 // This assumes unit is still configured at its default IMU rate of 400hz for VN-300, 800hz for others @@ -443,8 +456,9 @@ void AP_ExternalAHRS_VectorNav::initialize() { if (strncmp(model_name, "VN-3", 4) == 0) { has_dual_gnss = true; } - run_command("VNWRG,75,3,%u,34,072E,0106,0612", unsigned(imu_rate / get_rate())); - run_command("VNWRG,76,3,%u,4E,0002,0010,20B8,0018", unsigned(imu_rate / 5)); + run_command("VNWRG,75,3,%u,01,0721", unsigned(imu_rate / get_rate())); + run_command("VNWRG,76,3,%u,31,0001,0106,0613", unsigned(imu_rate / 50)); + run_command("VNWRG,77,3,%u,49,0003,26B8,0018", unsigned(imu_rate / 5)); } // Resume asynchronous communications @@ -469,129 +483,48 @@ const char* AP_ExternalAHRS_VectorNav::get_name() const return nullptr; } -/* - process INS mode INS packet - */ -void AP_ExternalAHRS_VectorNav::process_ins_packet1(const uint8_t *b) -{ - const struct VN_INS_packet1 &pkt1 = *(struct VN_INS_packet1 *)b; - const struct VN_INS_packet2 &pkt2 = *last_ins_pkt2; - - last_pkt1_ms = AP_HAL::millis(); - *last_ins_pkt1 = pkt1; - - const bool use_uncomp = option_is_set(AP_ExternalAHRS::OPTIONS::VN_UNCOMP_IMU); - - { - WITH_SEMAPHORE(state.sem); - if (use_uncomp) { - state.accel = Vector3f{pkt1.uncompAccel[0], pkt1.uncompAccel[1], pkt1.uncompAccel[2]}; - state.gyro = Vector3f{pkt1.uncompAngRate[0], pkt1.uncompAngRate[1], pkt1.uncompAngRate[2]}; - } else { - state.accel = Vector3f{pkt1.accel[0], pkt1.accel[1], pkt1.accel[2]}; - state.gyro = Vector3f{pkt1.gyro[0], pkt1.gyro[1], pkt1.gyro[2]}; - } - - state.quat = Quaternion{pkt1.quaternion[3], pkt1.quaternion[0], pkt1.quaternion[1], pkt1.quaternion[2]}; - state.have_quaternion = true; - - state.velocity = Vector3f{pkt1.velNED[0], pkt1.velNED[1], pkt1.velNED[2]}; - state.have_velocity = true; - - state.location = Location{int32_t(pkt1.positionLLA[0] * 1.0e7), - int32_t(pkt1.positionLLA[1] * 1.0e7), - int32_t(pkt1.positionLLA[2] * 1.0e2), - Location::AltFrame::ABSOLUTE}; - state.last_location_update_us = AP_HAL::micros(); - state.have_location = true; - } - -#if AP_BARO_EXTERNALAHRS_ENABLED - { - AP_ExternalAHRS::baro_data_message_t baro; - baro.instance = 0; - baro.pressure_pa = pkt1.pressure*1e3; - baro.temperature = pkt2.temp; +// Input data struct for EAHA logging message, used by both AHRS mode and INS mode +struct AP_ExternalAHRS_VectorNav::EAHA { + uint64_t timeUs; + float quat[4]; + float ypr[3]; + float yprU[3]; +}; - AP::baro().handle_external(baro); - } -#endif -#if AP_COMPASS_EXTERNALAHRS_ENABLED - { - AP_ExternalAHRS::mag_data_message_t mag; - mag.field = Vector3f{pkt1.mag[0], pkt1.mag[1], pkt1.mag[2]}; - mag.field *= 1000; // to mGauss +void AP_ExternalAHRS_VectorNav::write_eaha(const EAHA& data_to_log) const { - AP::compass().handle_external(mag); - } +#if HAL_LOGGING_ENABLED + // @LoggerMessage: EAHA + // @Description: External AHRS Attitude data + // @Field: TimeUS: Time since system startup + // @Field: Q1: Attitude quaternion 1 + // @Field: Q2: Attitude quaternion 2 + // @Field: Q3: Attitude quaternion 3 + // @Field: Q4: Attitude quaternion 4 + // @Field: Yaw: Yaw + // @Field: Pitch: Pitch + // @Field: Roll: Roll + // @Field: YU: Yaw unceratainty + // @Field: PU: Pitch uncertainty + // @Field: RU: Roll uncertainty + + AP::logger().WriteStreaming("EAHA", "TimeUS,Q1,Q2,Q3,Q4,Yaw,Pitch,Roll,YU,PU,RU", + "s----dddddd", "F0000000000", + "Qffffffffff", + data_to_log.timeUs, + data_to_log.quat[0], data_to_log.quat[1], data_to_log.quat[2], data_to_log.quat[3], + data_to_log.ypr[0], data_to_log.ypr[1], data_to_log.ypr[2], + data_to_log.yprU[0], data_to_log.yprU[1], data_to_log.yprU[2]); #endif - - { - AP_ExternalAHRS::ins_data_message_t ins; - - ins.accel = state.accel; - ins.gyro = state.gyro; - ins.temperature = pkt2.temp; - - AP::ins().handle_external(ins); - } } -/* - process INS mode GNSS packet - */ -void AP_ExternalAHRS_VectorNav::process_ins_packet2(const uint8_t *b) -{ - const struct VN_INS_packet2 &pkt2 = *(struct VN_INS_packet2 *)b; - const struct VN_INS_packet1 &pkt1 = *last_ins_pkt1; - - last_pkt2_ms = AP_HAL::millis(); - *last_ins_pkt2 = pkt2; - - AP_ExternalAHRS::gps_data_message_t gps; - // get ToW in milliseconds - gps.gps_week = pkt2.timeGPS / (AP_MSEC_PER_WEEK * 1000000ULL); - gps.ms_tow = (pkt2.timeGPS / 1000000ULL) % (60*60*24*7*1000ULL); - gps.fix_type = pkt2.GPS1Fix; - gps.satellites_in_view = pkt2.numGPS1Sats; - - gps.horizontal_pos_accuracy = pkt1.posU; - gps.vertical_pos_accuracy = pkt1.posU; - gps.horizontal_vel_accuracy = pkt1.velU; - - gps.hdop = pkt2.GPS1DOP[4]; - gps.vdop = pkt2.GPS1DOP[3]; - - gps.latitude = pkt2.GPS1posLLA[0] * 1.0e7; - gps.longitude = pkt2.GPS1posLLA[1] * 1.0e7; - gps.msl_altitude = pkt2.GPS1posLLA[2] * 1.0e2; - - gps.ned_vel_north = pkt2.GPS1velNED[0]; - gps.ned_vel_east = pkt2.GPS1velNED[1]; - gps.ned_vel_down = pkt2.GPS1velNED[2]; - - if (gps.fix_type >= 3 && !state.have_origin) { - WITH_SEMAPHORE(state.sem); - state.origin = Location{int32_t(pkt2.GPS1posLLA[0] * 1.0e7), - int32_t(pkt2.GPS1posLLA[1] * 1.0e7), - int32_t(pkt2.GPS1posLLA[2] * 1.0e2), - Location::AltFrame::ABSOLUTE}; - state.have_origin = true; - } - uint8_t instance; - if (AP::gps().get_first_external_instance(instance)) { - AP::gps().handle_external(gps, instance); - } -} -/* - process AHRS mode AHRS packet - */ -void AP_ExternalAHRS_VectorNav::process_ahrs_packet(const uint8_t *b) +// process INS mode INS packet +void AP_ExternalAHRS_VectorNav::process_imu_packet(const uint8_t *b) { - const struct VN_AHRS_packet1 &pkt = *(struct VN_AHRS_packet1 *)b; + const struct VN_IMU_packet &pkt = *(struct VN_IMU_packet *)b; last_pkt1_ms = AP_HAL::millis(); @@ -606,16 +539,13 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_packet(const uint8_t *b) state.accel = Vector3f{pkt.accel[0], pkt.accel[1], pkt.accel[2]}; state.gyro = Vector3f{pkt.gyro[0], pkt.gyro[1], pkt.gyro[2]}; } - - state.quat = Quaternion{pkt.quaternion[3], pkt.quaternion[0], pkt.quaternion[1], pkt.quaternion[2]}; - state.have_quaternion = true; } #if AP_BARO_EXTERNALAHRS_ENABLED { AP_ExternalAHRS::baro_data_message_t baro; baro.instance = 0; - baro.pressure_pa = pkt.pressure*1e3; + baro.pressure_pa = pkt.pressure * 1e3; baro.temperature = pkt.temp; AP::baro().handle_external(baro); @@ -625,11 +555,7 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_packet(const uint8_t *b) #if AP_COMPASS_EXTERNALAHRS_ENABLED { AP_ExternalAHRS::mag_data_message_t mag; - if (use_uncomp) { - mag.field = Vector3f{pkt.uncompMag[0], pkt.uncompMag[1], pkt.uncompMag[2]}; - } else { - mag.field = Vector3f{pkt.mag[0], pkt.mag[1], pkt.mag[2]}; - } + mag.field = Vector3f{pkt.mag[0], pkt.mag[1], pkt.mag[2]}; mag.field *= 1000; // to mGauss AP::compass().handle_external(mag); @@ -647,8 +573,8 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_packet(const uint8_t *b) } #if HAL_LOGGING_ENABLED - // @LoggerMessage: EAH3 - // @Description: External AHRS data + // @LoggerMessage: EAHI + // @Description: External AHRS IMU data // @Field: TimeUS: Time since system startup // @Field: Temp: Temprature // @Field: Pres: Pressure @@ -661,25 +587,131 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_packet(const uint8_t *b) // @Field: GX: Rotation rate X-axis // @Field: GY: Rotation rate Y-axis // @Field: GZ: Rotation rate Z-axis - // @Field: Q1: Attitude quaternion 1 - // @Field: Q2: Attitude quaternion 2 - // @Field: Q3: Attitude quaternion 3 - // @Field: Q4: Attitude quaternion 4 - AP::logger().WriteStreaming("EAH3", "TimeUS,Temp,Pres,MX,MY,MZ,AX,AY,AZ,GX,GY,GZ,Q1,Q2,Q3,Q4", - "sdPGGGoooEEE----", "F000000000000000", - "Qfffffffffffffff", + AP::logger().WriteStreaming("EAHI", "TimeUS,Temp,Pres,MX,MY,MZ,AX,AY,AZ,GX,GY,GZ", + "sdPGGGoooEEE", "F00000000000", + "Qfffffffffff", AP_HAL::micros64(), pkt.temp, pkt.pressure*1e3, - use_uncomp ? pkt.uncompMag[0] : pkt.mag[0], - use_uncomp ? pkt.uncompMag[1] : pkt.mag[1], - use_uncomp ? pkt.uncompMag[2] : pkt.mag[2], + pkt.mag[0], pkt.mag[1], pkt.mag[2], state.accel[0], state.accel[1], state.accel[2], state.gyro[0], state.gyro[1], state.gyro[2], state.quat[0], state.quat[1], state.quat[2], state.quat[3]); #endif // HAL_LOGGING_ENABLED } +// process AHRS mode AHRS packet +void AP_ExternalAHRS_VectorNav::process_ahrs_ekf_packet(const uint8_t *b) { + const struct VN_AHRS_ekf_packet &pkt = *(struct VN_AHRS_ekf_packet *)b; + + last_pkt2_ms = AP_HAL::millis(); + + state.quat = Quaternion{pkt.quaternion[3], pkt.quaternion[0], pkt.quaternion[1], pkt.quaternion[2]}; + state.have_quaternion = true; + +#if HAL_LOGGING_ENABLED + EAHA data_to_log; + data_to_log.timeUs = AP_HAL::micros64(); + memcpy(data_to_log.quat, pkt.quaternion, sizeof(pkt.quaternion)); + memcpy(data_to_log.ypr, pkt.ypr, sizeof(pkt.ypr)); + memcpy(data_to_log.yprU, pkt.yprU, sizeof(pkt.yprU)); + + write_eaha(data_to_log); +#endif // HAL_LOGGING_ENABLED +} + +// process INS mode EKF packet +void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) { + const struct VN_INS_ekf_packet &pkt = *(struct VN_INS_ekf_packet *)b; + + last_pkt2_ms = AP_HAL::millis(); + *latest_ins_ekf_packet = pkt; + + state.quat = Quaternion{pkt.quaternion[3], pkt.quaternion[0], pkt.quaternion[1], pkt.quaternion[2]}; + state.have_quaternion = true; + + state.velocity = Vector3f{pkt.velNed[0], pkt.velNed[1], pkt.velNed[2]}; + state.have_velocity = true; + + state.location = Location{int32_t(pkt.posLla[0] * 1.0e7), int32_t(pkt.posLla[1] * 1.0e7), int32_t(pkt.posLla[2] * 1.0e2), Location::AltFrame::ABSOLUTE}; + state.last_location_update_us = AP_HAL::micros(); + state.have_location = true; + +#if HAL_LOGGING_ENABLED + EAHA data_to_log; + auto now = AP_HAL::micros64(); + data_to_log.timeUs = now; + memcpy(data_to_log.quat, pkt.quaternion, sizeof(pkt.quaternion)); + memcpy(data_to_log.ypr, pkt.ypr, sizeof(pkt.ypr)); + memcpy(data_to_log.yprU, pkt.yprU, sizeof(pkt.yprU)); + write_eaha(data_to_log); + + // @LoggerMessage: EAHK + // @Description: External AHRS INS Kalman Filter data + // @Field: TimeUS: Time since system startup + // @Field: InsStatus: VectorNav INS health status + // @Field: Lat: Latitude + // @Field: Lon: Longitude + // @Field: Alt: Altitude + // @Field: VelN: Velocity Northing + // @Field: VelE: Velocity Easting + // @Field: VelD: Velocity Downing + // @Field: PosU: Filter estimated position uncertainty + // @Field: VelU: Filter estimated Velocity uncertainty + + AP::logger().WriteStreaming("EAHK", "TimeUS,InsStatus,Lat,Lon,Alt,VelN,VelE,VelD,PosU,VelU", + "s-ddmnnndn", "F000000000", + "QHdddfffff", + now, + pkt.insStatus, + pkt.posLla[0], pkt.posLla[1], pkt.posLla[2], + pkt.velNed[0], pkt.velNed[1], pkt.velNed[2], + pkt.posU, pkt.velU); +#endif // HAL_LOGGING_ENABLED + +} + +// process INS mode GNSS packet +void AP_ExternalAHRS_VectorNav::process_ins_gnss_packet(const uint8_t *b) { + const struct VN_INS_gnss_packet &pkt = *(struct VN_INS_gnss_packet *)b; + AP_ExternalAHRS::gps_data_message_t gps; + + + last_pkt3_ms = AP_HAL::millis(); + *latest_ins_gnss_packet = pkt; + + // get ToW in milliseconds + gps.gps_week = pkt.timeGps / (AP_MSEC_PER_WEEK * 1000000ULL); + gps.ms_tow = (pkt.timeGps / 1000000ULL) % (60 * 60 * 24 * 7 * 1000ULL); + gps.fix_type = pkt.fix1; + gps.satellites_in_view = pkt.numSats1; + + gps.horizontal_pos_accuracy = pkt.posU1[0]; + gps.vertical_pos_accuracy = pkt.posU1[2]; + gps.horizontal_vel_accuracy = pkt.velU1; + + gps.hdop = pkt.dop1[4]; + gps.vdop = pkt.dop1[3]; + + gps.latitude = pkt.posLla1[0] * 1.0e7; + gps.longitude = pkt.posLla1[1] * 1.0e7; + gps.msl_altitude = pkt.posLla1[2] * 1.0e2; + + gps.ned_vel_north = pkt.velNed1[0]; + gps.ned_vel_east = pkt.velNed1[1]; + gps.ned_vel_down = pkt.velNed1[2]; + + if (!state.have_origin && gps.fix_type >= 3) { + WITH_SEMAPHORE(state.sem); + state.origin = Location{int32_t(pkt.posLla1[0] * 1.0e7), int32_t(pkt.posLla1[1] * 1.0e7), + int32_t(pkt.posLla1[2] * 1.0e2), Location::AltFrame::ABSOLUTE}; + state.have_origin = true; + } + uint8_t instance; + if (AP::gps().get_first_external_instance(instance)) { + AP::gps().handle_external(gps, instance); + } +} // get serial port number for the uart int8_t AP_ExternalAHRS_VectorNav::get_port(void) const @@ -694,10 +726,7 @@ int8_t AP_ExternalAHRS_VectorNav::get_port(void) const bool AP_ExternalAHRS_VectorNav::healthy(void) const { const uint32_t now = AP_HAL::millis(); - if (type == TYPE::VN_AHRS) { - return (now - last_pkt1_ms < 40); - } - return (now - last_pkt1_ms < 40 && now - last_pkt2_ms < 500); + return (now - last_pkt1_ms < 40) && (now - last_pkt2_ms < 500) && (type == TYPE::VN_AHRS ? true: now - last_pkt3_ms < 1000); } bool AP_ExternalAHRS_VectorNav::initialised(void) const @@ -705,10 +734,7 @@ bool AP_ExternalAHRS_VectorNav::initialised(void) const if (!setup_complete) { return false; } - if (type == TYPE::VN_AHRS) { - return last_pkt1_ms != 0; - } - return last_pkt1_ms != 0 && last_pkt2_ms != 0; + return last_pkt1_ms != UINT32_MAX && last_pkt2_ms != UINT32_MAX && (type == TYPE::VN_AHRS ? true : last_pkt3_ms != UINT32_MAX); } bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const @@ -722,11 +748,11 @@ bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure return false; } if (type == TYPE::VN_INS) { - if (last_ins_pkt2->GPS1Fix < 3) { + if (latest_ins_gnss_packet->fix1 < 3) { hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS1 lock"); return false; } - if (has_dual_gnss && (last_ins_pkt2->GPS2Fix < 3)) { + if (has_dual_gnss && (latest_ins_gnss_packet->fix2 < 3)) { hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS2 lock"); return false; } @@ -741,37 +767,31 @@ bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure void AP_ExternalAHRS_VectorNav::get_filter_status(nav_filter_status &status) const { memset(&status, 0, sizeof(status)); - if (type == TYPE::VN_INS) { - if (last_ins_pkt1 && last_ins_pkt2) { - status.flags.initalized = true; - } - if (healthy() && last_ins_pkt2) { + status.flags.initalized = initialised(); + if (healthy()) { + if (type == TYPE::VN_AHRS) { + status.flags.attitude = true; + } else { status.flags.attitude = true; - status.flags.vert_vel = true; - status.flags.vert_pos = true; - - const struct VN_INS_packet2 &pkt2 = *last_ins_pkt2; - if (pkt2.GPS1Fix >= 3) { - status.flags.horiz_vel = true; - status.flags.horiz_pos_rel = true; - status.flags.horiz_pos_abs = true; + if (latest_ins_ekf_packet) { + status.flags.vert_vel = true; + status.flags.vert_pos = true; + + status.flags.horiz_vel = true; + status.flags.horiz_pos_rel = true; + status.flags.horiz_pos_abs = true; status.flags.pred_horiz_pos_rel = true; status.flags.pred_horiz_pos_abs = true; - status.flags.using_gps = true; + status.flags.using_gps = true; } } - } else { - status.flags.initalized = initialised(); - if (healthy()) { - status.flags.attitude = true; - } } } // send an EKF_STATUS message to GCS void AP_ExternalAHRS_VectorNav::send_status_report(GCS_MAVLINK &link) const { - if (!last_ins_pkt1) { + if (!latest_ins_ekf_packet) { return; } // prepare flags @@ -813,13 +833,13 @@ void AP_ExternalAHRS_VectorNav::send_status_report(GCS_MAVLINK &link) const } // send message - const struct VN_INS_packet1 &pkt1 = *(struct VN_INS_packet1 *)last_ins_pkt1; + const struct VN_INS_ekf_packet &pkt = *(struct VN_INS_ekf_packet *)latest_ins_ekf_packet; const float vel_gate = 5; const float pos_gate = 5; const float hgt_gate = 5; const float mag_var = 0; mavlink_msg_ekf_status_report_send(link.get_chan(), flags, - pkt1.velU/vel_gate, pkt1.posU/pos_gate, pkt1.posU/hgt_gate, + pkt.velU / vel_gate, pkt.posU / pos_gate, pkt.posU / hgt_gate, mag_var, 0, 0); } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h index db73138befe348..8e1a1c5ec1917c 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h @@ -63,20 +63,27 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend { void initialize(); - void process_ins_packet1(const uint8_t *b); - void process_ins_packet2(const uint8_t *b); - void process_ahrs_packet(const uint8_t *b); void run_command(const char *fmt, ...); + struct EAHA; + void write_eaha(const EAHA& data_to_log) const; + void process_imu_packet(const uint8_t *b); + void process_ahrs_ekf_packet(const uint8_t *b); + void process_ins_ekf_packet(const uint8_t *b); + void process_ins_gnss_packet(const uint8_t *b); + + uint8_t *pktbuf; uint16_t pktoffset; uint16_t bufsize; - struct VN_INS_packet1 *last_ins_pkt1; - struct VN_INS_packet2 *last_ins_pkt2; + struct VN_imu_packet *latest_imu_packet; + struct VN_INS_ekf_packet *latest_ins_ekf_packet; + struct VN_INS_gnss_packet *latest_ins_gnss_packet; uint32_t last_pkt1_ms; uint32_t last_pkt2_ms; + uint32_t last_pkt3_ms; enum class TYPE { VN_INS, // Full INS mode, requiring GNSS. Used by VN-2X0 and VN-3X0 diff --git a/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp index c234779d050165..28a2073492e97e 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp @@ -47,7 +47,7 @@ typedef struct { #define MAX_FILES 16 static FAT_FILE *file_table[MAX_FILES]; -static int isatty_(int fileno) +static bool isatty_(int fileno) { if (fileno >= 0 && fileno <= 2) { return true; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp index f9bf7872e892d6..6d474337ef227e 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp @@ -14,9 +14,9 @@ */ #include "AP_Frsky_Parameters.h" -#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL const AP_Param::GroupInfo AP_Frsky_Parameters::var_info[] = { +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL // @Param: UPLINK_ID // @DisplayName: Uplink sensor id // @Description: Change the uplink sensor id (SPort only) @@ -37,6 +37,7 @@ const AP_Param::GroupInfo AP_Frsky_Parameters::var_info[] = { // @Values: -1:Disable,7:7,8:8,9:9,10:10,11:11,12:12,13:13,14:14,15:15,16:16,17:17,18:18,19:19,20:20,21:21,22:22,23:23,24:24,25:25,26:26 // @User: Advanced AP_GROUPINFO("DNLINK2_ID", 3, AP_Frsky_Parameters, _dnlink2_id, 7), +#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL // @Param: DNLINK_ID // @DisplayName: Default downlink sensor id @@ -58,5 +59,3 @@ AP_Frsky_Parameters::AP_Frsky_Parameters() { AP_Param::setup_object_defaults(this, var_info); } - -#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h b/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h index 3826f340e6191f..f6a06ac962570b 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h @@ -16,7 +16,6 @@ #include "AP_Frsky_Telem.h" -#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL #include #include @@ -33,11 +32,11 @@ class AP_Frsky_Parameters private: // settable parameters - AP_Int8 _uplink_id; AP_Int8 _dnlink_id; +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL + AP_Int8 _uplink_id; AP_Int8 _dnlink1_id; AP_Int8 _dnlink2_id; +#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL AP_Int8 _options; }; - -#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index ec883049f141b2..26212761307b40 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -20,8 +20,8 @@ #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL #include "AP_Frsky_MAVlite.h" -#include "AP_Frsky_Parameters.h" #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL +#include "AP_Frsky_Parameters.h" /* for FrSky SPort Passthrough @@ -940,6 +940,16 @@ void AP_Frsky_SPort_Passthrough::process_tx_queue() send_sport_frame(SPORT_DOWNLINK_FRAME, packet.appid, packet.data); } +/* + * Send a mavlite message + * Message is chunked in sport packets pushed in the tx queue + * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + */ +bool AP_Frsky_SPort_Passthrough::send_message(const AP_Frsky_MAVlite_Message &txmsg) +{ + return mavlite_to_sport.process(_SPort_bidir.tx_packet_queue, txmsg); +} +#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL /* * Utility method to apply constraints in changing sensor id values * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) @@ -956,17 +966,6 @@ void AP_Frsky_SPort_Passthrough::set_sensor_id(AP_Int8 param_idx, uint8_t &senso sensor = calc_sensor_id(idx); } -/* - * Send a mavlite message - * Message is chunked in sport packets pushed in the tx queue - * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) - */ -bool AP_Frsky_SPort_Passthrough::send_message(const AP_Frsky_MAVlite_Message &txmsg) -{ - return mavlite_to_sport.process(_SPort_bidir.tx_packet_queue, txmsg); -} -#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL - namespace AP { AP_Frsky_SPort_Passthrough *frsky_passthrough_telem() diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h index e48541d8cdd8d3..441d8b5dbf4f17 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h @@ -149,7 +149,6 @@ class AP_Frsky_SPort_Passthrough : public AP_Frsky_SPort, public AP_RCTelemetry AP_Frsky_MAVlite_SPortToMAVlite sport_to_mavlite; AP_Frsky_MAVlite_MAVliteToSPort mavlite_to_sport; - void set_sensor_id(AP_Int8 idx, uint8_t &sensor); // tx/rx sport packet processing void queue_rx_packet(const AP_Frsky_SPort::sport_packet_t sp); void process_rx_queue(void); @@ -160,7 +159,7 @@ class AP_Frsky_SPort_Passthrough : public AP_Frsky_SPort, public AP_RCTelemetry bool send_message(const AP_Frsky_MAVlite_Message &txmsg); AP_Frsky_MAVliteMsgHandler mavlite{FUNCTOR_BIND_MEMBER(&AP_Frsky_SPort_Passthrough::send_message, bool, const AP_Frsky_MAVlite_Message &)}; #endif - + void set_sensor_id(AP_Int8 idx, uint8_t &sensor); void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data); // true if we need to respond to the last polling byte diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index da4ae589df661e..86689cfdba30b8 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -70,6 +70,9 @@ static const eventmask_t EVT_PARITY = EVENT_MASK(11); // event for transmit end for half-duplex static const eventmask_t EVT_TRANSMIT_END = EVENT_MASK(12); +// event for framing error +static const eventmask_t EVT_ERROR = EVENT_MASK(13); + // events for dma tx, thread per UART so can be from 0 static const eventmask_t EVT_TRANSMIT_DMA_START = EVENT_MASK(0); static const eventmask_t EVT_TRANSMIT_DMA_COMPLETE = EVENT_MASK(1); @@ -452,6 +455,13 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) sercfg.cr3 |= USART_CR3_DMAT; } sercfg.irq_cb = rx_irq_cb; +#if HAL_HAVE_LOW_NOISE_UART + if (sdef.low_noise_line) { + // we can mark UART to sample on one bit instead of default 3 bits + // this allows us to be slightly less sensitive to clock differences + sercfg.cr3 |= USART_CR3_ONEBIT; + } +#endif #endif // HAL_UART_NODMA if (!(sercfg.cr2 & USART_CR2_STOP2_BITS)) { sercfg.cr2 |= USART_CR2_STOP1_BITS; @@ -495,6 +505,16 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) vprintf_console_hook = hal_console_vprintf; #endif } + +#if HAL_UART_STATS_ENABLED && CH_CFG_USE_EVENTS == TRUE + if (!err_listener_initialised) { + chEvtRegisterMaskWithFlags(chnGetEventSource((SerialDriver*)sdef.serial), + &err_listener, + EVT_ERROR, + SD_FRAMING_ERROR | SD_OVERRUN_ERROR | SD_NOISE_ERROR); + err_listener_initialised = true; + } +#endif } #ifndef HAL_UART_NODMA @@ -1091,6 +1111,22 @@ void UARTDriver::_rx_timer_tick(void) _in_rx_timer = true; +#if HAL_UART_STATS_ENABLED && CH_CFG_USE_EVENTS == TRUE + if (!sdef.is_usb) { + const auto err_flags = chEvtGetAndClearFlags(&err_listener); + // count the number of errors + if (err_flags & SD_FRAMING_ERROR) { + _rx_stats_framing_errors++; + } + if (err_flags & SD_OVERRUN_ERROR) { + _rx_stats_overrun_errors++; + } + if (err_flags & SD_NOISE_ERROR) { + _rx_stats_noise_errors++; + } + } +#endif + #ifndef HAL_UART_NODMA if (rx_dma_enabled && rxdma) { chSysLock(); @@ -1748,13 +1784,22 @@ void UARTDriver::uart_info(ExpandingString &str, StatsTracker &stats, const uint } else { str.printf("UART%u ", unsigned(sdef.instance)); } - str.printf("TX%c=%8u RX%c=%8u TXBD=%6u RXBD=%6u FlowCtrl=%u\n", + str.printf("TX%c=%8u RX%c=%8u TXBD=%6u RXBD=%6u" +#if CH_CFG_USE_EVENTS == TRUE + " FE=%lu OE=%lu NE=%lu" +#endif + " FlowCtrl=%u\n", tx_dma_enabled ? '*' : ' ', unsigned(tx_bytes), rx_dma_enabled ? '*' : ' ', unsigned(rx_bytes), unsigned((tx_bytes * 10000) / dt_ms), unsigned((rx_bytes * 10000) / dt_ms), +#if CH_CFG_USE_EVENTS == TRUE + _rx_stats_framing_errors, + _rx_stats_overrun_errors, + _rx_stats_noise_errors, +#endif _flow_control); } #endif diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index f21c1d7aa8be4f..c549b8eebd5424 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -28,6 +28,10 @@ // enough for serial0 to serial9, plus IOMCU #define UART_MAX_DRIVERS 11 +#ifndef HAL_HAVE_LOW_NOISE_UART +#define HAL_HAVE_LOW_NOISE_UART 0 +#endif + class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { public: UARTDriver(uint8_t serial_num); @@ -79,6 +83,10 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { uint8_t get_index(void) const { return uint8_t(this - &_serial_tab[0]); } + +#if HAL_HAVE_LOW_NOISE_UART + bool low_noise_line; +#endif }; bool wait_timeout(uint16_t n, uint32_t timeout_ms) override; @@ -282,6 +290,13 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { // Getters for cumulative tx and rx counts uint32_t get_total_tx_bytes() const override { return _tx_stats_bytes; } uint32_t get_total_rx_bytes() const override { return _rx_stats_bytes; } +#if CH_CFG_USE_EVENTS == TRUE + uint32_t _rx_stats_framing_errors; + uint32_t _rx_stats_overrun_errors; + uint32_t _rx_stats_noise_errors; + event_listener_t err_listener; + bool err_listener_initialised; +#endif #endif }; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/defaults.parm new file mode 100644 index 00000000000000..f508c948650b0c --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/defaults.parm @@ -0,0 +1,15 @@ +NET_ENABLE 1 +NET_OPTIONS 1 + +# enable hw flow control +UART1_RTSCTS 1 + +# swap TX and RX +UART1_OPTIONS 8 + +SCR_ENABLE 1 +SCR_VM_I_COUNT 1000000 +SCR_HEAP_SIZE 150000 + +WEB_ENABLE 1 +WEB_PORT 80 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef-bl.dat index 5984ec4712d62c..03960b22470703 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef-bl.dat @@ -7,7 +7,7 @@ MCU STM32H7xx STM32H723xx # crystal frequency -OSCILLATOR_HZ 0 +OSCILLATOR_HZ 16000000 # setup build for a peripheral bootloader env AP_PERIPH 1 @@ -39,13 +39,18 @@ PA14 JTCK-SWCLK SWD PD0 CAN1_RX CAN1 PD1 CAN1_TX CAN1 +#PD4 USART2_DE USART2 +PD5 USART2_TX USART2 +PD6 USART2_RX USART2 + PD8 USART3_TX USART3 PD9 USART3_RX USART3 PD12 USART3_RTS USART3 PD11 USART3_CTS USART3 # LEDs -PE5 LED_BOOTLOADER OUTPUT OPENDRAIN HIGH # blue +PC0 LED_STT1 OUTPUT OPENDRAIN HIGH +PA4 LED_STT2 OUTPUT OPENDRAIN HIGH define HAL_LED_ON 0 define HAL_USE_EMPTY_STORAGE 1 @@ -55,15 +60,14 @@ PC1 ETH_MDC ETH1 PA2 ETH_MDIO ETH1 PC4 ETH_RMII_RXD0 ETH1 PC5 ETH_RMII_RXD1 ETH1 -#PB12 ETH_RMII_TXD0 ETH1 -PG13 ETH_RMII_TXD0 ETH1 +PB12 ETH_RMII_TXD0 ETH1 PB13 ETH_RMII_TXD1 ETH1 -#PB11 ETH_RMII_TX_EN ETH1 -PG11 ETH_RMII_TX_EN ETH1 +PB11 ETH_RMII_TX_EN ETH1 PA7 ETH_RMII_CRS_DV ETH1 PA1 ETH_RMII_REF_CLK ETH1 define BOARD_PHY_ID MII_LAN8742A_ID +define BOARD_PHY_ADDRESS 0x0005 define BOARD_PHY_RMII include ../include/network_bootloader.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef.dat index 24f6c6e702b2b1..f277e0068d3bb9 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef.dat @@ -8,7 +8,7 @@ DEFAULTGPIO OUTPUT LOW PULLDOWN MCU STM32H7xx STM32H723xx # crystal frequency -OSCILLATOR_HZ 0 +OSCILLATOR_HZ 16000000 # setup build for a peripheral bootloader env AP_PERIPH 1 @@ -42,46 +42,60 @@ PA9 VBUS INPUT OPENDRAIN PD0 CAN1_RX CAN1 PD1 CAN1_TX CAN1 +#PD4 USART2_DE USART2 +PD5 USART2_TX USART2 +PD6 USART2_RX USART2 + PD8 USART3_TX USART3 PD9 USART3_RX USART3 PD12 USART3_RTS USART3 PD11 USART3_CTS USART3 # LEDs -PE3 LED_RED OUTPUT OPENDRAIN HIGH -PE4 LED_GREEN OUTPUT OPENDRAIN HIGH -PE5 LED_BLUE OUTPUT OPENDRAIN HIGH +PC0 LED_STT1 OUTPUT OPENDRAIN HIGH +PA4 LED_STT2 OUTPUT OPENDRAIN HIGH define HAL_LED_ON 0 -# use blue LED -define HAL_GPIO_PIN_LED HAL_GPIO_PIN_LED_BLUE +# use first LED +define HAL_GPIO_PIN_LED HAL_GPIO_PIN_LED_STT1 + +# Ethernet switch chip reset pin +PD13 GPIO_ETH_RST OUTPUT HIGH PULLUP +# CAN1 standby GPIO +PA0 GPIO_CAN_S OUTPUT LOW PULLUP + +# GPIO/PWMs +#PC6 TIM3_CH1 TIM3 PWM(1) GPIO(100) +#PB14 TIM12_CH1 TIM12 PWM(2) GPIO(101) +#PB15 TIM12_CH2 TIM12 PWM(3) GPIO(102) PC1 ETH_MDC ETH1 PA2 ETH_MDIO ETH1 PC4 ETH_RMII_RXD0 ETH1 PC5 ETH_RMII_RXD1 ETH1 -#PB12 ETH_RMII_TXD0 ETH1 -PG13 ETH_RMII_TXD0 ETH1 +PB12 ETH_RMII_TXD0 ETH1 PB13 ETH_RMII_TXD1 ETH1 -#PB11 ETH_RMII_TX_EN ETH1 -PG11 ETH_RMII_TX_EN ETH1 +PB11 ETH_RMII_TX_EN ETH1 PA7 ETH_RMII_CRS_DV ETH1 PA1 ETH_RMII_REF_CLK ETH1 define BOARD_PHY_ID MII_LAN8742A_ID +define BOARD_PHY_ADDRESS 0x0005 define BOARD_PHY_RMII -include ../include/network_PPPGW.inc +define HAL_PERIPH_ENABLE_NETWORKING +define AP_NETWORKING_MAX_INSTANCES 4 # allow load from USB SERIAL_ORDER OTG1 USART3 -# no ADC pins +define HAL_RCIN_THREAD_ENABLED 1 +define HAL_SCHEDULER_LOOP_DELAY_ENABLED 1 + define HAL_USE_ADC FALSE -define STM32_ADC_USE_ADC1 FALSE -define STM32_ADC_USE_ADC2 FALSE -define STM32_ADC_USE_ADC3 FALSE +define AP_SERIALMANAGER_REGISTER_ENABLED 1 +define AP_SCRIPTING_ENABLED 1 +define AP_FILESYSTEM_ROMFS_ENABLED 1 -define HAL_RCIN_THREAD_ENABLED 1 -define HAL_SCHEDULER_LOOP_DELAY_ENABLED 1 +include ../include/network_PPPGW.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/CUAV-7-Nano-pinout.png b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/CUAV-7-Nano-pinout.png new file mode 100644 index 00000000000000..fe2ea5b4827bc6 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/CUAV-7-Nano-pinout.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/README.md b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/README.md new file mode 100644 index 00000000000000..ffdce53289aef6 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/README.md @@ -0,0 +1,80 @@ +# CUAV-7-Nano Flight Controller + +The CUAV-7-Nano flight controller produced by [CUAV](https://www.cuav.net). + +## Features + +- STM32H753 microcontroller +- 2 IMUs: IIM42652 and BMI088 +- builtin IST8310 magnetometer +- 2 barometers: BMP581 and ICP20100 +- microSD card slot +- USB-TypeC port +- 1 ETH network interface +- 5 UARTs plus USB +- 14 PWM outputs +- 3 I2C ports +- 3 CAN ports (two of which share a CAN bus and one is an independent CAN bus) +- Analog RSSI input +- 3.3V/5V configurable PWM ouput voltage + +## Pinout + +![CUAV-7-Nano_interface_definition.png](CUAV-7-Nano-pinout.png) + +## UART Mapping + +- SERIAL0 -> USB +- SERIAL1 -> UART7 (TELEM1) +- SERIAL2 -> UART5 (TELEM2) +- SERIAL3 -> USART1 (GPS&SAFETY) +- SERIAL4 -> UART8 (GPS2) +- SERIAL5 -> USART3 (FMU DEBUG) + +The TELEM1 and TELEM2 ports have RTS/CTS pins, the other UARTs do not have RTS/CTS. All have full DMA capability. + +## RC Input + +RC input is configured on the RCIN pin, at one end of the servo rail, marked RCIN in the above diagram. All ArduPilot supported unidirectional RC protocols can be input here including PPM. For bi-directional or half-duplex protocols, such as CRSF/ELRS a full UART will have to be used. + +## PWM Output + +The CUAV-7-Nano flight controller supports up to 14 PWM outputs. + +The 14 PWM outputs are in 6 groups: + +- PWM 1-4 in group1 (TIM5) +- PWM 5 and 6 in group2 (TIM4) +- PWM 7 and 8 in group3 (TIM1) +- PWM 9, 10 and 11 in group4 (TIM8) +- PWM 12 in group5 (TIM15) +- PWM 13 and 14 in group6 (TIM12) + +Channels within the same group need to use the same output rate. If any channel in a group uses DShot then all channels in the group need to use DShot. Outputs 1-4 support BDShot. + +First first 8 PWM outputs of CUAV-7-Nano flight controller support switching between 3.3V voltage and 5V voltage output. It can be switched to 5V by setting GPIO 80 high by setting up a Voltage-Level Translator to control it. + +## Battery Monitoring + +The board has a dedicated power monitor ports on 6 pin connectors(POWER A). The correct battery setting parameters are dependent on the type of power brick which is connected. + +## Compass + +The CUAV-7-Nano has an IST8310 builtin compass, but due to interference the board is usually used with an external I2C compass as part of a GPS/Compass combination. + +## Analog inputs + +The CUAV-7-Nano has 6 analog inputs. + +- ADC Pin9 -> Battery Voltage +- ADC Pin8 -> Battery Current Sensor +- ADC Pin5 -> Vdd 5V supply sense +- ADC Pin13 -> ADC 3.3V Sense +- ADC Pin12 -> ADC 6.6V Sense +- ADC Pin10 -> RSSI voltage monitoring + +## Loading Firmware + +Firmware for these boards can be found at https://firmware.ardupilot.org in sub-folders labeled "CUAV-7-Nano". + +The board comes pre-installed with an ArduPilot compatible bootloader, allowing the loading of *.apj firmware files with any ArduPilot compatible ground station. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/defaults.parm new file mode 100644 index 00000000000000..f46b1f17c1a3e4 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/defaults.parm @@ -0,0 +1,2 @@ +INS_ACCEL_FILTER 10 +CAN_P1_DRIVER 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/hwdef-bl.dat new file mode 100644 index 00000000000000..62d0bae7107ef4 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/hwdef-bl.dat @@ -0,0 +1,101 @@ +# hw definition file for processing by chibios_hwdef.py +# for CUAV-7-Nano board + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# crystal frequency +OSCILLATOR_HZ 16000000 + +# board ID for firmware load +APJ_BOARD_ID 7000 + +# bootloader is installed at zero offset +FLASH_RESERVE_START_KB 0 + +# the location where the bootloader will put the firmware +FLASH_BOOTLOADER_LOAD_KB 128 + +# flash size +FLASH_SIZE_KB 2048 + +env OPTIMIZE -Os + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 UART7 UART5 USART3 + +# USB +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 +PA9 VBUS INPUT OPENDRAIN + +# pins for SWD debugging +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# CS pins +PI9 ICM42652_CS CS +PH5 BMI088_A_CS CS +PG2 BMI088_G_CS CS +PD15 BMP581_CS CS +PG7 FRAM_CS CS + +# telem1 +PE8 UART7_TX UART7 +PF6 UART7_RX UART7 + +# telem2 +PC12 UART5_TX UART5 +PD2 UART5_RX UART5 + +# debug uart +PD8 USART3_TX USART3 +PD9 USART3_RX USART3 + +# armed indication +PE7 nARMED OUTPUT HIGH + +# start peripheral power off +PG4 VDD_5V_PERIPH_EN OUTPUT HIGH +PG10 VDD_5V_HIPOWER_EN OUTPUT HIGH + +# LEDs +PE3 LED_RED OUTPUT LOW # red +PE4 LED_ACTIVITY OUTPUT LOW # green +PE5 LED_BOOTLOADER OUTPUT LOW # blue +define HAL_LED_ON 0 + +define HAL_USE_EMPTY_STORAGE 1 +define HAL_STORAGE_SIZE 16384 + +# enable DFU by default +ENABLE_DFU_BOOT 1 + +# support flashing from SD card: +# power enable pins +PC13 VDD_3V3_SD_CARD_EN OUTPUT HIGH + +# FATFS support: +define CH_CFG_USE_MEMCORE 1 +define CH_CFG_USE_HEAP 1 +define CH_CFG_USE_SEMAPHORES 0 +define CH_CFG_USE_MUTEXES 1 +define CH_CFG_USE_DYNAMIC 1 +define CH_CFG_USE_WAITEXIT 1 +define CH_CFG_USE_REGISTRY 1 + +# microSD support +PD6 SDMMC2_CK SDMMC2 +PD7 SDMMC2_CMD SDMMC2 +PB14 SDMMC2_D0 SDMMC2 +PB15 SDMMC2_D1 SDMMC2 +PG11 SDMMC2_D2 SDMMC2 +PB4 SDMMC2_D3 SDMMC2 +define FATFS_HAL_DEVICE SDCD2 + +DMA_PRIORITY SDMMC* USART6* ADC* UART* USART* SPI* TIM* + +# enable FAT filesystem support (needs a microSD defined via SDMMC) +define HAL_OS_FATFS_IO 1 + +define AP_BOOTLOADER_FLASH_FROM_SD_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/hwdef.dat new file mode 100644 index 00000000000000..eea49e50f0d618 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/hwdef.dat @@ -0,0 +1,294 @@ +# hw definition file for processing by chibios_hwdef.py +# for CUAV-7-Nano board + +# default to all pins low to avoid ESD issues +DEFAULTGPIO OUTPUT LOW PULLDOWN + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# crystal frequency +OSCILLATOR_HZ 16000000 + +# ChibiOS system timer +STM32_ST_USE_TIMER 2 + +# board ID for firmware load +APJ_BOARD_ID 7000 + +# bootloader takes first sector +FLASH_RESERVE_START_KB 128 + +# to be compatible with the px4 bootloader we need +# to use a different RAM_MAP +env USE_ALT_RAM_MAP 1 + +# flash size +FLASH_SIZE_KB 2048 + +# with 2M flash we can afford to optimize for speed +env OPTIMIZE -O2 + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 UART7 UART5 USART1 UART8 USART3 OTG2 + +# USB +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 +PA9 VBUS INPUT OPENDRAIN + +# pins for SWD debugging +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# telem1 +PE8 UART7_TX UART7 +PF6 UART7_RX UART7 +PF8 UART7_RTS UART7 +PE10 UART7_CTS UART7 + +# telem2 +PC8 UART5_RTS UART5 +PC9 UART5_CTS UART5 +PC12 UART5_TX UART5 +PD2 UART5_RX UART5 + +# GPS1 +PB6 USART1_TX USART1 +PB7 USART1_RX USART1 + +# GPS2 +PE0 UART8_RX UART8 +PE1 UART8_TX UART8 + +# debug uart +PD8 USART3_TX USART3 +PD9 USART3_RX USART3 + +# uart6, RX only, RC input, if no IOMCU +PC7 USART6_RX USART6 + +# ethernet +PC1 ETH_MDC ETH1 +PA2 ETH_MDIO ETH1 +PC4 ETH_RMII_RXD0 ETH1 +PC5 ETH_RMII_RXD1 ETH1 +PG13 ETH_RMII_TXD0 ETH1 +PG12 ETH_RMII_TXD1 ETH1 +PB11 ETH_RMII_TX_EN ETH1 +PA7 ETH_RMII_CRS_DV ETH1 +PA1 ETH_RMII_REF_CLK ETH1 +#PG15 ETH_POWER_EN ETH1 + +PG15 Ethernet_PWR_EN OUTPUT HIGH # disable power on ethernet + +define BOARD_PHY_ID MII_LAN8742A_ID +define BOARD_PHY_RMII + +# ADC +PA0 SCALED1_V3V3 ADC1 SCALE(2) +PA4 SCALED2_V3V3 ADC1 SCALE(2) +PB1 VDD_5V_SENS ADC1 SCALE(2) +PC0 RSSI_IN ADC1 SCALE(1) +PF12 FMU_SERVORAIL_VCC_SENS ADC1 SCALE(3) + +# analog in +PC2 BATT_VOLTAGE_SENS ADC1 SCALE(1) ANALOG(9) +PB0 BATT_CURRENT_SENS ADC1 SCALE(1) ANALOG(8) + +# pin7 on AD&IO, analog 12 +PF3 ADC3_6V6 ADC3 SCALE(2) ANALOG(12) + +# pin6 on AD&IO, analog 13 +PC3 ADC1_3V3 ADC1 SCALE(1) ANALOG(13) + +define HAL_BATT_VOLT_PIN 9 +define HAL_BATT_CURR_PIN 8 +define HAL_BATT_VOLT_SCALE 10.1 +define HAL_BATT_CURR_SCALE 17.0 + +# SPI1 - IIM42652 +PA5 SPI1_SCK SPI1 +PB5 SPI1_MOSI SPI1 +PG9 SPI1_MISO SPI1 +PI9 IIM42652_CS CS +PF2 IIM42652_DRDY INPUT + +# SPI2 - BMI088 +PI1 SPI2_SCK SPI2 +PI2 SPI2_MISO SPI2 +PI3 SPI2_MOSI SPI2 +PH5 BMI088_A_CS CS +PG2 BMI088_G_CS CS +PG3 BMI088_DRDY_GYR INPUT +PA10 BMI088_DRDY_ACC INPUT + +# SPI4 - BMP581 +PE12 SPI4_SCK SPI4 +PE13 SPI4_MISO SPI4 +PE14 SPI4_MOSI SPI4 +PD15 BMP581_CS CS +PG1 BMP581_DRDY INPUT + +# SPI5 - FRAM +PF7 SPI5_SCK SPI5 +PH7 SPI5_MISO SPI5 +PF11 SPI5_MOSI SPI5 +PG7 FRAM_CS CS + +# SPI devices +SPIDEV bmi088_g SPI2 DEVID1 BMI088_G_CS MODE3 10*MHZ 10*MHZ +SPIDEV bmi088_a SPI2 DEVID2 BMI088_A_CS MODE3 10*MHZ 10*MHZ +SPIDEV iim42652 SPI1 DEVID1 IIM42652_CS MODE3 2*MHZ 8*MHZ +SPIDEV ramtron SPI5 DEVID1 FRAM_CS MODE3 8*MHZ 8*MHZ +SPIDEV bmp581 SPI4 DEVID1 BMP581_CS MODE3 7.5*MHZ 12*MHZ + +# PWM output pins +PI0 TIM5_CH4 TIM5 PWM(1) GPIO(50) BIDIR +PH12 TIM5_CH3 TIM5 PWM(2) GPIO(51) +PH11 TIM5_CH2 TIM5 PWM(3) GPIO(52) BIDIR +PH10 TIM5_CH1 TIM5 PWM(4) GPIO(53) +PD13 TIM4_CH2 TIM4 PWM(5) GPIO(54) +PD14 TIM4_CH3 TIM4 PWM(6) GPIO(55) +PE11 TIM1_CH2 TIM1 PWM(7) GPIO(56) +PE9 TIM1_CH1 TIM1 PWM(8) GPIO(57) +PI6 TIM8_CH2 TIM8 PWM(9) GPIO(58) +PI7 TIM8_CH3 TIM8 PWM(10) GPIO(59) +PI5 TIM8_CH1 TIM8 PWM(11) GPIO(60) +PE6 TIM15_CH2 TIM15 PWM(12) GPIO(61) + +# we need to disable DMA on the last 2 FMU channels +# as timer 12 doesn't have a TIMn_UP DMA option +PH6 TIM12_CH1 TIM12 PWM(13) GPIO(62) NODMA +PH9 TIM12_CH2 TIM12 PWM(14) GPIO(63) NODMA + +# CAN bus +PD0 CAN1_RX CAN1 +PD1 CAN1_TX CAN1 + +PB12 CAN2_RX CAN2 +PB13 CAN2_TX CAN2 + +# control for silent (no output) for CAN +PE2 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(70) +PI8 GPIO_CAN2_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(71) + +# I2C buses + +# I2C1, GPS+MAG +PB9 I2C1_SDA I2C1 +PB8 I2C1_SCL I2C1 + +# I2C2, GPS2+MAG +PF1 I2C2_SCL I2C2 +PF0 I2C2_SDA I2C2 + +# I2C3, IST8310 +PA8 I2C3_SCL I2C3 +PH8 I2C3_SDA I2C3 + +# I2C4, ICM20100 +PF14 I2C4_SCL I2C4 +PF15 I2C4_SDA I2C4 +PG5 DRDY1_ICP20100 INPUT + +# order of I2C buses +I2C_ORDER I2C3 I2C4 I2C1 I2C2 +define HAL_I2C_INTERNAL_MASK 3 + +# armed indication +PE7 nARMED OUTPUT HIGH + +# power enable pins +PC13 VDD_3V3_SD_CARD_EN OUTPUT HIGH +PI11 VDD_3V3_SENSORS1_EN OUTPUT HIGH +PF4 VDD_3V3_SENSORS2_EN OUTPUT HIGH +PH2 VDD_3V3_SENSORS3_EN OUTPUT HIGH + +# start peripheral power off, then enable after init +# this prevents a problem with radios that use RTS for +# bootloader hold +PG10 VDD_5V_HIPOWER_EN OUTPUT HIGH +PG4 VDD_5V_PERIPH_EN OUTPUT HIGH + +# power sensing +PE15 VDD_5V_PERIPH_nOC INPUT PULLUP +PF13 VDD_5V_HIPOWER_nOC INPUT PULLUP + +# Pin for PWM Voltage Selection, 0 means 3.3v, 1 means 5v, 3.3v default +PG8 PWM_VOLT_SEL OUTPUT LOW GPIO(80) +define HAL_GPIO_PWM_VOLT_PIN 80 +define HAL_GPIO_PWM_VOLT_3v3 0 + +# microSD support +PD6 SDMMC2_CK SDMMC2 +PD7 SDMMC2_CMD SDMMC2 +PB14 SDMMC2_D0 SDMMC2 +PB15 SDMMC2_D1 SDMMC2 +PG11 SDMMC2_D2 SDMMC2 +PB4 SDMMC2_D3 SDMMC2 +define FATFS_HAL_DEVICE SDCD2 + +# safety +PD10 LED_SAFETY OUTPUT +PF5 SAFETY_IN INPUT PULLDOWN + +# LEDs +PE3 LED_RED OUTPUT GPIO(90) LOW +PE4 LED_GREEN OUTPUT GPIO(91) LOW +PE5 LED_BLUE OUTPUT GPIO(92) LOW + +# setup for "pixracer" RGB LEDs +define AP_NOTIFY_GPIO_LED_RGB_RED_PIN 90 +define AP_NOTIFY_GPIO_LED_RGB_GREEN_PIN 91 +define AP_NOTIFY_GPIO_LED_RGB_BLUE_PIN 92 + +define AP_NOTIFY_GPIO_LED_RGB_ENABLED 1 + +# ID pins +PG0 HW_VER_REV_DRIVE OUTPUT LOW +# PH3 HW_VER_SENS ADC3 SCALE(1) +# PH4 HW_REV_SENS ADC3 SCALE(1) + +# PWM output for buzzer +PF9 TIM14_CH1 TIM14 GPIO(77) ALARM + +# RC input +PC6 TIM3_CH1 TIM3 RCININT PULLDOWN LOW + +# other I2C devices +# 24LC64T eeprom 64Kbit, address 0x50 on I2C4 + +BARO BMP581 SPI:bmp581 +BARO ICP201XX I2C:1:0x63 + +# compass +define HAL_PROBE_EXTERNAL_I2C_COMPASSES +define HAL_SKIP_AUTO_INTERNAL_I2C_PROBE +define HAL_COMPASS_DISABLE_IST8310_INTERNAL_PROBE +define AP_COMPASS_IST8310_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_90 +COMPASS IST8310 I2C:0:0x0E false ROTATION_ROLL_90_YAW_90 +COMPASS IST8310 I2C:ALL_EXTERNAL:0x0E true ROTATION_ROLL_180_YAW_90 + +# IMUs +IMU Invensensev3 SPI:iim42652 ROTATION_ROLL_90_YAW_90 +IMU BMI088 SPI:bmi088_a SPI:bmi088_g ROTATION_PITCH_90 + +define HAL_DEFAULT_INS_FAST_SAMPLE 3 + +# enable RAMTROM parameter storage +define HAL_STORAGE_SIZE 32768 +define HAL_WITH_RAMTRON 1 + +# allow to have have a dedicated safety switch pin +define HAL_HAVE_SAFETY_SWITCH 1 + +DMA_PRIORITY TIM5* SDMMC* USART6* ADC* UART* USART* SPI* TIM* + +# enable FAT filesystem support (needs a microSD defined via SDMMC) +define HAL_OS_FATFS_IO 1 + +# enable DFU reboot for installing bootloader +# note that if firmware is build with --secure-bl then DFU is +# disabled +ENABLE_DFU_BOOT 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/defaults.parm index cdd83a642efe87..22f785c1a3b188 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/defaults.parm @@ -6,3 +6,22 @@ ADSB_TYPE 1 SERIAL5_BAUD 57 SERIAL5_PROTOCOL 1 EK3_PRIMARY 1 + +NET_ENABLE 1 +NET_OPTIONS 1 +NET_DHCP 0 +NET_P1_TYPE 1 +NET_P1_PROTOCOL 2 +NET_P1_IP0 192 +NET_P1_IP1 168 +NET_P1_IP2 144 +NET_P1_IP3 10 +NET_P1_PORT 14553 + +NET_P2_TYPE 4 +NET_P2_PROTOCOL 2 +NET_P2_IP0 192 +NET_P2_IP1 168 +NET_P2_IP2 144 +NET_P2_IP3 100 +NET_P2_PORT 14554 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat index d6e7dfa8e38aad..5bd8d3c71c6394 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat @@ -274,17 +274,20 @@ PG15 USART6_CTS USART6 PG8 USART6_RTS USART6 # primary <-> secondary -PE7 UART7_RX UART7 -PE8 UART7_TX UART7 +PE7 UART7_RX UART7 SPEED_VERYLOW LOW_NOISE +PE8 UART7_TX UART7 SPEED_VERYLOW # order of UARTs (and USB) -SERIAL_ORDER OTG1 USART2 USART6 USART3 UART4 UART8 EMPTY UART7 +SERIAL_ORDER OTG1 USART2 USART6 USART3 UART4 UART8 OTG2 UART7 EXT_FLASH_SIZE_MB 32 INT_FLASH_PRIMARY 1 -# forward Serial traffic from USB OTG2 to Serial7(UART7) -define HAL_FORWARD_OTG2_SERIAL 7 -define HAL_HAVE_DUAL_USB_CDC 1 -define DEFAULT_SERIAL7_PROTOCOL SerialProtocol_MAVLink2 -define DEFAULT_SERIAL7_BAUD 2000000 +# set protocol for UART7 to SerialProtocol_PPP +define DEFAULT_SERIAL7_PROTOCOL SerialProtocol_PPP +define DEFAULT_SERIAL7_BAUD 8000000 + +define AP_NETWORKING_DEFAULT_STATIC_IP_ADDR "192.168.144.100" +define AP_NETWORKING_DEFAULT_STATIC_NETMASK "255.255.255.0" +define AP_NETWORKING_DEFAULT_STATIC_GW_ADDR "192.168.144.11" +define AP_NETWORKING_BACKEND_PPP 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/defaults.parm index 3be7684ba40cad..342b61b6d584b8 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/defaults.parm @@ -1,2 +1,12 @@ SERIAL3_OPTIONS 8 SERIAL4_OPTIONS 8 +NET_ENABLE 1 +NET_P1_TYPE 3 +NET_P1_PROTOCOL 2 +NET_P1_IP0 192 +NET_P1_IP1 168 +NET_P1_IP2 144 +NET_P1_IP3 100 +NET_P1_PORT 14554 + +SYSID_THISMAV 2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/hwdef.dat index bd77ac79c934f3..e24d8c5299effb 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/hwdef.dat @@ -7,6 +7,8 @@ MCU STM32H7xx STM32H757xx define CORE_CM7 define SMPS_PWR +env OPTIMIZE -Os + # crystal frequency OSCILLATOR_HZ 24000000 @@ -39,7 +41,8 @@ PA6 RSSI_IN ADC1 SCALE(2) # CAN config PB14 GPIOCAN2_TERM OUTPUT HIGH - +PC12 GPIOCAN1_SHUTDOWN OUTPUT LOW +PF1 GPIOCAN2_SHUTDOWN OUTPUT LOW PA12 CAN1_TX CAN1 PB8 CAN1_RX CAN1 @@ -118,8 +121,8 @@ PA7 HP_UNIDIR_ENABLED OUTPUT HIGH GPIO(5) # UART connected to FMU, uses DMA -PE7 UART7_RX UART7 SPEED_HIGH -PE8 UART7_TX UART7 SPEED_HIGH +PE7 UART7_RX UART7 SPEED_VERYLOW LOW_NOISE +PE8 UART7_TX UART7 SPEED_VERYLOW # UART for SBUS out PC7 USART6_RX USART6 SPEED_HIGH LOW @@ -144,13 +147,16 @@ PD4 USART2_RTS USART2 SPEED_HIGH PD3 USART2_CTS USART2 SPEED_HIGH # order of UARTs -SERIAL_ORDER UART7 UART8 USART3 USART6 UART4 USART2 +SERIAL_ORDER UART8 UART7 USART3 USART6 UART4 USART2 # use 2 MBaud when talking to primary controller -define DEFAULT_SERIAL0_BAUD 2000000 +define DEFAULT_SERIAL1_BAUD 8000000 +define DEFAULT_SERIAL1_PROTOCOL SerialProtocol_PPP define DEFAULT_SERIAL3_PROTOCOL SerialProtocol_Sbus1 define DEFAULT_SERIAL4_PROTOCOL SerialProtocol_RCIN +define AP_NETWORKING_BACKEND_PPP 1 + # only use pulse input for PPM, other protocols # are on serial define HAL_RCIN_PULSE_PPM_ONLY diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonF427/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonF427/hwdef.dat index 15306778fb10bb..5df193ee03de25 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonF427/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonF427/hwdef.dat @@ -66,7 +66,7 @@ PD2 UART5_RX UART5 NODMA PD0 CAN1_RX CAN1 PD1 CAN1_TX CAN1 -# SPI1 for IMU OSD +# SPI1 for IMU PA5 SPI1_SCK SPI1 PA6 SPI1_MISO SPI1 PA7 SPI1_MOSI SPI1 @@ -74,7 +74,6 @@ PB0 ICM20689_CS CS PB1 ICM42605_CS CS PB2 ICM20649_CS CS PA4 RM3100_CS CS -PE10 MAX7456_CS CS # SPI bus for dataflash AND SD PB13 SPI2_SCK SPI2 @@ -93,7 +92,6 @@ SPIDEV icm20689 SPI1 DEVID1 ICM20689_CS MODE3 2*MHZ 8*MHZ SPIDEV icm20649 SPI1 DEVID2 ICM20649_CS MODE3 2*MHZ 8*MHZ SPIDEV icm42605 SPI1 DEVID3 ICM42605_CS MODE3 2*MHZ 8*MHZ SPIDEV rm3100 SPI1 DEVID4 RM3100_CS MODE3 1*MHZ 1*MHZ -SPIDEV osd SPI1 DEVID5 MAX7456_CS MODE0 10*MHZ 10*MHZ SPIDEV ramtron SPI2 DEVID1 FRAM_CS MODE3 8*MHZ 8*MHZ SPIDEV sdcard SPI2 DEVID2 FLASH_CS MODE0 1*MHZ 8*MHZ @@ -187,7 +185,3 @@ PE7 LED_SAFETY OUTPUT PE8 SAFETY_IN INPUT PULLDOWN PE5 TIM9_CH1 TIM9 ALARM -# setup for OSD -define OSD_ENABLED 1 -define HAL_OSD_TYPE_DEFAULT 1 -ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonH743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonH743/hwdef.dat index 4d81362bd1f951..9e1c637b83bcc2 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonH743/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonH743/hwdef.dat @@ -73,7 +73,7 @@ PD1 CAN1_TX CAN1 PB12 CAN2_RX CAN2 PB13 CAN2_TX CAN2 -# SPI1 for IMU OSD +# SPI1 for IMU PA5 SPI1_SCK SPI1 PA6 SPI1_MISO SPI1 PA7 SPI1_MOSI SPI1 @@ -81,7 +81,6 @@ PB0 ICM20689_CS CS PB1 ICM42605_CS CS PB2 ICM20649_CS CS PA4 RM3100_CS CS -PA8 MAX7456_CS CS # SPI bus for dataflash AND SD PD3 SPI2_SCK SPI2 @@ -100,7 +99,6 @@ SPIDEV icm20689 SPI1 DEVID1 ICM20689_CS MODE3 2*MHZ 8*MHZ SPIDEV icm20649 SPI1 DEVID2 ICM20649_CS MODE3 2*MHZ 8*MHZ SPIDEV icm42605 SPI1 DEVID3 ICM42605_CS MODE3 2*MHZ 8*MHZ SPIDEV rm3100 SPI1 DEVID4 RM3100_CS MODE3 1*MHZ 1*MHZ -SPIDEV osd SPI1 DEVID5 MAX7456_CS MODE0 10*MHZ 10*MHZ SPIDEV ramtron SPI2 DEVID1 FRAM_CS MODE3 8*MHZ 8*MHZ SPIDEV sdcard SPI2 DEVID2 FLASH_CS MODE0 1*MHZ 8*MHZ @@ -195,8 +193,3 @@ PE8 SAFETY_IN INPUT PULLDOWN # PWM output for buzzer PE5 TIM15_CH1 TIM15 GPIO(77) ALARM - -# setup for OSD -define OSD_ENABLED 1 -define HAL_OSD_TYPE_DEFAULT 1 -ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/hwdef.dat index 1543f2b3379a81..0624b4e0680e4f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/hwdef.dat @@ -162,10 +162,6 @@ ROMFS_WILDCARD libraries/AP_OSD/fonts/font0.bin # define HAL_GYROFFT_ENABLED 1 # define AP_OAPATHPLANNER_ENABLED 0 -# EK2 options (disabled by default) -# define HAL_NAVEKF2_AVAILABLE 1 -# define HAL_NAVEKF3_AVAILABLE 0 - # save some flash include ../include/minimize_fpv_osd.inc include ../include/no_bootloader_DFU.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/README.md b/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/README.md similarity index 92% rename from libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/README.md rename to libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/README.md index 1264790660f38f..9cb60c81804dcc 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/README.md +++ b/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/README.md @@ -7,7 +7,7 @@ The iFlight 2RAW H743 is a flight controller produced by [iFlight](https://shop. - MCU - STM32H743 32-bit processor running at 480 MHz - IMU - ICM42688P - Barometer - SPL06-001 - - Onboard Flash: 1GBit (not useable by ArduPilot) + - Onboard Flash: 1GBit exposed as microSD card - 6x UARTs - 9x PWM Outputs (8 Motor Output, 1 LED) - Battery input voltage: 2S-6S @@ -88,6 +88,11 @@ The iFlight 2RAW H743 does not have a builtin compass, but you can attach an ext GPIO 81 controls the VTX BEC output to pins marked "10V". Setting this GPIO low removes voltage supply to pins. By default RELAY2 is configured to control this pin and sets the GPIO high. +## Logging + +Logging is via a 1GBit flash chip exposed via a microSD interface. In order to be used you must format the card using mission planner +after the first time you have loaded the firmware + ## Loading Firmware Initial firmware load can be done with DFU by plugging in USB with the diff --git a/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/Thunder-H7-1.png b/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/Thunder-H7-1.png similarity index 100% rename from libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/Thunder-H7-1.png rename to libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/Thunder-H7-1.png diff --git a/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/Thunder-H7-2.png b/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/Thunder-H7-2.png similarity index 100% rename from libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/Thunder-H7-2.png rename to libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/Thunder-H7-2.png diff --git a/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/defaults.parm similarity index 100% rename from libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/defaults.parm rename to libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/defaults.parm diff --git a/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/hwdef-bl.dat similarity index 100% rename from libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/hwdef-bl.dat rename to libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/hwdef-bl.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/hwdef.dat similarity index 95% rename from libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/hwdef.dat rename to libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/hwdef.dat index 800775bb31e304..d90fe2e67904ec 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/hwdef.dat @@ -36,6 +36,11 @@ PB13 SPI2_SCK SPI2 PB14 SPI2_MISO SPI2 PB15 SPI2_MOSI SPI2 +# SPI3 +PC12 SPI3_MOSI SPI3 +PC11 SPI3_MISO SPI3 +PC10 SPI3_SCK SPI3 + # Chip select pins PA15 SDCARD1_CS CS PB12 OSD1_CS CS @@ -123,10 +128,11 @@ define HAL_GPIO_B_LED_PIN 91 BARO SPL06 I2C:1:0x76 -# IMU setup +# SPI setup +SPIDEV imu1 SPI1 DEVID1 GYRO1_CS MODE3 1*MHZ 16*MHZ +SPIDEV sdcard SPI3 DEVID1 SDCARD1_CS MODE0 400*KHZ 25*MHZ # IMU setup -SPIDEV imu1 SPI1 DEVID1 GYRO1_CS MODE3 1*MHZ 16*MHZ IMU Invensensev3 SPI:imu1 ROTATION_ROLL_180 DMA_NOSHARE TIM3_UP TIM5_UP TIM4_UP SPI1* @@ -141,3 +147,4 @@ define HAL_COMPASS_AUTO_ROT_DEFAULT 2 define HAL_DEFAULT_INS_FAST_SAMPLE 1 # Motor order implies Betaflight/X for standard ESCs define HAL_FRAME_TYPE_DEFAULT 12 +define HAL_OS_FATFS_IO 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat index a3aff9a67c3778..f063295ea6d73d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat @@ -172,14 +172,13 @@ PC13 PINIO1 OUTPUT GPIO(81) LOW include ../include/minimize_fpv_osd.inc undef AP_CAMERA_MOUNT_ENABLED -undef AP_LANDINGGEAR_ENABLED undef HAL_MOUNT_ENABLED undef HAL_MOUNT_SERVO_ENABLED undef QAUTOTUNE_ENABLED define AP_CAMERA_MOUNT_ENABLED 1 -define AP_LANDINGGEAR_ENABLED 1 define HAL_MOUNT_ENABLED 1 +define AP_MOUNT_BACKEND_DEFAULT_ENABLED 0 define HAL_MOUNT_SERVO_ENABLED 1 define QAUTOTUNE_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index e01e4283a45d30..e59b850f75eac9 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -1865,6 +1865,7 @@ def write_UART_config(self, f): OTG2_index = None devlist = [] have_rts_cts = False + have_low_noise = False crash_uart = None # write config for CrashCatcher UART @@ -1878,6 +1879,14 @@ def write_UART_config(self, f): f.write('#define IRQ_DISABLE_HAL_CRASH_SERIAL_PORT() nvicDisableVector(STM32_%s_NUMBER)\n' % crash_uart) f.write('#define RCC_RESET_HAL_CRASH_SERIAL_PORT() rccReset%s(); rccEnable%s(true)\n' % (crash_uart, crash_uart)) f.write('#define HAL_CRASH_SERIAL_PORT_CLOCK STM32_%sCLK\n' % crash_uart) + # check if we have a UART with a low noise RX pin + for num, dev in enumerate(serial_list): + if not dev.startswith('UART') and not dev.startswith('USART'): + continue + rx_port = dev + '_RX' + if rx_port in self.bylabel and self.bylabel[rx_port].has_extra('LOW_NOISE'): + have_low_noise = True + break for num, dev in enumerate(serial_list): if dev.startswith('UART'): n = int(dev[4:]) @@ -1904,12 +1913,20 @@ def write_UART_config(self, f): if dev.startswith('OTG2'): f.write( - '#define HAL_%s_CONFIG {(BaseSequentialStream*) &SDU2, 2, true, false, 0, 0, false, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, UINT8_MAX}\n' % dev) # noqa + '#define HAL_%s_CONFIG {(BaseSequentialStream*) &SDU2, 2, true, false, 0, 0, false, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, UINT8_MAX,' % dev) # noqa + if have_low_noise: + f.write('false}\n') + else: + f.write('}\n') OTG2_index = serial_list.index(dev) self.dual_USB_enabled = True elif dev.startswith('OTG'): f.write( - '#define HAL_%s_CONFIG {(BaseSequentialStream*) &SDU1, 1, true, false, 0, 0, false, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, UINT8_MAX}\n' % dev) # noqa + '#define HAL_%s_CONFIG {(BaseSequentialStream*) &SDU1, 1, true, false, 0, 0, false, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, UINT8_MAX,' % dev) # noqa + if have_low_noise: + f.write('false}\n') + else: + f.write('}\n') else: need_uart_driver = True f.write( @@ -1948,9 +1965,17 @@ def get_RTS_alt_function(): if s not in lib.AltFunction_map: return "UINT8_MAX" return lib.AltFunction_map[s] + if have_low_noise: + low_noise = 'false' + rx_port = dev + '_RX' + if rx_port in self.bylabel and self.bylabel[rx_port].has_extra('LOW_NOISE'): + low_noise = 'true' + f.write("%s, %s}\n" % (get_RTS_alt_function(), low_noise)) + else: + f.write("%s}\n" % get_RTS_alt_function()) - f.write("%s}\n" % get_RTS_alt_function()) - + if have_low_noise: + f.write('#define HAL_HAVE_LOW_NOISE_UART 1\n') if have_rts_cts: f.write('#define AP_FEATURE_RTSCTS 1\n') if OTG2_index is not None: diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index ffe9c7f38ed7d2..6d254dccec9071 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -736,6 +736,14 @@ bool AP_InertialSensor::register_gyro(uint8_t &instance, uint16_t raw_sample_rat return false; } + // Loop over the existing instances and check if the instance already exists + for (uint8_t instance_to_check = 0; instance_to_check < _gyro_count; instance_to_check++) { + if ((uint32_t)_gyro_id(instance_to_check) == id) { + // if it does, then bail + return false; + } + } + _gyro_raw_sample_rates[_gyro_count] = raw_sample_rate_hz; _gyro_over_sampling[_gyro_count] = 1; _gyro_raw_sampling_multiplier[_gyro_count] = INT16_MAX/radians(2000); @@ -796,6 +804,14 @@ bool AP_InertialSensor::register_accel(uint8_t &instance, uint16_t raw_sample_ra return false; } + // Loop over the existing instances and check if the instance already exists + for (uint8_t instance_to_check = 0; instance_to_check < _accel_count; instance_to_check++) { + if ((uint32_t)_accel_id(instance_to_check) == id) { + // if it does, then bail + return false; + } + } + _accel_raw_sample_rates[_accel_count] = raw_sample_rate_hz; _accel_over_sampling[_accel_count] = 1; _accel_raw_sampling_multiplier[_accel_count] = INT16_MAX/(16*GRAVITY_MSS); diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp index bd64d62a02c8be..0dfd75032b9021 100644 --- a/libraries/AP_Math/matrix3.cpp +++ b/libraries/AP_Math/matrix3.cpp @@ -21,7 +21,7 @@ #include "AP_Math.h" // create a rotation matrix given some euler angles -// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf +// this is based on https://github.com/ArduPilot/Datasheets/blob/main/References/EulerAngles.pdf template void Matrix3::from_euler(T roll, T pitch, T yaw) { @@ -44,7 +44,7 @@ void Matrix3::from_euler(T roll, T pitch, T yaw) } // calculate euler angles from a rotation matrix -// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf +// this is based on https://github.com/ArduPilot/Datasheets/blob/main/References/EulerAngles.pdf template void Matrix3::to_euler(T *roll, T *pitch, T *yaw) const { diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h index ab861e222efe4c..02e18699a53506 100644 --- a/libraries/AP_Math/matrix3.h +++ b/libraries/AP_Math/matrix3.h @@ -235,13 +235,17 @@ class Matrix3 { return a.is_nan() || b.is_nan() || c.is_nan(); } - // create a rotation matrix from Euler angles + /* + create a rotation matrix from Euler angles in 321 euler ordering + */ void from_euler(T roll, T pitch, T yaw); - // create eulers from a rotation matrix. - // roll is from -Pi to Pi - // pitch is from -Pi/2 to Pi/2 - // yaw is from -Pi to Pi + /* create eulers from a rotation matrix. + roll is from -Pi to Pi + pitch is from -Pi/2 to Pi/2 + yaw is from -Pi to Pi + euler order is 321 + */ void to_euler(T *roll, T *pitch, T *yaw) const; // create matrix from rotation enum diff --git a/libraries/AP_Math/quaternion.h b/libraries/AP_Math/quaternion.h index 1c47540c678ae8..00f29d08989d2e 100644 --- a/libraries/AP_Math/quaternion.h +++ b/libraries/AP_Math/quaternion.h @@ -81,11 +81,11 @@ class QuaternionT { // convert a vector from earth to body frame void earth_to_body(Vector3 &v) const; - // create a quaternion from Euler angles + // create a quaternion from Euler angles using 321 euler ordering void from_euler(T roll, T pitch, T yaw); void from_euler(const Vector3 &v); - // create a quaternion from Euler angles applied in yaw, roll, pitch order + // create a quaternion from Euler angles applied in yaw, roll, pitch order (312) // instead of the normal yaw, pitch, roll order void from_vector312(T roll, T pitch, T yaw); @@ -129,7 +129,7 @@ class QuaternionT { // get euler yaw angle in radians T get_euler_yaw() const; - // create eulers (in radians) from a quaternion + // create eulers (in radians) from a quaternion, using 321 ordering void to_euler(float &roll, float &pitch, float &yaw) const; void to_euler(Vector3f &rpy) const { to_euler(rpy.x, rpy.y, rpy.z); @@ -139,7 +139,7 @@ class QuaternionT { to_euler(rpy.x, rpy.y, rpy.z); } - // create eulers from a quaternion + // create eulers from a quaternion with 312 ordering Vector3 to_vector312(void) const; T length_squared(void) const; diff --git a/libraries/AP_Math/tests/test_rotations.cpp b/libraries/AP_Math/tests/test_rotations.cpp index 15fdfa6238055a..d9de8f82e59dfe 100644 --- a/libraries/AP_Math/tests/test_rotations.cpp +++ b/libraries/AP_Math/tests/test_rotations.cpp @@ -189,5 +189,83 @@ TEST(RotationsTest, TestFailedGetLinux) } }*/ +/* + rotate a matrix using a give order, specified as a string + for example "321" + */ +static void rotate_ordered(Matrix3f &m, const char *order, + const float roll_deg, + const float pitch_deg, + const float yaw_deg) +{ + while (*order) { + Matrix3f m2; + switch (*order) { + case '1': + m2.from_euler(radians(roll_deg), 0, 0); + break; + case '2': + m2.from_euler(0, radians(pitch_deg), 0); + break; + case '3': + m2.from_euler(0, 0, radians(yaw_deg)); + break; + } + m *= m2; + order++; + } +} + +/* + test the two euler orders we use in ArduPilot + */ +TEST(RotationsTest, TestEulerOrder) +{ + const float roll_deg = 20; + const float pitch_deg = 31; + const float yaw_deg = 72; + float r, p, y; + Vector3f v; + + Matrix3f m; + + // apply in 321 ordering + m.identity(); + rotate_ordered(m, "321", roll_deg, pitch_deg, yaw_deg); + + // get using to_euler + m.to_euler(&r, &p, &y); + + EXPECT_FLOAT_EQ(degrees(r), roll_deg); + EXPECT_FLOAT_EQ(degrees(p), pitch_deg); + EXPECT_FLOAT_EQ(degrees(y), yaw_deg); + + // get using to_euler312, should not match + v = m.to_euler312(); + + EXPECT_GE(fabsf(degrees(v.x)-roll_deg), 1); + EXPECT_GE(fabsf(degrees(v.y)-pitch_deg), 1); + EXPECT_GE(fabsf(degrees(v.z)-yaw_deg), 1); + + // apply in 312 ordering + m.identity(); + rotate_ordered(m, "312", roll_deg, pitch_deg, yaw_deg); + + // get using to_euler312 + v = m.to_euler312(); + + EXPECT_FLOAT_EQ(degrees(v.x), roll_deg); + EXPECT_FLOAT_EQ(degrees(v.y), pitch_deg); + EXPECT_FLOAT_EQ(degrees(v.z), yaw_deg); + + // get using to_euler, should not match + m.to_euler(&r, &p, &y); + + EXPECT_GE(fabsf(degrees(r)-roll_deg), 1); + EXPECT_GE(fabsf(degrees(p)-pitch_deg), 1); + EXPECT_GE(fabsf(degrees(y)-yaw_deg), 1); +} + + AP_GTEST_PANIC() AP_GTEST_MAIN() diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 3e138e7cacb403..ea441be219ba89 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -95,7 +95,7 @@ void AP_Mission::init() init_jump_tracking(); // If Mission Clear bit is set then it should clear the mission, otherwise retain the mission. - if (AP_MISSION_MASK_MISSION_CLEAR & _options) { + if (option_is_set(Option::CLEAR_ON_BOOT)) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Clearing Mission"); clear(); } @@ -1821,7 +1821,7 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c #if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED case MAV_CMD_NAV_PAYLOAD_PLACE: - packet.param1 = cmd.p1/100.0f; // copy max-descend parameter (cm->m) + packet.param1 = cmd.p1*0.01f; // copy max-descend parameter (cm->m) break; #endif @@ -2502,7 +2502,7 @@ bool AP_Mission::is_best_land_sequence(const Location ¤t_loc) } // check if MIS_OPTIONS bit set to allow distance calculation to be done - if (!(_options & AP_MISSION_MASK_DIST_TO_LAND_CALC)) { + if (!option_is_set(Option::FAILSAFE_TO_BEST_LANDING)) { return false; } diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 69f8fe2b46b979..32bb2fa77dd8de 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -45,9 +45,6 @@ #define AP_MISSION_RESTART_DEFAULT 0 // resume the mission from the last command run by default #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_DIST_TO_LAND_CALC (1<<1) // Allow distance to best landing calculation to be run on failsafe -#define AP_MISSION_MASK_CONTINUE_AFTER_LAND (1<<2) // Allow mission to continue after land #define AP_MISSION_MAX_WP_HISTORY 7 // The maximum number of previous wp commands that will be stored from the active missions history #define LAST_WP_PASSED (AP_MISSION_MAX_WP_HISTORY-2) @@ -732,14 +729,23 @@ class AP_Mission void reset_wp_history(void); /* - return true if MIS_OPTIONS is set to allow continue of mission - logic after a land and the next waypoint is a takeoff. If this + Option::FailsafeToBestLanding - continue mission + logic after a land if the next waypoint is a takeoff. If this is false then after a landing is complete the vehicle should disarm and mission logic should stop */ + enum class Option { + CLEAR_ON_BOOT = 0, // clear mission on vehicle boot + FAILSAFE_TO_BEST_LANDING = 1, // on failsafe, find fastest path along mission home + CONTINUE_AFTER_LAND = 2, // continue running mission (do not disarm) after land if takeoff is next waypoint + }; + bool option_is_set(Option option) const { + return (_options.get() & (uint16_t)option) != 0; + } + bool continue_after_land_check_for_takeoff(void); bool continue_after_land(void) const { - return (_options.get() & AP_MISSION_MASK_CONTINUE_AFTER_LAND) != 0; + return option_is_set(Option::CONTINUE_AFTER_LAND); } // user settable parameters diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 777256cb050a97..f3225bbd63880c 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -46,7 +46,7 @@ AP_Mount::AP_Mount() } _singleton = this; - AP_Param::setup_object_defaults(this, var_info); + AP_Param::setup_object_defaults(this, var_info); } // init - detect and initialise all mounts @@ -184,6 +184,8 @@ void AP_Mount::init() set_mode_to_default(instance); } } + + (void)serial_instance; } // update - give mount opportunity to update servos. should be called at 10hz or higher diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index bb2ee5ee025fc8..2158eb6dad707f 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -916,39 +916,24 @@ bool AP_Mount_Siyi::set_lens(uint8_t lens) } // maps lens to siyi camera image type so that lens of 0, 1, 2 are more useful - CameraImageType cam_image_type = CameraImageType::MAIN_ZOOM_SUB_THERMAL; - switch (lens) { - case 0: - cam_image_type = CameraImageType::MAIN_ZOOM_SUB_THERMAL; // 3 - break; - case 1: - cam_image_type = CameraImageType::MAIN_WIDEANGLE_SUB_THERMAL; // 5 - break; - case 2: - cam_image_type = CameraImageType::MAIN_THERMAL_SUB_ZOOM; // 7 - break; - case 3: - cam_image_type = CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE; // 0 - break; - case 4: - cam_image_type = CameraImageType::MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM; // 1 - break; - case 5: - cam_image_type = CameraImageType::MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL; // 2 - break; - case 6: - cam_image_type = CameraImageType::MAIN_ZOOM_SUB_WIDEANGLE; // 4 - break; - case 7: - cam_image_type = CameraImageType::MAIN_WIDEANGLE_SUB_ZOOM; // 6 - break; - case 8: - cam_image_type = CameraImageType::MAIN_THERMAL_SUB_WIDEANGLE; // 8 - break; + static const CameraImageType cam_image_type_table[] { + CameraImageType::MAIN_ZOOM_SUB_THERMAL, // 3 + CameraImageType::MAIN_WIDEANGLE_SUB_THERMAL, // 5 + CameraImageType::MAIN_THERMAL_SUB_ZOOM, // 7 + CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE, // 0 + CameraImageType::MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM, // 1 + CameraImageType::MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL, // 2 + CameraImageType::MAIN_ZOOM_SUB_WIDEANGLE, // 4 + CameraImageType::MAIN_WIDEANGLE_SUB_ZOOM, // 6 + CameraImageType::MAIN_THERMAL_SUB_WIDEANGLE, // 8 + }; + + if (lens >= ARRAY_SIZE(cam_image_type_table)) { + return false; } // send desired image type to camera - return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)cam_image_type); + return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)cam_image_type_table[lens]); } // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index 6c0f63509e17db..a2da37fac58805 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/libraries/AP_Mount/AP_Mount_config.h @@ -7,7 +7,9 @@ #define HAL_MOUNT_ENABLED 1 #endif +#ifndef AP_MOUNT_BACKEND_DEFAULT_ENABLED #define AP_MOUNT_BACKEND_DEFAULT_ENABLED HAL_MOUNT_ENABLED +#endif #ifndef HAL_MOUNT_ALEXMOS_ENABLED #define HAL_MOUNT_ALEXMOS_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 390dfceb8705ae..67ac2a57943a4c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -294,6 +294,7 @@ void NavEKF3_core::SelectMagFusion() have_fused_gps_yaw = true; lastSynthYawTime_ms = imuSampleTime_ms; last_gps_yaw_fuse_ms = imuSampleTime_ms; + recordYawResetsCompleted(); } else if (tiltAlignComplete && yawAlignComplete) { have_fused_gps_yaw = fuseEulerYaw(yawFusionMethod::GPS); if (have_fused_gps_yaw) { diff --git a/libraries/AP_Networking/AP_Networking_PPP.cpp b/libraries/AP_Networking/AP_Networking_PPP.cpp index 6bcf3651bba098..5d1128a781f2a7 100644 --- a/libraries/AP_Networking/AP_Networking_PPP.cpp +++ b/libraries/AP_Networking/AP_Networking_PPP.cpp @@ -25,6 +25,9 @@ extern const AP_HAL::HAL& hal; #define LWIP_TCPIP_UNLOCK() #endif +#define PPP_DEBUG_TX 0 +#define PPP_DEBUG_RX 0 + /* output some data to the uart */ @@ -34,6 +37,27 @@ uint32_t AP_Networking_PPP::ppp_output_cb(ppp_pcb *pcb, const void *data, uint32 LWIP_UNUSED_ARG(pcb); uint32_t remaining = len; const uint8_t *ptr = (const uint8_t *)data; +#if PPP_DEBUG_TX + bool flag_end = false; + if (ptr[len-1] == 0x7E) { + flag_end = true; + remaining--; + } + if (ptr[0] == 0x7E) { + // send byte size + if (pkt_size > 0) { + printf("PPP: tx[%lu] %u\n", tx_index++, pkt_size); + } + // dump the packet + if (!(tx_index % 10)) { + for (uint32_t i = 0; i < pkt_size; i++) { + printf(" %02X", tx_bytes[i]); + } + printf("\n"); + } + pkt_size = 0; + } +#endif while (remaining > 0) { const auto n = driver.uart->write(ptr, remaining); if (n > 0) { @@ -43,6 +67,22 @@ uint32_t AP_Networking_PPP::ppp_output_cb(ppp_pcb *pcb, const void *data, uint32 hal.scheduler->delay_microseconds(100); } } +#if PPP_DEBUG_TX + memcpy(&tx_bytes[pkt_size], data, len); + pkt_size += len; + if (flag_end) { + driver.uart->write(0x7E); + printf("PPP: tx[%lu] %u\n", tx_index++, pkt_size); + // dump the packet + if (!(tx_index % 10)) { + for (uint32_t i = 0; i < pkt_size; i++) { + printf(" %02X", tx_bytes[i]); + } + printf("\n"); + } + pkt_size = 0; + } +#endif return len; } @@ -224,6 +264,25 @@ void AP_Networking_PPP::ppp_loop(void) } else { hal.scheduler->delay_microseconds(200); } +#if PPP_DEBUG_RX + auto pppos = (pppos_pcb *)ppp->link_ctx_cb; + for (uint32_t i = 0; i < n; i++) { + if (buf[i] == 0x7E && last_ppp_frame_size != 1) { + // dump the packet + if (pppos->bad_pkt) { + printf("PPP: rx[%lu] %u\n", rx_index, last_ppp_frame_size); + for (uint32_t j = 0; j < last_ppp_frame_size; j++) { + printf("0x%02X,", rx_bytes[j]); + } + printf("\n"); + hal.scheduler->delay(1); + } + rx_index++; + last_ppp_frame_size = 0; + } + rx_bytes[last_ppp_frame_size++] = buf[i]; + } +#endif } } } diff --git a/libraries/AP_Notify/RGBLed.cpp b/libraries/AP_Notify/RGBLed.cpp index 944fb1d0e82ac5..caa549fd94cae9 100644 --- a/libraries/AP_Notify/RGBLed.cpp +++ b/libraries/AP_Notify/RGBLed.cpp @@ -165,16 +165,13 @@ uint32_t RGBLed::get_colour_sequence(void) const if (!AP_Notify::flags.pre_arm_check) { return sequence_prearm_failing; } - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS && - AP_Notify::flags.pre_arm_gps_check && - good_ahrs_location) { - return sequence_disarmed_good_dgps_and_location; - } - - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D && - AP_Notify::flags.pre_arm_gps_check && - good_ahrs_location) { - return sequence_disarmed_good_gps_and_location; + if (AP_Notify::flags.pre_arm_gps_check && good_ahrs_location) { + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + return sequence_disarmed_good_dgps_and_location; + } + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) { + return sequence_disarmed_good_gps_and_location; + } } #endif diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 55fb024043b1da..e3843a02aeabcf 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -222,7 +222,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = { // @Param: TKOFF_IGAIN // @DisplayName: Controller integrator during takeoff - // @Description: This is the integrator gain on the control loop during takeoff. When set to 0 then TECS_INTEG_GAIN is used. Increase to increase the rate at which speed and height offsets are trimmed out. Typically values higher than TECS_INTEG_GAIN work best + // @Description: This is the integrator gain on the control loop during takeoff. Increase to increase the rate at which speed and height offsets are trimmed out. // @Range: 0.0 0.5 // @Increment: 0.02 // @User: Advanced @@ -779,7 +779,6 @@ void AP_TECS::_update_throttle_with_airspeed(void) // ensure we run at full throttle until we reach the target airspeed _throttle_dem = MAX(_throttle_dem, _THRmaxf - _integTHR_state); } - _integTHR_state = integ_max; } else { _integTHR_state = constrain_float(_integTHR_state, integ_min, integ_max); } @@ -827,7 +826,12 @@ void AP_TECS::_update_throttle_with_airspeed(void) #endif } - // Constrain throttle demand and record clipping + constrain_throttle(); + +} + +// Constrain throttle demand and record clipping +void AP_TECS::constrain_throttle() { if (_throttle_dem > _THRmaxf) { _thr_clip_status = clipStatus::MAX; _throttle_dem = _THRmaxf; @@ -843,9 +847,7 @@ float AP_TECS::_get_i_gain(void) { float i_gain = _integGain; if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { - if (!is_zero(_integGain_takeoff)) { - i_gain = _integGain_takeoff; - } + i_gain = _integGain_takeoff; } else if (_flags.is_doing_auto_land) { if (!is_zero(_integGain_land)) { i_gain = _integGain_land; @@ -893,18 +895,6 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge, float pi _throttle_dem = nomThr; } - if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF) { - const uint32_t now = AP_HAL::millis(); - if (_takeoff_start_ms == 0) { - _takeoff_start_ms = now; - } - const uint32_t dt = now - _takeoff_start_ms; - if (dt*0.001 < aparm.takeoff_throttle_max_t) { - _throttle_dem = _THRmaxf; - } - } else { - _takeoff_start_ms = 0; - } if (_flags.is_gliding) { _throttle_dem = 0.0f; return; @@ -918,6 +908,8 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge, float pi float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y)); float STEdot_dem = _rollComp * (1.0f/constrain_float(cosPhi * cosPhi, 0.1f, 1.0f) - 1.0f); _throttle_dem = _throttle_dem + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf); + + constrain_throttle(); } void AP_TECS::_detect_bad_descent(void) @@ -1088,17 +1080,17 @@ void AP_TECS::_update_pitch(void) // @Field: PEW: Potential energy weighting // @Field: KEW: Kinetic energy weighting // @Field: EBD: Energy balance demand - // @Field: EBE: Energy balance error + // @Field: EBE: Energy balance estimate // @Field: EBDD: Energy balance rate demand - // @Field: EBDE: Energy balance rate error + // @Field: EBDE: Energy balance rate estimate // @Field: EBDDT: Energy balance rate demand + Energy balance rate error*pitch_damping // @Field: Imin: Minimum integrator value // @Field: Imax: Maximum integrator value // @Field: I: Energy balance error integral // @Field: KI: Pitch demand kinetic energy integral - // @Field: pmin: Pitch min - // @Field: pmax: Pitch max - AP::logger().WriteStreaming("TEC2","TimeUS,PEW,KEW,EBD,EBE,EBDD,EBDE,EBDDT,Imin,Imax,I,KI,pmin,pmax", + // @Field: tmin: Throttle min + // @Field: tmax: Throttle max + AP::logger().WriteStreaming("TEC2","TimeUS,PEW,KEW,EBD,EBE,EBDD,EBDE,EBDDT,Imin,Imax,I,KI,tmin,tmax", "Qfffffffffffff", AP_HAL::micros64(), (double)SPE_weighting, @@ -1112,8 +1104,8 @@ void AP_TECS::_update_pitch(void) (double)integSEBdot_max, (double)_integSEBdot, (double)_integKE, - (double)_PITCHminf, - (double)_PITCHmaxf); + (double)_THRminf, + (double)_THRmaxf); } #endif } @@ -1172,22 +1164,25 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) _hgt_dem = hgt_afe; _hgt_dem_in_prev = hgt_afe; _hgt_dem_in_raw = hgt_afe; - _TAS_dem_adj = _TAS_dem; - _flags.reset = true; - _post_TO_hgt_offset = _climb_rate * _hgt_dem_tconst; _flags.underspeed = false; _flags.badDescent = false; - + _post_TO_hgt_offset = _climb_rate_limit * _hgt_dem_tconst; // Replacement prevents oscillating hgt_rate_dem. + _TAS_dem_adj = _TAS_dem; _max_climb_scaler = 1.0f; _max_sink_scaler = 1.0f; - _pitch_demand_lpf.reset(_ahrs.get_pitch()); _pitch_measured_lpf.reset(_ahrs.get_pitch()); + + if (!_flag_have_reset_after_takeoff) { + _flags.reset = true; + _flag_have_reset_after_takeoff = true; + } } if (_flight_stage != AP_FixedWing::FlightStage::TAKEOFF && _flight_stage != AP_FixedWing::FlightStage::ABORT_LANDING) { // reset takeoff speed flag when not in takeoff _flags.reached_speed_takeoff = false; + _flag_have_reset_after_takeoff = false; } } @@ -1252,16 +1247,8 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, _hgt_dem_in = _hgt_dem_in_raw; } - if (aparm.takeoff_throttle_max != 0 && - (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)) { - _THRmaxf = aparm.takeoff_throttle_max * 0.01f; - } else { - _THRmaxf = aparm.throttle_max * 0.01f; - } - _THRminf = aparm.throttle_min * 0.01f; - - // min of 1% throttle range to prevent a numerical error - _THRmaxf = MAX(_THRmaxf, _THRminf+0.01); + // Update the throttle limits. + _update_throttle_limits(); // work out the maximum and minimum pitch // if TECS_PITCH_{MAX,MIN} isn't set then use @@ -1312,9 +1299,6 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, // note that this allows a flare pitch outside the normal TECS auto limits _PITCHmaxf = _land_pitch_max; } - - // and allow zero throttle - _THRminf = 0; } else if (_landing.is_on_approach()) { _PITCHminf = MAX(_PITCHminf, aparm.pitch_limit_min); _pitch_min_at_flare_entry = _PITCHminf; @@ -1339,7 +1323,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, } if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF || flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { - if (!_flags.reached_speed_takeoff && _TAS_state >= _TAS_dem_adj) { + if (!_flags.reached_speed_takeoff && _TAS_state >= _TASmin && _TASmin > 0) { // we have reached our target speed in takeoff, allow for // normal throttle control _flags.reached_speed_takeoff = true; @@ -1440,3 +1424,80 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, } #endif } + +// set minimum throttle override, [-1, -1] range +// it is applicable for one control cycle only +void AP_TECS::set_throttle_min(const float thr_min) { + // Don't change the limit if it is already covered. + if (thr_min > _THRminf_ext) { + _THRminf_ext = thr_min; + } +} + +// set minimum throttle override, [0, -1] range +// it is applicable for one control cycle only +void AP_TECS::set_throttle_max(const float thr_max) { + // Don't change the limit if it is already covered. + if (thr_max < _THRmaxf_ext) { + _THRmaxf_ext = thr_max; + } +} + +void AP_TECS::_update_throttle_limits() { + + // Configure max throttle. + + // Read the maximum throttle limit. + _THRmaxf = aparm.throttle_max * 0.01f; + // If more max throttle is allowed during takeoff, use it. + if (aparm.takeoff_throttle_max*0.01f > _THRmaxf + && (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) + ) { + _THRmaxf = aparm.takeoff_throttle_max * 0.01f; + } + // In any case, constrain to the external safety limits. + _THRmaxf = MIN(_THRmaxf, _THRmaxf_ext); + + // Configure min throttle. + + // If less min throttle is allowed during takeoff, use it. + bool use_takeoff_throttle = _flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING; + const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE); + use_takeoff_throttle = use_takeoff_throttle && (use_throttle_range == 1) && (aparm.takeoff_throttle_min != 0); + if ( use_takeoff_throttle ) { + _THRminf = MIN(_THRminf, aparm.takeoff_throttle_min * 0.01f); + } + else { // Otherwise, during normal situations let regular limit. + _THRminf = aparm.throttle_min * 0.01f; + } + // Raise min to force max throttle for TKOFF_THR_MAX_T after a takeoff. + if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF) { + const uint32_t now = AP_HAL::millis(); + if (_takeoff_start_ms == 0) { + _takeoff_start_ms = now; + } + const uint32_t dt = now - _takeoff_start_ms; + if (dt*0.001 < aparm.takeoff_throttle_max_t) { + _THRminf = _THRmaxf; + } + } else { + _takeoff_start_ms = 0; + } + // If we are flaring, allow the throttle to go to 0. + if (_landing.is_flaring()) { + _THRminf = 0; + } + // In any case, constrain to the external safety limits. + _THRminf = MAX(_THRminf, _THRminf_ext); + + // Allow a minimum of 1% throttle range, primarily to prevent TECS numerical errors. + if (_THRmaxf < 1) { + _THRmaxf = MAX(_THRmaxf, _THRminf + 0.01f); + } else { + _THRminf = MIN(_THRminf, _THRmaxf - 0.01f); + } + + // Reset the external throttle limits. + _THRminf_ext = -1.0f; + _THRmaxf_ext = 1.0f; +} \ No newline at end of file diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index fcd3fe97f9bfe7..8ffff211fbf032 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -134,6 +134,14 @@ class AP_TECS { _pitch_max_limit = pitch_limit; } + // set minimum throttle override, [-1, -1] range + // it is applicable for one control cycle only + void set_throttle_min(const float thr_min); + + // set minimum throttle override, [0, -1] range + // it is applicable for one control cycle only + void set_throttle_max(const float thr_max); + // force use of synthetic airspeed for one loop void use_synthetic_airspeed(void) { _use_synthetic_airspeed_once = true; @@ -360,6 +368,9 @@ class AP_TECS { // Maximum and minimum floating point throttle limits float _THRmaxf; float _THRminf; + // Maximum and minimum throttle safety limits, set externally, typically by servos.cpp:apply_throttle_limits() + float _THRmaxf_ext = 1.0f; + float _THRminf_ext = -1.0f; // Maximum and minimum floating point pitch limits float _PITCHmaxf; @@ -419,6 +430,9 @@ class AP_TECS { // need to reset on next loop bool _need_reset; + // Checks if we reset at the beginning of takeoff. + bool _flag_have_reset_after_takeoff; + float _SKE_weighting; AP_Int8 _use_synthetic_airspeed; @@ -458,6 +472,9 @@ class AP_TECS { // Update Demanded Throttle Non-Airspeed void _update_throttle_without_airspeed(int16_t throttle_nudge, float pitch_trim_deg); + // Constrain throttle demand and record clipping + void constrain_throttle(); + // get integral gain which is flight_stage dependent float _get_i_gain(void); @@ -478,4 +495,7 @@ class AP_TECS { // current time constant float timeConstant(void) const; + + // Update the allowable throttle range. + void _update_throttle_limits(); }; diff --git a/libraries/AP_Vehicle/AP_FixedWing.h b/libraries/AP_Vehicle/AP_FixedWing.h index df3692a61b6552..722c53dcaf9af8 100644 --- a/libraries/AP_Vehicle/AP_FixedWing.h +++ b/libraries/AP_Vehicle/AP_FixedWing.h @@ -11,6 +11,8 @@ struct AP_FixedWing { AP_Int8 throttle_slewrate; AP_Int8 throttle_cruise; AP_Int8 takeoff_throttle_max; + AP_Int8 takeoff_throttle_min; + AP_Int32 takeoff_options; AP_Int16 airspeed_min; AP_Int16 airspeed_max; AP_Float airspeed_cruise; @@ -49,4 +51,9 @@ struct AP_FixedWing { LAND = 4, ABORT_LANDING = 7 }; + + // Bitfields of TKOFF_OPTIONS + enum class TakeoffOption { + THROTTLE_RANGE = (1U << 0), // Unset: Max throttle. Set: Throttle range. + }; }; diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 8d27aeb426eb10..e0bdfed8e43b64 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -247,6 +247,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Values{Plane}: 210:Airbrakes // @Values{Rover}: 211:Walking Height // @Values{Copter, Rover, Plane}: 212:Mount1 Roll, 213:Mount1 Pitch, 214:Mount1 Yaw, 215:Mount2 Roll, 216:Mount2 Pitch, 217:Mount2 Yaw + // @Values{Copter}: 219:Transmitter Tuning // @Values{Copter, Rover, Plane}: 300:Scripting1, 301:Scripting2, 302:Scripting3, 303:Scripting4, 304:Scripting5, 305:Scripting6, 306:Scripting7, 307:Scripting8 // @User: Standard AP_GROUPINFO_FRAME("OPTION", 6, RC_Channel, option, 0, AP_PARAM_FRAME_COPTER|AP_PARAM_FRAME_ROVER|AP_PARAM_FRAME_PLANE|AP_PARAM_FRAME_BLIMP), diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 99959480704dd2..97dddce75b0d87 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -271,6 +271,7 @@ class RC_Channel { MOUNT2_PITCH = 216, // mount3 pitch input MOUNT2_YAW = 217, // mount4 yaw input LOWEHEISER_THROTTLE= 218, // allows for throttle on slider + TRANSMITTER_TUNING = 219, // use a transmitter knob or slider for in-flight tuning // inputs 248-249 are reserved for the Skybrush fork at // https://github.com/skybrush-io/ardupilot diff --git a/libraries/SITL/SIM_VectorNav.cpp b/libraries/SITL/SIM_VectorNav.cpp index b3f9ecf6a4d89f..ad61f0101077e3 100644 --- a/libraries/SITL/SIM_VectorNav.cpp +++ b/libraries/SITL/SIM_VectorNav.cpp @@ -29,37 +29,49 @@ VectorNav::VectorNav() : { } -struct PACKED VN_INS_packet1 { - float uncompMag[3]; + +struct PACKED VN_IMU_packet_sim { + static constexpr uint8_t header[]{0x01, 0x21, 0x07}; + uint64_t timeStartup; + float gyro[3]; + float accel[3]; float uncompAccel[3]; float uncompAngRate[3]; - float pressure; float mag[3]; - float accel[3]; - float gyro[3]; + float temp; + float pressure; +}; +constexpr uint8_t VN_IMU_packet_sim::header[]; + +struct PACKED VN_INS_ekf_packet_sim { + static constexpr uint8_t header[]{0x31, 0x01, 0x00, 0x06, 0x01, 0x13, 0x06}; + uint64_t timeStartup; float ypr[3]; float quaternion[4]; float yprU[3]; - double positionLLA[3]; - float velNED[3]; + uint16_t insStatus; + double posLla[3]; + float velNed[3]; float posU; float velU; }; - -struct PACKED VN_INS_packet2 { - uint64_t timeGPS; - float temp; - uint8_t numGPS1Sats; - uint8_t GPS1Fix; - double GPS1posLLA[3]; - float GPS1velNED[3]; - float GPS1DOP[7]; - uint8_t numGPS2Sats; - uint8_t GPS2Fix; +constexpr uint8_t VN_INS_ekf_packet_sim::header[]; + +struct PACKED VN_INS_gnss_packet_sim { + static constexpr uint8_t header[]{0x49, 0x03, 0x00, 0xB8, 0x26, 0x18, 0x00}; + uint64_t timeStartup; + uint64_t timeGps; + uint8_t numSats1; + uint8_t fix1; + double posLla1[3]; + float velNed1[3]; + float posU1[3]; + float velU1; + float dop1[7]; + uint8_t numSats2; + uint8_t fix2; }; - -#define VN_PKT1_HEADER { 0xFA, 0x34, 0x2E, 0x07, 0x06, 0x01, 0x12, 0x06 } -#define VN_PKT2_HEADER { 0xFA, 0x4E, 0x02, 0x00, 0x10, 0x00, 0xB8, 0x20, 0x18, 0x00 } +constexpr uint8_t VN_INS_gnss_packet_sim::header[]; /* get timeval using simulation time @@ -80,36 +92,62 @@ static void simulation_timeval(struct timeval *tv) tv->tv_usec = new_usec % 1000000ULL; } -void VectorNav::send_packet1(void) +void VectorNav::send_imu_packet(void) { const auto &fdm = _sitl->state; - struct VN_INS_packet1 pkt {}; + struct VN_IMU_packet_sim pkt {}; + + pkt.timeStartup = AP_HAL::micros() * 1e3; + + + const float gyro_noise = 0.05; + + pkt.gyro[0] = radians(fdm.rollRate + rand_float() * gyro_noise); + pkt.gyro[1] = radians(fdm.pitchRate + rand_float() * gyro_noise); + pkt.gyro[2] = radians(fdm.yawRate + rand_float() * gyro_noise); + + pkt.accel[0] = fdm.xAccel; + pkt.accel[1] = fdm.yAccel; + pkt.accel[2] = fdm.zAccel; pkt.uncompAccel[0] = fdm.xAccel; pkt.uncompAccel[1] = fdm.yAccel; pkt.uncompAccel[2] = fdm.zAccel; - const float gyro_noise = 0.05; + pkt.uncompAngRate[0] = radians(fdm.rollRate + gyro_noise * rand_float()); pkt.uncompAngRate[1] = radians(fdm.pitchRate + gyro_noise * rand_float()); pkt.uncompAngRate[2] = radians(fdm.yawRate + gyro_noise * rand_float()); - const float pressure_Pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude); - pkt.pressure = pressure_Pa*0.001 + rand_float() * 0.01; - pkt.mag[0] = fdm.bodyMagField.x*0.001; pkt.mag[1] = fdm.bodyMagField.y*0.001; pkt.mag[2] = fdm.bodyMagField.z*0.001; - pkt.uncompMag[0] = pkt.mag[0]; - pkt.uncompMag[1] = pkt.mag[1]; - pkt.uncompMag[2] = pkt.mag[2]; - pkt.accel[0] = fdm.xAccel; - pkt.accel[1] = fdm.yAccel; - pkt.accel[2] = fdm.zAccel; - pkt.gyro[0] = radians(fdm.rollRate + rand_float() * gyro_noise); - pkt.gyro[1] = radians(fdm.pitchRate + rand_float() * gyro_noise); - pkt.gyro[2] = radians(fdm.yawRate + rand_float() * gyro_noise); + pkt.temp = AP_Baro::get_temperatureC_for_alt_amsl(fdm.altitude); + + const float pressure_Pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude); + pkt.pressure = pressure_Pa*0.001 + rand_float() * 0.01; + + const uint8_t sync_byte = 0xFA; + write_to_autopilot((const char *)&sync_byte, 1); + write_to_autopilot((const char *)&VN_IMU_packet_sim::header, sizeof(VN_IMU_packet_sim::header)); + write_to_autopilot((const char *)&pkt, sizeof(pkt)); + + uint16_t crc = crc16_ccitt(&VN_IMU_packet_sim::header[0], sizeof(VN_IMU_packet_sim::header), 0); + crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc); + uint16_t crc2; + swab(&crc, &crc2, 2); + + write_to_autopilot((const char *)&crc2, sizeof(crc2)); +} + +void VectorNav::send_ins_ekf_packet(void) +{ + const auto &fdm = _sitl->state; + + struct VN_INS_ekf_packet_sim pkt {}; + + pkt.timeStartup = AP_HAL::micros() * 1e3; pkt.ypr[0] = fdm.yawDeg; pkt.ypr[1] = fdm.pitchDeg; @@ -120,64 +158,80 @@ void VectorNav::send_packet1(void) pkt.quaternion[2] = fdm.quaternion.q4; pkt.quaternion[3] = fdm.quaternion.q1; - pkt.positionLLA[0] = fdm.latitude; - pkt.positionLLA[1] = fdm.longitude; - pkt.positionLLA[2] = fdm.altitude; - pkt.velNED[0] = fdm.speedN; - pkt.velNED[1] = fdm.speedE; - pkt.velNED[2] = fdm.speedD; + pkt.yprU[0] = 0.03; + pkt.yprU[1] = 0.03; + pkt.yprU[2] = 0.15; + + pkt.insStatus = 0x0306; + + pkt.posLla[0] = fdm.latitude; + pkt.posLla[1] = fdm.longitude; + pkt.posLla[2] = fdm.altitude; + pkt.velNed[0] = fdm.speedN; + pkt.velNed[1] = fdm.speedE; + pkt.velNed[2] = fdm.speedD; pkt.posU = 0.5; pkt.velU = 0.25; - const uint8_t header[] VN_PKT1_HEADER; + const uint8_t sync_byte = 0xFA; + write_to_autopilot((const char *)&sync_byte, 1); + write_to_autopilot((const char *)&VN_INS_ekf_packet_sim::header, sizeof(VN_INS_ekf_packet_sim::header)); + write_to_autopilot((const char *)&pkt, sizeof(pkt)); - write_to_autopilot((char *)&header, sizeof(header)); - write_to_autopilot((char *)&pkt, sizeof(pkt)); - - uint16_t crc = crc16_ccitt(&header[1], sizeof(header)-1, 0); + uint16_t crc = crc16_ccitt(&VN_INS_ekf_packet_sim::header[0], sizeof(VN_INS_ekf_packet_sim::header), 0); crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc); + uint16_t crc2; swab(&crc, &crc2, 2); - write_to_autopilot((char *)&crc2, sizeof(crc2)); + write_to_autopilot((const char *)&crc2, sizeof(crc2)); } -void VectorNav::send_packet2(void) +void VectorNav::send_ins_gnss_packet(void) { const auto &fdm = _sitl->state; - struct VN_INS_packet2 pkt {}; + struct VN_INS_gnss_packet_sim pkt {}; + + pkt.timeStartup = AP_HAL::micros() * 1e3; struct timeval tv; simulation_timeval(&tv); - pkt.timeGPS = tv.tv_usec * 1000ULL; - - pkt.temp = AP_Baro::get_temperatureC_for_alt_amsl(fdm.altitude); - pkt.numGPS1Sats = 19; - pkt.GPS1Fix = 3; - pkt.GPS1posLLA[0] = fdm.latitude; - pkt.GPS1posLLA[1] = fdm.longitude; - pkt.GPS1posLLA[2] = fdm.altitude; - pkt.GPS1velNED[0] = fdm.speedN; - pkt.GPS1velNED[1] = fdm.speedE; - pkt.GPS1velNED[2] = fdm.speedD; - // pkt.GPS1DOP = - pkt.numGPS2Sats = 18; - pkt.GPS2Fix = 3; - - const uint8_t header[] VN_PKT2_HEADER; - - write_to_autopilot((char *)&header, sizeof(header)); - write_to_autopilot((char *)&pkt, sizeof(pkt)); - - uint16_t crc = crc16_ccitt(&header[1], sizeof(header)-1, 0); + pkt.timeGps = tv.tv_usec * 1000ULL; + + pkt.numSats1 = 19; + pkt.fix1 = 3; + pkt.posLla1[0] = fdm.latitude; + pkt.posLla1[1] = fdm.longitude; + pkt.posLla1[2] = fdm.altitude; + pkt.velNed1[0] = fdm.speedN; + pkt.velNed1[1] = fdm.speedE; + pkt.velNed1[2] = fdm.speedD; + + pkt.posU1[0] = 1; + pkt.posU1[0] = 1; + pkt.posU1[0] = 1.5; + + pkt.velNed1[0] = 0.05; + pkt.velNed1[0] = 0.05; + pkt.velNed1[0] = 0.05; + // pkt.dop1 = + pkt.numSats2 = 18; + pkt.fix2 = 3; + + const uint8_t sync_byte = 0xFA; + write_to_autopilot((const char *)&sync_byte, 1); + write_to_autopilot((const char *)&VN_INS_gnss_packet_sim::header, sizeof(VN_INS_gnss_packet_sim::header)); + write_to_autopilot((const char *)&pkt, sizeof(pkt)); + + uint16_t crc = crc16_ccitt(&VN_INS_gnss_packet_sim::header[0], sizeof(VN_INS_gnss_packet_sim::header), 0); crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc); uint16_t crc2; swab(&crc, &crc2, 2); - write_to_autopilot((char *)&crc2, sizeof(crc2)); + write_to_autopilot((const char *)&crc2, sizeof(crc2)); } void VectorNav::nmea_printf(const char *fmt, ...) @@ -203,14 +257,19 @@ void VectorNav::update(void) } uint32_t now = AP_HAL::micros(); - if (now - last_pkt1_us >= 20000) { - last_pkt1_us = now; - send_packet1(); + if (now - last_imu_pkt_us >= 20000) { + last_imu_pkt_us = now; + send_imu_packet(); } - - if (now - last_pkt2_us >= 200000) { - last_pkt2_us = now; - send_packet2(); + + if (now - last_ekf_pkt_us >= 20000) { + last_ekf_pkt_us = now; + send_ins_ekf_packet(); + } + + if (now - last_gnss_pkt_us >= 200000) { + last_gnss_pkt_us = now; + send_ins_gnss_packet(); } char receive_buf[50]; diff --git a/libraries/SITL/SIM_VectorNav.h b/libraries/SITL/SIM_VectorNav.h index 589539e5d9a8f3..77fde25625733a 100644 --- a/libraries/SITL/SIM_VectorNav.h +++ b/libraries/SITL/SIM_VectorNav.h @@ -41,12 +41,13 @@ class VectorNav : public SerialDevice { void update(void); private: - uint32_t last_pkt1_us; - uint32_t last_pkt2_us; - uint32_t last_type_us; + uint32_t last_imu_pkt_us; + uint32_t last_ekf_pkt_us; + uint32_t last_gnss_pkt_us; - void send_packet1(); - void send_packet2(); + void send_imu_packet(); + void send_ins_ekf_packet(); + void send_ins_gnss_packet(); void nmea_printf(const char *fmt, ...); };