diff --git a/AntennaTracker/ReleaseNotes.txt b/AntennaTracker/ReleaseNotes.txt index 135db4c9a9af1..9c93be93dce76 100644 --- a/AntennaTracker/ReleaseNotes.txt +++ b/AntennaTracker/ReleaseNotes.txt @@ -1,4 +1,66 @@ ArduPilot Antenna Tracker Release Notes: +------------------------------------------------------------------ +Release 4.7.0-beta3 10-Apr-2026 + +Changes from 4.7.0-beta2 + +1) Board specific changes + +- Atlas-Control has updated hwdef.dat file (@AltasFlight, PR:32268) +- HWH7 flight controller added to supported boards (@Arwayday, PR:32248) +- TBS_LUCID_H7 now has an MPU6000 version (@andyp1per, PR:32512) + +2) Driver changes + +- ZB25VQ128D flash chip now supported for onboard logging (@LoyalLinjy, PR:32408) +- Kebni SensAItion as External AHRS now supported in IMU and INS modes (@magnus-lindhe-kebni, PR:31775) +- IST8310 compass probing corrected on fmuv3-based boards (@peterbarker, PR:32462) + +3) AHRS specific changes + +- EKF3 flash space requirements reduced (@tpwrules, PRs:32213, 32618) +- EKF3 is fixed to no longer reset to external altitude when it is configured to another source (@snktshrma, PR:32508) +- AHRS recorded origin check will correctly use the GPS (@andyp1per, PR: 32469) +- AHRS external origin setting will be delayed until filter is initialised (@andyp1per, PR: 32505) +- EKF3 missing constraints to FuseDragForces added (@tpwrules, PR:32128) +- Fix replay involving body odometry data (@twprules, PR:32598) + +4) Camera and mount specific changes + +- Camera servo driver's CAMERA_INFORMATION flags fixed (@Davidsastresas, PR:32564) +- RunCam param metadata names are fixed to match code (@tpwrules, PR:32601) +- Mount fix to angular rates when using FPV mode (@KSimeonAVTA, PR:32540) +- Mount scripting driver accepts rate targets (@peterbarker, PR:32626) +- Fix wire data issues with various Mount backends (@peterbarker, PR:32630) + +5) Other libraries changes + +- Fence for Plane and Copter supports alt above terrain (@andyp1per, PR:31619) +- Ignore-receiver in RC_OPTIONS is now respected during RC loss (@peterbarker, PR:32488) +- ATC_INPUT_TC and Q_A_INPUT_TC param defaults reduced (@rmackay9, PR:32643) + +6) Copter/Tradheli specific changes + +- Spool state is logged (@Georacer, PR:31668) + +7) Plane specific changes + +- Rangefinder state is logged (@Georacer, PR:31579) +- AHRS log message rate not affected by full rate attitude logging (@IamPete1, PR:32623) + +8) Sub specific changes + +- Automatically deal with misconfigured lights/actuators (@Williangalvani, PR:31109) + +9) Rover specific changes + +- Battery watt limiter feature added to custom build server (@mackay9, PR:32687) + +10) Scripting changes + +- plane_follow.lua messages will no longer crash the script if there is no target vehicle (@timtuxworth, PR:32567) +- plane_follow.lua has a fix for crosstrack oscillation (@timtuxworth, PR:32575) + ------------------------------------------------------------------ Release 4.7.0-beta2 21-Mar-2026 diff --git a/AntennaTracker/version.h b/AntennaTracker/version.h index eb69665fac258..a8caa9771f72e 100644 --- a/AntennaTracker/version.h +++ b/AntennaTracker/version.h @@ -6,10 +6,10 @@ #include "ap_version.h" -#define THISFIRMWARE "AntennaTracker V4.7.0-beta2" +#define THISFIRMWARE "AntennaTracker V4.7.0-beta3" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_BETA+1 +#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_BETA+2 #define FW_MAJOR 4 #define FW_MINOR 7 diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index bcab68c2d007b..29bc950a425d4 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -651,6 +651,8 @@ void Copter::loop_rate_logging() if (should_log(MASK_LOG_IMU_FAST)) { AP::ins().Write_IMU(); } + + motors->Log_Write_SPOL(); } // ten_hz_logging_loop diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 9fa9cd118b93c..720c0ec97885a 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,4 +1,66 @@ ArduPilot Copter Release Notes: +------------------------------------------------------------------ +Release 4.7.0-beta3 10-Apr-2026 + +Changes from 4.7.0-beta2 + +1) Board specific changes + +- Atlas-Control has updated hwdef.dat file (@AltasFlight, PR:32268) +- HWH7 flight controller added to supported boards (@Arwayday, PR:32248) +- TBS_LUCID_H7 now has an MPU6000 version (@andyp1per, PR:32512) + +2) Driver changes + +- ZB25VQ128D flash chip now supported for onboard logging (@LoyalLinjy, PR:32408) +- Kebni SensAItion as External AHRS now supported in IMU and INS modes (@magnus-lindhe-kebni, PR:31775) +- IST8310 compass probing corrected on fmuv3-based boards (@peterbarker, PR:32462) + +3) AHRS specific changes + +- EKF3 flash space requirements reduced (@tpwrules, PRs:32213, 32618) +- EKF3 is fixed to no longer reset to external altitude when it is configured to another source (@snktshrma, PR:32508) +- AHRS recorded origin check will correctly use the GPS (@andyp1per, PR: 32469) +- AHRS external origin setting will be delayed until filter is initialised (@andyp1per, PR: 32505) +- EKF3 missing constraints to FuseDragForces added (@tpwrules, PR:32128) +- Fix replay involving body odometry data (@twprules, PR:32598) + +4) Camera and mount specific changes + +- Camera servo driver's CAMERA_INFORMATION flags fixed (@Davidsastresas, PR:32564) +- RunCam param metadata names are fixed to match code (@tpwrules, PR:32601) +- Mount fix to angular rates when using FPV mode (@KSimeonAVTA, PR:32540) +- Mount scripting driver accepts rate targets (@peterbarker, PR:32626) +- Fix wire data issues with various Mount backends (@peterbarker, PR:32630) + +5) Other libraries changes + +- Fence for Plane and Copter supports alt above terrain (@andyp1per, PR:31619) +- Ignore-receiver in RC_OPTIONS is now respected during RC loss (@peterbarker, PR:32488) +- ATC_INPUT_TC and Q_A_INPUT_TC param defaults reduced (@rmackay9, PR:32643) + +6) Copter/Tradheli specific changes + +- Spool state is logged (@Georacer, PR:31668) + +7) Plane specific changes + +- Rangefinder state is logged (@Georacer, PR:31579) +- AHRS log message rate not affected by full rate attitude logging (@IamPete1, PR:32623) + +8) Sub specific changes + +- Automatically deal with misconfigured lights/actuators (@Williangalvani, PR:31109) + +9) Rover specific changes + +- Battery watt limiter feature added to custom build server (@mackay9, PR:32687) + +10) Scripting changes + +- plane_follow.lua messages will no longer crash the script if there is no target vehicle (@timtuxworth, PR:32567) +- plane_follow.lua has a fix for crosstrack oscillation (@timtuxworth, PR:32575) + ------------------------------------------------------------------ Release 4.7.0-beta2 21-Mar-2026 diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 1425d30473001..5235a5ac6cd1b 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -567,8 +567,8 @@ void ModeRTL::compute_return_target() // the vehicle not climbing at all as RTL begins. This can be overly conservative and it might be better // to apply the fence alt limit independently on the origin_point and return_target if ((copter.fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) { - // get return target as alt-above-home so it can be compared to fence's alt - if (rtl_path.return_target.get_alt_m(Location::AltFrame::ABOVE_HOME, target_alt_m)) { + // get return target in max alt frame so it can be compared to fence's alt + if (rtl_path.return_target.get_alt_m(copter.fence.get_alt_max_frame(), target_alt_m)) { float fence_alt_m = copter.fence.get_safe_alt_max_m(); if (target_alt_m > fence_alt_m) { // reduce target alt to the fence alt diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 21f65b2be6837..b57711e9fa0e0 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -6,10 +6,10 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduCopter V4.7.0-beta2" +#define THISFIRMWARE "ArduCopter V4.7.0-beta3" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_BETA+1 +#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_BETA+2 #define FW_MAJOR 4 #define FW_MINOR 7 diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 827960bcde15a..b23a2fc02e863 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -53,8 +53,6 @@ void Plane::Log_Write_Attitude(void) if (steerController.active()) { logger.Write_PID(LOG_PIDS_MSG, steerController.get_pid_info()); } - - AP::ahrs().Log_Write(); } // do fast logging for plane @@ -192,6 +190,45 @@ void Plane::Log_Write_Nav_Tuning() logger.WriteBlock(&pkt, sizeof(pkt)); } +#if AP_RANGEFINDER_ENABLED +struct PACKED log_RFNS { + LOG_PACKET_HEADER; + uint64_t time_us; + bool in_use; + bool in_range; + uint8_t in_range_count; + bool have_initial_reading; + float initial_range; + float last_distance; + float correction; + float initial_correction; + float last_stable_correction; + uint32_t last_correction_time_ms; + float height_estimate; +}; + +// Write a Rangefinder State packet +void Plane::Log_Write_RFNS() +{ + const struct log_RFNS pkt { + LOG_PACKET_HEADER_INIT(LOG_RFNS_MSG), + time_us : AP_HAL::micros64(), + in_use : rangefinder_state.in_use, + in_range : rangefinder_state.in_range, + in_range_count : rangefinder_state.in_range_count, + have_initial_reading : rangefinder_state.have_initial_reading, + initial_range : rangefinder_state.initial_range, + last_distance : rangefinder_state.last_distance, + correction : rangefinder_state.correction, + initial_correction : rangefinder_state.initial_correction, + last_stable_correction : rangefinder_state.last_stable_correction, + last_correction_time_ms : rangefinder_state.last_correction_time_ms, + height_estimate : rangefinder_state.height_estimate + }; + logger.WriteBlock(&pkt, sizeof(pkt)); +} +#endif // AP_RANGEFINDER_ENABLED + struct PACKED log_Status { LOG_PACKET_HEADER; uint64_t time_us; @@ -501,6 +538,25 @@ const struct LogStructure Plane::log_structure[] = { { LOG_OFG_MSG, sizeof(log_OFG_Guided), "OFG", "QffffBffB", "TimeUS,Arsp,ArspA,Alt,AltA,AltF,Hdg,HdgA,AltL", "snnmo-d--", "F--------" , true }, #endif + +#if AP_RANGEFINDER_ENABLED +// @LoggerMessage: RFNS +// @Description: Rangefinder state +// @Field: TimeUS: Time since system startup +// @Field: InUse: Whether the rangefinder is in use. +// @Field: InRng: Whether the rangefinder is in range. +// @Field: IRCnt: Count of in-range measurements +// @Field: Ms0OK: Whether there has been an initial measurement. +// @Field: Ms0: The initial measured range +// @Field: Dst: The last recorded distance +// @Field: Cor: The rangefinder correction +// @Field: Cor0: The initial rangefinder correction +// @Field: CorL: The last stable correction +// @Field: TimeCL: The last correction time +// @Field: HE: Height estimate + { LOG_RFNS_MSG, sizeof(log_RFNS), + "RFNS", "QBBBBfffffIf", "TimeUS,InUse,InRng,IRCnt,Ms0OK,Ms0,Dst,Cor,Cor0,CorL,TimeCL,HE", "s----mmmmmsm", "F----00000C0", true }, +#endif // AP_RANGEFINDER_ENABLED }; uint8_t Plane::get_num_log_structures() const diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp index 59ebcd4f83447..f77d116fcb26a 100644 --- a/ArduPlane/Plane.cpp +++ b/ArduPlane/Plane.cpp @@ -268,18 +268,33 @@ void Plane::update_compass(void) */ void Plane::update_logging10(void) { - bool log_faster = (should_log(MASK_LOG_ATTITUDE_FULLRATE) || should_log(MASK_LOG_ATTITUDE_FAST)); - if (should_log(MASK_LOG_ATTITUDE_MED) && !log_faster) { + const bool attitude_med = should_log(MASK_LOG_ATTITUDE_MED); + const bool attitude_faster = should_log(MASK_LOG_ATTITUDE_FULLRATE) || should_log(MASK_LOG_ATTITUDE_FAST); + + // Log attitude only if no faster logging is selected + if (attitude_med && !attitude_faster) { Log_Write_Attitude(); - ahrs.Write_AOA_SSA(); - } else if (log_faster) { + AP::ahrs().Log_Write(); + } + + // If any attitude logging is enabled log AOA and SSA + if (attitude_med || attitude_faster) { ahrs.Write_AOA_SSA(); } + #if HAL_MOUNT_ENABLED if (should_log(MASK_LOG_CAMERA)) { camera_mount.write_log(); } #endif +#if AP_RANGEFINDER_ENABLED + if (should_log(MASK_LOG_NTUN)) { + if (rangefinder.has_orientation(rangefinder_orientation()) && + (g.rangefinder_landing.get() > 0)) { + Log_Write_RFNS(); + } + } +#endif } /* @@ -287,13 +302,19 @@ void Plane::update_logging10(void) */ void Plane::update_logging25(void) { - // MASK_LOG_ATTITUDE_FULLRATE logs at 400Hz, MASK_LOG_ATTITUDE_FAST at 25Hz, MASK_LOG_ATTIUDE_MED logs at 10Hz - // highest rate selected wins - bool log_faster = should_log(MASK_LOG_ATTITUDE_FULLRATE); - if (should_log(MASK_LOG_ATTITUDE_FAST) && !log_faster) { + const bool attitude_fast = should_log(MASK_LOG_ATTITUDE_FAST); + const bool attitude_full_rate = should_log(MASK_LOG_ATTITUDE_FULLRATE); + + // Log at fast rate if fast logging is the fastest enabled + if (attitude_fast && !attitude_full_rate) { Log_Write_Attitude(); } + // Log AHRS at fast rate if either fast or full rate is selected + if (attitude_fast || attitude_full_rate) { + AP::ahrs().Log_Write(); + } + if (should_log(MASK_LOG_CTUN)) { Log_Write_Control_Tuning(); #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 1889fdee16052..567f170c44122 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -955,6 +955,9 @@ class Plane : public AP_Vehicle { void Log_Write_OFG_Guided(); void Log_Write_Guided(void); void Log_Write_Nav_Tuning(); +#if AP_RANGEFINDER_ENABLED + void Log_Write_RFNS(); +#endif void Log_Write_Status(); void Log_Write_RC(void); void Log_Write_Vehicle_Startup_Messages(); diff --git a/ArduPlane/ReleaseNotes.txt b/ArduPlane/ReleaseNotes.txt index 23a8958a46d9f..2c8657e2461ae 100644 --- a/ArduPlane/ReleaseNotes.txt +++ b/ArduPlane/ReleaseNotes.txt @@ -1,4 +1,66 @@ ArduPilot Plane Release Notes: +------------------------------------------------------------------ +Release 4.7.0-beta3 10-Apr-2026 + +Changes from 4.7.0-beta2 + +1) Board specific changes + +- Atlas-Control has updated hwdef.dat file (@AltasFlight, PR:32268) +- HWH7 flight controller added to supported boards (@Arwayday, PR:32248) +- TBS_LUCID_H7 now has an MPU6000 version (@andyp1per, PR:32512) + +2) Driver changes + +- ZB25VQ128D flash chip now supported for onboard logging (@LoyalLinjy, PR:32408) +- Kebni SensAItion as External AHRS now supported in IMU and INS modes (@magnus-lindhe-kebni, PR:31775) +- IST8310 compass probing corrected on fmuv3-based boards (@peterbarker, PR:32462) + +3) AHRS specific changes + +- EKF3 flash space requirements reduced (@tpwrules, PRs:32213, 32618) +- EKF3 is fixed to no longer reset to external altitude when it is configured to another source (@snktshrma, PR:32508) +- AHRS recorded origin check will correctly use the GPS (@andyp1per, PR: 32469) +- AHRS external origin setting will be delayed until filter is initialised (@andyp1per, PR: 32505) +- EKF3 missing constraints to FuseDragForces added (@tpwrules, PR:32128) +- Fix replay involving body odometry data (@twprules, PR:32598) + +4) Camera and mount specific changes + +- Camera servo driver's CAMERA_INFORMATION flags fixed (@Davidsastresas, PR:32564) +- RunCam param metadata names are fixed to match code (@tpwrules, PR:32601) +- Mount fix to angular rates when using FPV mode (@KSimeonAVTA, PR:32540) +- Mount scripting driver accepts rate targets (@peterbarker, PR:32626) +- Fix wire data issues with various Mount backends (@peterbarker, PR:32630) + +5) Other libraries changes + +- Fence for Plane and Copter supports alt above terrain (@andyp1per, PR:31619) +- Ignore-receiver in RC_OPTIONS is now respected during RC loss (@peterbarker, PR:32488) +- ATC_INPUT_TC and Q_A_INPUT_TC param defaults reduced (@rmackay9, PR:32643) + +6) Copter/Tradheli specific changes + +- Spool state is logged (@Georacer, PR:31668) + +7) Plane specific changes + +- Rangefinder state is logged (@Georacer, PR:31579) +- AHRS log message rate not affected by full rate attitude logging (@IamPete1, PR:32623) + +8) Sub specific changes + +- Automatically deal with misconfigured lights/actuators (@Williangalvani, PR:31109) + +9) Rover specific changes + +- Battery watt limiter feature added to custom build server (@mackay9, PR:32687) + +10) Scripting changes + +- plane_follow.lua messages will no longer crash the script if there is no target vehicle (@timtuxworth, PR:32567) +- plane_follow.lua has a fix for crosstrack oscillation (@timtuxworth, PR:32575) + ------------------------------------------------------------------ Release 4.7.0-beta2 21-Mar-2026 diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 74fe102183f9b..73d1466d68cb9 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -412,11 +412,11 @@ void Plane::check_fbwb_altitude(void) // taking fence max and min altitude (with margin) const uint8_t enabled_fences = plane.fence.get_enabled_fences(); if ((enabled_fences & AC_FENCE_TYPE_ALT_MIN) != 0) { - min_alt_cm = plane.fence.get_safe_alt_min_m()*100.0; + min_alt_cm = plane.fence.get_relative_safe_alt_min_m()*100.0; should_check_min = true; } if ((enabled_fences & AC_FENCE_TYPE_ALT_MAX) != 0) { - max_alt_cm = plane.fence.get_safe_alt_max_m()*100.0; + max_alt_cm = plane.fence.get_relative_safe_alt_max_m()*100.0; should_check_max = true; } #endif diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 2a3d3bb39829b..55d5644f1ab78 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -96,6 +96,7 @@ enum log_messages { LOG_OFG_MSG, LOG_TSIT_MSG, LOG_TILT_MSG, + LOG_RFNS_MSG, }; #define MASK_LOG_ATTITUDE_FAST (1<<0) diff --git a/ArduPlane/version.h b/ArduPlane/version.h index 1fd131ccbacfe..39d716cf2b4a9 100644 --- a/ArduPlane/version.h +++ b/ArduPlane/version.h @@ -6,10 +6,10 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduPlane V4.7.0-beta2" +#define THISFIRMWARE "ArduPlane V4.7.0-beta3" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_BETA+1 +#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_BETA+2 #define FW_MAJOR 4 #define FW_MINOR 7 diff --git a/ArduSub/AP_Arming_Sub.cpp b/ArduSub/AP_Arming_Sub.cpp index a2294e7f91712..298d8e15718a2 100644 --- a/ArduSub/AP_Arming_Sub.cpp +++ b/ArduSub/AP_Arming_Sub.cpp @@ -13,30 +13,9 @@ bool AP_Arming_Sub::rc_calibration_checks(bool display_failure) } bool AP_Arming_Sub::has_disarm_function() const { - bool has_shift_function = false; - // make sure the craft has a disarm button assigned before it is armed - // check all the standard btn functions - for (uint8_t i = 0; i < 16; i++) { - switch (sub.get_button(i)->function(false)) { - case JSButton::k_shift : - has_shift_function = true; - break; - case JSButton::k_arm_toggle : - return true; - case JSButton::k_disarm : - return true; - } - } - - // check all the shift functions if there's shift assigned - if (has_shift_function) { - for (uint8_t i = 0; i < 16; i++) { - switch (sub.get_button(i)->function(true)) { - case JSButton::k_arm_toggle : - case JSButton::k_disarm : - return true; - } - } + if (sub.jsbutton_function_is_assigned(JSButton::k_arm_toggle) || + sub.jsbutton_function_is_assigned(JSButton::k_disarm)) { + return true; } // check if an AUX function that disarms or estops is setup if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP) || diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 0041fab2f40d6..72e29c8712e26 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -768,6 +768,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: ../ArduSub/actuators.cpp AP_SUBGROUPINFO(actuators, "ACTUATOR", 23, ParametersG2, Actuators), + // Hidden param used as a flag for param conversion + // This allows one time conversion while allowing user to flash between versions with and without converted params + AP_GROUPINFO_FLAGS("PARM_FMT_VER", 24, ParametersG2, param_conversion_increment, 0, AP_PARAM_FLAG_HIDDEN), + AP_GROUPEND }; @@ -955,3 +959,160 @@ void Sub::update_relay_pins() } } #endif + +// Helper function to set servo function by channel number, 1-indexed +static void set_servo_function(uint8_t channel, SRV_Channel::Function function) +{ + char param_name[20]; + snprintf(param_name, sizeof(param_name), "SERVO%u_FUNCTION", channel); + AP_Param::set_and_save_by_name(param_name, static_cast(function)); +} + + +// PARAMETER_CONVERSION - Added: Mar-2026 +void Sub::update_actuators_from_jsbuttons() +{ + /* + This function is used to update parameters from Sub <=4.5.4 to the newer actuators implementation. + servo_1_inc/min/dec/etc.. were hard-coded to work only on channels 9,10,11. This update means we now have to change + these functions to the corresponding actuator function to keep the same functionality. + For each of SERVO9,10,11, if they are set to DISABLED, check if there are joystick buttons set to actuator functions. + if so, and no other channel is set to that actuator function, set the servo to the actuator function. + */ + + // Configuration constants + const uint8_t FIRST_LEGACY_CHANNEL = 9; + const uint8_t NUM_LEGACY_ACTUATORS = 3; + const size_t FUNCTIONS_PER_SERVO = 9; + + // Legacy servo button functions mapped to actuators 1-3 + static constexpr JSButton::button_function_t servo_functions[NUM_LEGACY_ACTUATORS][FUNCTIONS_PER_SERVO] = { + { + // Actuator 1 (was servo_1_*) + JSButton::button_function_t::k_servo_1_inc, + JSButton::button_function_t::k_servo_1_dec, + JSButton::button_function_t::k_servo_1_min, + JSButton::button_function_t::k_servo_1_max, + JSButton::button_function_t::k_servo_1_center, + JSButton::button_function_t::k_servo_1_min_momentary, + JSButton::button_function_t::k_servo_1_max_momentary, + JSButton::button_function_t::k_servo_1_min_toggle, + JSButton::button_function_t::k_servo_1_max_toggle + }, + { + // Actuator 2 (was servo_2_*) + JSButton::button_function_t::k_servo_2_inc, + JSButton::button_function_t::k_servo_2_dec, + JSButton::button_function_t::k_servo_2_min, + JSButton::button_function_t::k_servo_2_max, + JSButton::button_function_t::k_servo_2_center, + JSButton::button_function_t::k_servo_2_min_momentary, + JSButton::button_function_t::k_servo_2_max_momentary, + JSButton::button_function_t::k_servo_2_min_toggle, + JSButton::button_function_t::k_servo_2_max_toggle + }, + { + // Actuator 3 (was servo_3_*) + JSButton::button_function_t::k_servo_3_inc, + JSButton::button_function_t::k_servo_3_dec, + JSButton::button_function_t::k_servo_3_min, + JSButton::button_function_t::k_servo_3_max, + JSButton::button_function_t::k_servo_3_center, + JSButton::button_function_t::k_servo_3_min_momentary, + JSButton::button_function_t::k_servo_3_max_momentary, + JSButton::button_function_t::k_servo_3_min_toggle, + JSButton::button_function_t::k_servo_3_max_toggle + } + }; + + // Target actuator functions for assignment + static constexpr SRV_Channel::Function actuator_functions[NUM_LEGACY_ACTUATORS] = { + SRV_Channel::Function::k_actuator1, + SRV_Channel::Function::k_actuator2, + SRV_Channel::Function::k_actuator3 + }; + + // Process legacy channels 9-11 + for (uint8_t actuator_idx = 0; actuator_idx < NUM_LEGACY_ACTUATORS; actuator_idx++) { + const uint8_t channel = FIRST_LEGACY_CHANNEL + actuator_idx; + const auto target_function = actuator_functions[actuator_idx]; + + // Skip if actuator function already assigned to any channel + uint8_t existing_channel; + if (SRV_Channels::find_channel(target_function, existing_channel)) { + continue; + } + + // Skip if channel is not disabled + if (SRV_Channels::channel_function(channel - 1) != SRV_Channel::Function::k_none) { + continue; + } + + // Check if any servo/actuator buttons are assigned + bool has_assigned_button = false; + for (size_t func_idx = 0; func_idx < FUNCTIONS_PER_SERVO; func_idx++) { + if (sub.jsbutton_function_is_assigned(servo_functions[actuator_idx][func_idx])) { + has_assigned_button = true; + break; + } + } + + if (has_assigned_button) { + // Assign actuator function to preserve legacy behavior + set_servo_function(channel, target_function); + } + } +} + +// PARAMETER_CONVERSION - Added: Mar-2026 +void Sub::update_lights_from_rcin() +{ + /* + Maps older systems from using RCIN9 and RCIN10 to using lights1 and lights2. + This is only done if there are joystick buttons assigned to the lights functions and there are no channels assigned to the lights functions. + */ + + const uint8_t NUM_LIGHTS = 2; + const uint8_t FUNCTIONS_PER_LIGHT = 3; + static constexpr JSButton::button_function_t lights_button_functions[NUM_LIGHTS][FUNCTIONS_PER_LIGHT] = { + { + JSButton::button_function_t::k_lights1_brighter, + JSButton::button_function_t::k_lights1_dimmer, + JSButton::button_function_t::k_lights1_cycle, + }, + { + JSButton::button_function_t::k_lights2_brighter, + JSButton::button_function_t::k_lights2_dimmer, + JSButton::button_function_t::k_lights2_cycle, + } + }; + + // New, dedicated lights output functions + const SRV_Channel::Function lights_functions[NUM_LIGHTS] = { + SRV_Channel::Function::k_lights1, + SRV_Channel::Function::k_lights2 + }; + + // Legacy, overloaded and hardcoded RCIN passthrough outputs + const SRV_Channel::Function rcin_functions[NUM_LIGHTS] = { + SRV_Channel::Function::k_rcin9, + SRV_Channel::Function::k_rcin10 + }; + + // Confirm the new output is not already assigned, and extract the legacy output for remapping + for (uint8_t light = 0; light < NUM_LIGHTS; light++) { + uint8_t existing_channel; + if (SRV_Channels::find_channel(lights_functions[light], existing_channel) + || !SRV_Channels::find_channel(rcin_functions[light], existing_channel)) { + continue; + } + // We have a potential lights RCIN channel. Do we have lights buttons? + for (uint8_t func_idx = 0; func_idx < FUNCTIONS_PER_LIGHT; func_idx++) { + if (sub.jsbutton_function_is_assigned(lights_button_functions[light][func_idx])) { + // We have buttons assigned to lights. Set the channel to the new, dedicated lights function. + set_servo_function(existing_channel + 1, lights_functions[light]); + break; + } + } + } +} diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index a204937ef72d4..e37bda26fb09a 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -408,6 +408,8 @@ class ParametersG2 { AP_Float surface_nobaro_thrust; Actuators actuators; + // Used to track parameter conversions + AP_Int8 param_conversion_increment; }; extern const AP_Param::Info var_info[]; diff --git a/ArduSub/ReleaseNotes.txt b/ArduSub/ReleaseNotes.txt index c597e2e84bf57..e2b4688f514f1 100644 --- a/ArduSub/ReleaseNotes.txt +++ b/ArduSub/ReleaseNotes.txt @@ -1,4 +1,66 @@ ArduPilot Sub Release Notes: +------------------------------------------------------------------ +Release 4.7.0-beta3 10-Apr-2026 + +Changes from 4.7.0-beta2 + +1) Board specific changes + +- Atlas-Control has updated hwdef.dat file (@AltasFlight, PR:32268) +- HWH7 flight controller added to supported boards (@Arwayday, PR:32248) +- TBS_LUCID_H7 now has an MPU6000 version (@andyp1per, PR:32512) + +2) Driver changes + +- ZB25VQ128D flash chip now supported for onboard logging (@LoyalLinjy, PR:32408) +- Kebni SensAItion as External AHRS now supported in IMU and INS modes (@magnus-lindhe-kebni, PR:31775) +- IST8310 compass probing corrected on fmuv3-based boards (@peterbarker, PR:32462) + +3) AHRS specific changes + +- EKF3 flash space requirements reduced (@tpwrules, PRs:32213, 32618) +- EKF3 is fixed to no longer reset to external altitude when it is configured to another source (@snktshrma, PR:32508) +- AHRS recorded origin check will correctly use the GPS (@andyp1per, PR: 32469) +- AHRS external origin setting will be delayed until filter is initialised (@andyp1per, PR: 32505) +- EKF3 missing constraints to FuseDragForces added (@tpwrules, PR:32128) +- Fix replay involving body odometry data (@twprules, PR:32598) + +4) Camera and mount specific changes + +- Camera servo driver's CAMERA_INFORMATION flags fixed (@Davidsastresas, PR:32564) +- RunCam param metadata names are fixed to match code (@tpwrules, PR:32601) +- Mount fix to angular rates when using FPV mode (@KSimeonAVTA, PR:32540) +- Mount scripting driver accepts rate targets (@peterbarker, PR:32626) +- Fix wire data issues with various Mount backends (@peterbarker, PR:32630) + +5) Other libraries changes + +- Fence for Plane and Copter supports alt above terrain (@andyp1per, PR:31619) +- Ignore-receiver in RC_OPTIONS is now respected during RC loss (@peterbarker, PR:32488) +- ATC_INPUT_TC and Q_A_INPUT_TC param defaults reduced (@rmackay9, PR:32643) + +6) Copter/Tradheli specific changes + +- Spool state is logged (@Georacer, PR:31668) + +7) Plane specific changes + +- Rangefinder state is logged (@Georacer, PR:31579) +- AHRS log message rate not affected by full rate attitude logging (@IamPete1, PR:32623) + +8) Sub specific changes + +- Automatically deal with misconfigured lights/actuators (@Williangalvani, PR:31109) + +9) Rover specific changes + +- Battery watt limiter feature added to custom build server (@mackay9, PR:32687) + +10) Scripting changes + +- plane_follow.lua messages will no longer crash the script if there is no target vehicle (@timtuxworth, PR:32567) +- plane_follow.lua has a fix for crosstrack oscillation (@timtuxworth, PR:32575) + ------------------------------------------------------------------ Release 4.7.0-beta2 21-Mar-2026 diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 17f42599e8873..cb904489696ae 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -509,6 +509,7 @@ class Sub : public AP_Vehicle { JSButton* get_button(uint8_t index); void default_js_buttons(void); void clear_input_hold(); + bool jsbutton_function_is_assigned(JSButton::button_function_t function); void read_barometer(void); void init_rangefinder(void); void read_rangefinder(void); @@ -579,6 +580,9 @@ class Sub : public AP_Vehicle { #if AP_RELAY_ENABLED void update_relay_pins(); #endif + + void update_actuators_from_jsbuttons(); + void update_lights_from_rcin(); bool handle_do_motor_test(mavlink_command_int_t command); bool init_motor_test(); bool verify_motor_test(); diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index 64d9e035baa17..404801df1d546 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -849,6 +849,33 @@ void Sub::clear_input_hold() input_hold_engaged = false; } +bool Sub::jsbutton_function_is_assigned(JSButton::button_function_t function) +{ + bool shift_is_assigned = false; + bool function_is_assigned_to_shift = false; + const uint8_t num_buttons = 32; + // Check all 32 buttons for the specified function + for (uint8_t i = 0; i < num_buttons; i++) { + JSButton* current_button = get_button(i); + if (current_button == nullptr) { + continue; + } + if (current_button->function(false) == function) { + return true; + } + if (current_button->function(false) == JSButton::k_shift) { + shift_is_assigned = true; + } + if (current_button->function(true) == function) { + if (shift_is_assigned) { + return true; + } + function_is_assigned_to_shift = true; + } + } + return shift_is_assigned && function_is_assigned_to_shift; +} + #if AP_SCRIPTING_ENABLED bool Sub::is_button_pressed(uint8_t index) { diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 7e439b71b9150..fbb9db76067b4 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -154,6 +154,14 @@ void Sub::init_ardupilot() mainloop_failsafe_enable(); ins.set_log_raw_bit(MASK_LOG_IMU_RAW); + + // PARAMETER_CONVERSION - Added: Mar-2026 + if (g2.param_conversion_increment < 1) { + update_actuators_from_jsbuttons(); + update_lights_from_rcin(); + g2.param_conversion_increment.set_and_save(1); + } + g2.actuators.initialize_actuators(); #if LEAKDETECTOR_MAX_INSTANCES > 0 diff --git a/ArduSub/version.h b/ArduSub/version.h index d01d831afd039..fed32e416307a 100644 --- a/ArduSub/version.h +++ b/ArduSub/version.h @@ -6,10 +6,10 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduSub V4.7.0-beta2" +#define THISFIRMWARE "ArduSub V4.7.0-beta3" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_BETA+1 +#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_BETA+2 #define FW_MAJOR 4 #define FW_MINOR 7 diff --git a/Rover/ReleaseNotes.txt b/Rover/ReleaseNotes.txt index b455c251d48ad..2abc17e68b0d8 100644 --- a/Rover/ReleaseNotes.txt +++ b/Rover/ReleaseNotes.txt @@ -1,4 +1,66 @@ ArduPilot Rover Release Notes: +------------------------------------------------------------------ +Release 4.7.0-beta3 10-Apr-2026 + +Changes from 4.7.0-beta2 + +1) Board specific changes + +- Atlas-Control has updated hwdef.dat file (@AltasFlight, PR:32268) +- HWH7 flight controller added to supported boards (@Arwayday, PR:32248) +- TBS_LUCID_H7 now has an MPU6000 version (@andyp1per, PR:32512) + +2) Driver changes + +- ZB25VQ128D flash chip now supported for onboard logging (@LoyalLinjy, PR:32408) +- Kebni SensAItion as External AHRS now supported in IMU and INS modes (@magnus-lindhe-kebni, PR:31775) +- IST8310 compass probing corrected on fmuv3-based boards (@peterbarker, PR:32462) + +3) AHRS specific changes + +- EKF3 flash space requirements reduced (@tpwrules, PRs:32213, 32618) +- EKF3 is fixed to no longer reset to external altitude when it is configured to another source (@snktshrma, PR:32508) +- AHRS recorded origin check will correctly use the GPS (@andyp1per, PR: 32469) +- AHRS external origin setting will be delayed until filter is initialised (@andyp1per, PR: 32505) +- EKF3 missing constraints to FuseDragForces added (@tpwrules, PR:32128) +- Fix replay involving body odometry data (@twprules, PR:32598) + +4) Camera and mount specific changes + +- Camera servo driver's CAMERA_INFORMATION flags fixed (@Davidsastresas, PR:32564) +- RunCam param metadata names are fixed to match code (@tpwrules, PR:32601) +- Mount fix to angular rates when using FPV mode (@KSimeonAVTA, PR:32540) +- Mount scripting driver accepts rate targets (@peterbarker, PR:32626) +- Fix wire data issues with various Mount backends (@peterbarker, PR:32630) + +5) Other libraries changes + +- Fence for Plane and Copter supports alt above terrain (@andyp1per, PR:31619) +- Ignore-receiver in RC_OPTIONS is now respected during RC loss (@peterbarker, PR:32488) +- ATC_INPUT_TC and Q_A_INPUT_TC param defaults reduced (@rmackay9, PR:32643) + +6) Copter/Tradheli specific changes + +- Spool state is logged (@Georacer, PR:31668) + +7) Plane specific changes + +- Rangefinder state is logged (@Georacer, PR:31579) +- AHRS log message rate not affected by full rate attitude logging (@IamPete1, PR:32623) + +8) Sub specific changes + +- Automatically deal with misconfigured lights/actuators (@Williangalvani, PR:31109) + +9) Rover specific changes + +- Battery watt limiter feature added to custom build server (@mackay9, PR:32687) + +10) Scripting changes + +- plane_follow.lua messages will no longer crash the script if there is no target vehicle (@timtuxworth, PR:32567) +- plane_follow.lua has a fix for crosstrack oscillation (@timtuxworth, PR:32575) + ------------------------------------------------------------------ Release 4.7.0-beta2 21-Mar-2026 diff --git a/Rover/version.h b/Rover/version.h index a7ad7d70d3c35..558a92999d3e0 100644 --- a/Rover/version.h +++ b/Rover/version.h @@ -6,10 +6,10 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduRover V4.7.0-beta2" +#define THISFIRMWARE "ArduRover V4.7.0-beta3" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_BETA+1 +#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_BETA+2 #define FW_MAJOR 4 #define FW_MINOR 7 diff --git a/Tools/AP_Periph/ReleaseNotes.txt b/Tools/AP_Periph/ReleaseNotes.txt index 13b27387563bb..c74ee3ade8eec 100644 --- a/Tools/AP_Periph/ReleaseNotes.txt +++ b/Tools/AP_Periph/ReleaseNotes.txt @@ -1,3 +1,67 @@ +ArduPilot AP_Periph Release Notes +------------------------------------------------------------------ +Release 4.7.0-beta3 10-Apr-2026 + +Changes from 4.7.0-beta2 + +1) Board specific changes + +- Atlas-Control has updated hwdef.dat file (@AltasFlight, PR:32268) +- HWH7 flight controller added to supported boards (@Arwayday, PR:32248) +- TBS_LUCID_H7 now has an MPU6000 version (@andyp1per, PR:32512) + +2) Driver changes + +- ZB25VQ128D flash chip now supported for onboard logging (@LoyalLinjy, PR:32408) +- Kebni SensAItion as External AHRS now supported in IMU and INS modes (@magnus-lindhe-kebni, PR:31775) +- IST8310 compass probing corrected on fmuv3-based boards (@peterbarker, PR:32462) + +3) AHRS specific changes + +- EKF3 flash space requirements reduced (@tpwrules, PRs:32213, 32618) +- EKF3 is fixed to no longer reset to external altitude when it is configured to another source (@snktshrma, PR:32508) +- AHRS recorded origin check will correctly use the GPS (@andyp1per, PR: 32469) +- AHRS external origin setting will be delayed until filter is initialised (@andyp1per, PR: 32505) +- EKF3 missing constraints to FuseDragForces added (@tpwrules, PR:32128) +- Fix replay involving body odometry data (@twprules, PR:32598) + +4) Camera and mount specific changes + +- Camera servo driver's CAMERA_INFORMATION flags fixed (@Davidsastresas, PR:32564) +- RunCam param metadata names are fixed to match code (@tpwrules, PR:32601) +- Mount fix to angular rates when using FPV mode (@KSimeonAVTA, PR:32540) +- Mount scripting driver accepts rate targets (@peterbarker, PR:32626) +- Fix wire data issues with various Mount backends (@peterbarker, PR:32630) + +5) Other libraries changes + +- Fence for Plane and Copter supports alt above terrain (@andyp1per, PR:31619) +- Ignore-receiver in RC_OPTIONS is now respected during RC loss (@peterbarker, PR:32488) +- ATC_INPUT_TC and Q_A_INPUT_TC param defaults reduced (@rmackay9, PR:32643) + +6) Copter/Tradheli specific changes + +- Spool state is logged (@Georacer, PR:31668) + +7) Plane specific changes + +- Rangefinder state is logged (@Georacer, PR:31579) +- AHRS log message rate not affected by full rate attitude logging (@IamPete1, PR:32623) + +8) Sub specific changes + +- Automatically deal with misconfigured lights/actuators (@Williangalvani, PR:31109) + +9) Rover specific changes + +- Battery watt limiter feature added to custom build server (@mackay9, PR:32687) + +10) Scripting changes + +- plane_follow.lua messages will no longer crash the script if there is no target vehicle (@timtuxworth, PR:32567) +- plane_follow.lua has a fix for crosstrack oscillation (@timtuxworth, PR:32575) + +------------------------------------------------------------------ Release 4.7.0-beta2 21-Mar-2026 Changes from 1.8.0 diff --git a/Tools/AP_Periph/version.h b/Tools/AP_Periph/version.h index 383eaadae317b..515703b143336 100644 --- a/Tools/AP_Periph/version.h +++ b/Tools/AP_Periph/version.h @@ -7,7 +7,7 @@ #include "ap_version.h" #include -#define THISFIRMWARE "AP_Periph 4.7.0-beta2" +#define THISFIRMWARE "AP_Periph 4.7.0-beta3" // defines needed due to lack of GCS includes #ifndef HAVE_ENUM_FIRMWARE_VERSION_TYPE @@ -17,7 +17,7 @@ #endif // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_BETA+1 +#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_BETA+2 #define FW_MAJOR 4 #define FW_MINOR 7 diff --git a/Tools/autotest/ArduPlane_Tests/InertialLabsEAHRS/ap1.txt b/Tools/autotest/ArduPlane_Tests/KebniSensAItionExternalIMU/ap1.txt similarity index 100% rename from Tools/autotest/ArduPlane_Tests/InertialLabsEAHRS/ap1.txt rename to Tools/autotest/ArduPlane_Tests/KebniSensAItionExternalIMU/ap1.txt diff --git a/Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS7/ap1.txt b/Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS7/ap1.txt deleted file mode 100644 index ab2be0d1a4547..0000000000000 --- a/Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS7/ap1.txt +++ /dev/null @@ -1,7 +0,0 @@ -QGC WPL 110 -0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.090027 1 -1 0 3 22 0.000000 0.000000 0.000000 0.000000 -35.361553 149.163956 20.000000 1 -2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364540 149.162857 50.000000 1 -3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.367333 149.163164 28.000000 1 -4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.366814 149.165878 28.000000 1 -5 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.362947 149.165179 0.000000 1 diff --git a/Tools/autotest/ArduPlane_Tests/VectorNavEAHRS/ap1.txt b/Tools/autotest/ArduPlane_Tests/VectorNavEAHRS/ap1.txt deleted file mode 100644 index ab2be0d1a4547..0000000000000 --- a/Tools/autotest/ArduPlane_Tests/VectorNavEAHRS/ap1.txt +++ /dev/null @@ -1,7 +0,0 @@ -QGC WPL 110 -0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.090027 1 -1 0 3 22 0.000000 0.000000 0.000000 0.000000 -35.361553 149.163956 20.000000 1 -2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364540 149.162857 50.000000 1 -3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.367333 149.163164 28.000000 1 -4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.366814 149.165878 28.000000 1 -5 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.362947 149.165179 0.000000 1 diff --git a/Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS5/ap1.txt b/Tools/autotest/Generic_Missions/externalahrs.txt similarity index 100% rename from Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS5/ap1.txt rename to Tools/autotest/Generic_Missions/externalahrs.txt diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 2ed7235ed63c9..747d82430c895 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -150,14 +150,16 @@ def takeoff(self, self.change_mode(mode) if not self.armed(): self.wait_ready_to_arm(require_absolute=require_absolute, timeout=timeout) - self.zero_throttle() + if mode != 'GUIDED': + self.zero_throttle() self.arm_vehicle() if mode == 'GUIDED': self.user_takeoff(alt_min=alt_min, timeout=timeout, max_err=max_err) else: self.set_rc(3, takeoff_throttle) self.wait_altitude(alt_min-1, alt_min+max_err, relative=True, timeout=timeout, minimum_duration=alt_minimum_duration) - self.hover() + if mode != 'GUIDED': + self.hover() self.progress("TAKEOFF COMPLETE") def land_and_disarm(self, timeout=60): @@ -199,6 +201,7 @@ def takeoffAndMoveAway(self, dAlt=50, dDist=50): 4: 1500, }) self.takeoff(alt_min=dAlt, mode='GUIDED') + self.hover() self.change_mode("ALT_HOLD") self.progress("Yaw to east") @@ -635,7 +638,7 @@ def assert_no_lateral_movement(mav, m): self.wait_circling_point_with_radius( loc, radius, epsilon=1, - min_circle_time=10, + min_circle_time=7, timeout=30, track_angle=False, ) @@ -768,7 +771,7 @@ def assert_no_lateral_movement(mav, m): def do_RTL(self, distance_min=None, check_alt=True, distance_max=10, timeout=250, quiet=False): """Enter RTL mode and wait for the vehicle to disarm at Home.""" self.change_mode("RTL") - self.hover() + self.zero_throttle() self.wait_rtl_complete(check_alt=check_alt, distance_max=distance_max, timeout=timeout, quiet=True) def wait_rtl_complete(self, check_alt=True, distance_max=10, timeout=250, quiet=False): @@ -845,6 +848,7 @@ def ThrottleFailsafe(self, side=60, timeout=360): self.start_subtest("If you haven't taken off yet RC failure should be instant disarm") self.change_mode("STABILIZE") self.set_parameter("DISARM_DELAY", 0) + self.wait_ready_to_arm() self.arm_vehicle() self.set_parameter("SIM_RC_FAIL", 1) self.disarm_wait(timeout=1) @@ -1955,19 +1959,25 @@ def HorizontalFence(self, timeout=180): "timed out after %u seconds" % (home_distance, timeout,)) # MaxAltFence - fly up until you hit the fence ceiling - def MaxAltFence(self): - '''Test Max Alt Fence''' - self.takeoff(10, mode="LOITER") - """Hold loiter position.""" + def max_alt_fence_frame(self, frame, alt, expected_breach_alt, terrain=0): + self.start_subtest("Test Max Alt Fence in frame %u" % frame) # enable fence, disable avoidance self.set_parameters({ "FENCE_ENABLE": 1, "AVOID_ENABLE": 0, "FENCE_TYPE": 1, - "FENCE_ENABLE" : 1, + "FENCE_ALT_MAX_TP": frame, + "FENCE_ALT_MAX": alt, + "TERRAIN_ENABLE": terrain, + "SIM_TERRAIN": terrain, }) + if terrain == 1: + self.install_terrain_handlers_context() + + self.takeoff(10, mode="LOITER") + """Hold loiter position.""" self.change_alt(10) # first east @@ -1989,10 +1999,30 @@ def MaxAltFence(self): # wait for fence to trigger self.wait_mode('RTL', timeout=120) + # verify breach occurred at expected altitude + self.assert_altitude(expected_breach_alt, accuracy=5) + self.wait_rtl_complete() self.zero_throttle() + # MaxAltFence - fly up until you hit the fence ceiling + def MaxAltFence(self): + '''Test Max Alt Fences''' + self.poll_home_position(quiet=False) + home_loc = self.mav.location() + origin_alt = home_loc.alt + + self.max_alt_fence_frame(0, origin_alt + 80, origin_alt + 80) # absolute + self.max_alt_fence_frame(1, 100, origin_alt + 100) # above home + + # set home 50m higher than origin to test that frame 2 uses origin not home + home_loc.alt = origin_alt + 50 + self.set_home(home_loc) + self.max_alt_fence_frame(2, 120, origin_alt + 120) # above origin + + self.max_alt_fence_frame(3, 90, origin_alt + 90, terrain=1) # above terrain + # MaxAltFence - fly up and make sure fence action does not trigger # Also check that the vehicle will not try and descend too fast when trying to backup from a max alt fence due to avoidance def MaxAltFenceAvoid(self): @@ -2046,8 +2076,16 @@ def get_climb_rate(mav, m): self.do_RTL() # fly_alt_min_fence_test - fly down until you hit the fence floor - def MinAltFence(self): + def min_alt_fence_frame(self, frame, alt, terrain=0): '''Test Min Alt Fence''' + + if terrain == 1: + self.set_parameters({ + "TERRAIN_ENABLE": terrain, + "SIM_TERRAIN": terrain, + }) + self.install_terrain_handlers_context() + self.takeoff(30, mode="LOITER", timeout=60) # enable fence, disable avoidance @@ -2055,7 +2093,8 @@ def MinAltFence(self): "AVOID_ENABLE": 0, "FENCE_ENABLE" : 1, "FENCE_TYPE": 8, - "FENCE_ALT_MIN": 20, + "FENCE_ALT_MIN": alt, + "FENCE_ALT_MIN_TP": frame, }) self.change_alt(30) @@ -2090,6 +2129,24 @@ def MinAltFence(self): self.zero_throttle() + def MinAltFence(self): + '''Test Min Alt Fences''' + self.poll_home_position(quiet=False) + home_loc = self.mav.location() + self.set_home(home_loc) + + self.min_alt_fence_frame(0, home_loc.alt + 20) # absolute + self.min_alt_fence_frame(3, 20, 1) # above terrain + self.min_alt_fence_frame(2, 20) # above origin + + # set origin below home alt - test should still pass + nz = mavutil.location(home_loc.lat, home_loc.lng, home_loc.alt - 20, 270) + self.set_origin(nz) + self.set_parameters({ + "SIM_GPS1_ENABLE": 1, + }) + self.min_alt_fence_frame(1, 20) # above home + # MinAltFenceAvoid - fly down and make sure fence action does not trigger # Also check that the vehicle will not try and ascend too fast when trying to backup from a min alt fence due to avoidance def MinAltFenceAvoid(self): @@ -2222,9 +2279,8 @@ def FenceFloorAutoDisableLanding(self): self.assert_fence_enabled() # Change to RC controlled mode - self.change_mode('LOITER') - self.set_rc(3, 1800) + self.change_mode('LOITER') self.wait_mode('RTL', timeout=120) @@ -2261,9 +2317,8 @@ def FenceFloorAutoEnableOnArming(self): self.assert_fence_enabled() # Change to RC controlled mode - self.change_mode('LOITER') - self.set_rc(3, 1800) + self.change_mode('LOITER') self.wait_mode('RTL', timeout=120) # Assert fence is not healthy now that we are in RTL @@ -6198,8 +6253,8 @@ def ManualThrottleModeChange(self): self.wait_disarmed() def constrained_mount_pitch(self, pitch_angle_deg, mount_instance=1): - PITCH_MIN = self.get_parameter("MNT%u_PITCH_MIN" % mount_instance) - PITCH_MAX = self.get_parameter("MNT%u_PITCH_MAX" % mount_instance) + PITCH_MIN = self.get_parameter("MNT%u_PITCH_MIN" % mount_instance, verbose=False) + PITCH_MAX = self.get_parameter("MNT%u_PITCH_MAX" % mount_instance, verbose=False) return min(max(pitch_angle_deg, PITCH_MIN), PITCH_MAX) def test_mount_pitch(self, despitch, despitch_tolerance, mount_mode, timeout=10, hold=0, constrained=True): @@ -6374,7 +6429,7 @@ def test_mount_rc_targetting_rate_control(self, pitch_rc_neutral=1500): self.set_rc(12, 1500) self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - def mount_test_body(self, pitch_rc_neutral=1500, do_rate_tests=True, constrain_sysid_target=True): + def mount_test_body(self, pitch_rc_neutral=1500, do_rate_tests=True, constrain_sysid_target=True, neutral_tol_deg=0): '''Test Camera/Antenna Mount - assumes a camera is set up and ready to go''' if True: # make sure we're getting gimbal device attitude status @@ -6383,10 +6438,18 @@ def mount_test_body(self, pitch_rc_neutral=1500, do_rate_tests=True, constrain_s # change mount to neutral mode (point forward, not stabilising) self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL) - # test pitch is not neutral to start with - mount_roll_deg, mount_pitch_deg, mount_yaw_deg, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg() - if mount_roll_deg != 0 or mount_pitch_deg != 0 or mount_yaw_deg != 0: - raise NotAchievedException("Mount not neutral") + # wait for mount to report neutral angles (external MAVLink gimbals may + # have in-flight messages from the previous mode in the receive queue) + tstart = self.get_sim_time() + while True: + mount_roll_deg, mount_pitch_deg, mount_yaw_deg, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg() + if (abs(mount_roll_deg) <= neutral_tol_deg and + abs(mount_pitch_deg) <= neutral_tol_deg and + abs(mount_yaw_deg) <= neutral_tol_deg): + break + if self.get_sim_time() - tstart > 5: + raise NotAchievedException("Mount not neutral (r=%f p=%f y=%f)" % ( + mount_roll_deg, mount_pitch_deg, mount_yaw_deg)) self.takeoff(30, mode='GUIDED') @@ -6401,7 +6464,9 @@ def mount_test_body(self, pitch_rc_neutral=1500, do_rate_tests=True, constrain_s # check gimbal is still not stabilising mount_roll_deg, mount_pitch_deg, mount_yaw_deg, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg() - if mount_roll_deg != 0 or mount_pitch_deg != 0 or mount_yaw_deg != 0: + if (abs(mount_roll_deg) > neutral_tol_deg or + abs(mount_pitch_deg) > neutral_tol_deg or + abs(mount_yaw_deg) > neutral_tol_deg): raise NotAchievedException("Mount stabilising when not requested") # center RC tilt control and change mount to RC_TARGETING mode @@ -6702,6 +6767,160 @@ def MountSolo(self): constrain_sysid_target=False, # not everything constrains all angles ) + def mount_check_camera_information( + self, + expected_vendor: str, + expected_model: str, + expected_fw_version: int | None = None, + expected_cap_flags: int | None = None, + expected_gimbal_device_id: int | None = None, + timeout: int = 30, + ) -> None: + '''poll CAMERA_INFORMATION, retrying until vendor name, model name and + firmware version all match what the simulated backend sends. Vendor + name is used to detect driver initialisation; model name and firmware + version may arrive in separate handshake messages so we keep retrying + on empty/zero values rather than failing immediately.''' + self.assert_receive_message('GIMBAL_DEVICE_ATTITUDE_STATUS', timeout=10, very_verbose=True) + # param2 of MAV_CMD_REQUEST_MESSAGE selects a specific camera instance + # (1-based); 0 requests all cameras. Use the gimbal_device_id directly + # as the selector so each call retrieves exactly one camera's data. + poll_p2 = expected_gimbal_device_id if expected_gimbal_device_id is not None else 0 + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > timeout: + raise NotAchievedException( + "Never received complete CAMERA_INFORMATION for vendor=%s" % expected_vendor) + try: + m = self.poll_message('CAMERA_INFORMATION', timeout=5, p2=poll_p2) + except NotAchievedException: + continue + got_vendor = bytes(m.vendor_name).split(b'\x00')[0].decode('utf-8') + if not got_vendor: + continue # driver not initialised yet + if got_vendor != expected_vendor: + raise NotAchievedException( + "Unexpected vendor name: want=%s got=%s" % (expected_vendor, got_vendor)) + got_model = bytes(m.model_name).split(b'\x00')[0].decode('utf-8') + if not got_model: + continue # model name handshake not complete yet + if got_model != expected_model: + raise NotAchievedException( + "Unexpected model name: want=%s got=%s" % (expected_model, got_model)) + if expected_fw_version is not None: + if m.firmware_version == 0: + continue # firmware version handshake not complete yet + if m.firmware_version != expected_fw_version: + raise NotAchievedException( + "Unexpected firmware version: want=0x%08x got=0x%08x" % + (expected_fw_version, m.firmware_version)) + if expected_cap_flags is not None: + if m.flags != expected_cap_flags: + raise NotAchievedException( + "Unexpected cap flags: want=0x%08x got=0x%08x" % + (expected_cap_flags, m.flags)) + return + + def MountSiyiZT30(self): + '''test Siyi ZT30 gimbal using SIM_Siyi_ZT30 simulator''' + self.set_parameters({ + "MNT1_TYPE": 8, # Siyi + "CAM1_TYPE": 4, # Mount + "SERIAL5_PROTOCOL": 8, # gimbal + "RC6_OPTION": 213, # MOUNT1_PITCH + }) + self.customise_SITL_commandline(["--serial5=sim:siyi_zt30:"]) + # camera fw 1.2.3 from SIM_Siyi_ZT30: major=1, minor=2, patch=3 + # firmware_version = major | (minor<<8) | (patch<<16) = 0x030201 + # cap flags: CAPTURE_VIDEO | CAPTURE_IMAGE | HAS_BASIC_ZOOM | HAS_BASIC_FOCUS + self.mount_check_camera_information( + "Siyi", "ZT30", + expected_fw_version=0x030201, + expected_cap_flags=0xC3, + ) + + def MountTopotek(self): + '''test Topotek gimbal using SIM_Topotek simulator''' + self.set_parameters({ + "MNT1_TYPE": 12, # Topotek + "CAM1_TYPE": 4, # Mount + "SERIAL5_PROTOCOL": 8, # gimbal + "RC6_OPTION": 213, # MOUNT1_PITCH + }) + self.customise_SITL_commandline(["--serial5=sim:topotek:"]) + # version "1.0.0" from SIM_Topotek: major=1 | (minor=0)<<8 | (patch=0)<<16 = 1 + # cap flags: CAPTURE_VIDEO | CAPTURE_IMAGE | HAS_BASIC_ZOOM | HAS_BASIC_FOCUS | + # HAS_TRACKING_POINT | HAS_TRACKING_RECTANGLE + self.mount_check_camera_information( + "Topotek", "SIM_TP", + expected_fw_version=1, + expected_cap_flags=0x6C3, + ) + # pitch_rc_neutral=1818: with RC6 min=1000 max=2000 trim=1500 and + # default MNT1_PITCH_MIN=-90 / MNT1_PITCH_MAX=20, norm_input=0.636 + # maps to exactly 0 deg pitch. + # constrain_sysid_target=False: the driver does not yet clamp pitch to + # MNT1_PITCH_MAX before sending, so the 68-deg sysid test is skipped + # until AP_Mount_Topotek::send_target_angles enforces the limit. + self.mount_test_body(pitch_rc_neutral=1818, do_rate_tests=False, + constrain_sysid_target=False) + + def MountViewPro(self): + '''test Viewpro gimbal using SIM_Viewpro simulator''' + self.set_parameters({ + "MNT1_TYPE": 11, # Viewpro + "CAM1_TYPE": 4, # Mount + "SERIAL5_PROTOCOL": 8, # gimbal + "RC6_OPTION": 213, # MOUNT1_PITCH + }) + self.customise_SITL_commandline(["--serial5=sim:viewpro:"]) + # firmware "Sys20220101" from SIM_Viewpro: major=20, minor=22, patch=1 + # firmware_version = major | (minor<<8) | (patch<<16) = 0x011614 + # cap flags: CAPTURE_VIDEO | CAPTURE_IMAGE | HAS_BASIC_ZOOM | HAS_BASIC_FOCUS | + # HAS_TRACKING_POINT | HAS_TRACKING_RECTANGLE + self.mount_check_camera_information( + "Viewpro", "SIM_VP", + expected_fw_version=0x011614, + expected_cap_flags=0x6C3, + ) + # Viewpro roll uses 12-bit encoding (0..4095 = -90..+90 deg); the + # midpoint 2047.5 is not an integer so neutral 0 deg cannot be + # represented exactly. Max quantisation error is 180/(4095*2) ≈ 0.022 deg. + # pitch_rc_neutral=1818: with RC6 min=1000 max=2000 trim=1500 and + # default MNT1_PITCH_MIN=-90 / MNT1_PITCH_MAX=20, norm_input=0.636 + # maps to exactly 0 deg pitch. + # constrain_sysid_target=False: the driver does not yet clamp pitch to + # MNT1_PITCH_MAX before sending, so the 68-deg sysid test is skipped + # until AP_Mount_Viewpro::send_target_angles enforces the limit. + self.mount_test_body(pitch_rc_neutral=1818, neutral_tol_deg=0.05, + do_rate_tests=False, constrain_sysid_target=False) + + def MountAVTCM62(self): + '''test MAVLink (Gimbal Protocol v2) gimbal using SIM_AVT_CM62 simulator''' + self.set_parameters({ + "MNT1_TYPE": 6, # MAVLink + "CAM1_TYPE": 4, # Mount + "SERIAL5_PROTOCOL": 2, # MAVLink2 + "RC6_OPTION": 213, # MOUNT1_PITCH + }) + self.customise_SITL_commandline(["--serial5=sim:avt_cm62_gimbal:"]) + + def MountAVTCM62Dual(self): + '''test two simultaneous MAVLink (Gimbal Protocol v2) gimbals using + two SIM_AVT_CM62 simulators on separate serial ports''' + self.set_parameters({ + "MNT1_TYPE": 6, # MAVLink + "MNT2_TYPE": 6, # MAVLink + "CAM1_TYPE": 4, # Mount + "CAM2_TYPE": 4, # Mount + "SERIAL5_PROTOCOL": 2, # MAVLink2 + "SERIAL6_PROTOCOL": 2, # MAVLink2 + }) + self.customise_SITL_commandline([ + "--serial5=sim:avt_cm62_gimbal:", + "--serial6=sim:avt_cm62_gimbal:", + ]) + def assert_mount_rpy(self, r, p, y, tolerance=1): '''assert mount atttiude in degrees''' got_r, got_p, got_y, yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg() @@ -11275,6 +11494,40 @@ def test_replay_wind_and_airspeed_bit(self): return current_log_filepath + def test_replay_body_odom_bit(self): + self.set_parameters({ + "LOG_REPLAY": 1, + "LOG_DISARMED": 1, + + "EK3_SRC1_POSXY": 6, + "EK3_SRC1_VELXY": 6, + "EK3_SRC1_POSZ": 6, + "EK3_SRC1_VELZ": 6, + + "SIM_VICON_TMASK": 8, # send VISION_POSITION_DELTA for FuseBodyOdom + + "VISO_TYPE": 2, + "SERIAL5_PROTOCOL": 2, + "ARMING_SKIPCHK": (1 << 18), # suppress yaw misalignment complaint + }) + + self.customise_SITL_commandline(["--serial5=sim:vicon:"]) # implicit reboot + + self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_LOGGING, True, True, True) + + current_log_filepath = self.current_onboard_log_filepath() + self.progress("Current log path: %s" % str(current_log_filepath)) + + self.change_mode("LOITER") + self.wait_ready_to_arm(require_absolute=False) + self.arm_vehicle() + self.takeoffAndMoveAway() + self.do_RTL() + + self.customise_SITL_commandline([]) # implicit reboot + + return current_log_filepath + def GPSBlendingLog(self): '''Test GPS Blending''' '''ensure we get dataflash log messages for blended instance''' @@ -11637,6 +11890,7 @@ def Replay(self): ('GPS', self.test_replay_gps_bit), ('GPSForYaw', self.test_replay_gps_yaw_bit), ('WindAndAirspeed', self.test_replay_wind_and_airspeed_bit), + ('BodyOdom', self.test_replay_body_odom_bit), ('Beacon', self.test_replay_beacon_bit), ('OpticalFlow', self.test_replay_optical_flow_bit), ] @@ -12137,7 +12391,7 @@ def SMART_RTL_EnterLeave(self): def SMART_RTL_Repeat(self): '''Test whether Smart RTL catches the repeat''' self.takeoff(alt_min=10, mode='GUIDED') - self.set_rc(3, 1500) + self.hover() self.change_mode("CIRCLE") self.delay_sim_time(1300) self.change_mode("SMART_RTL") @@ -13035,6 +13289,21 @@ def tests1e(self): self.MAV_CMD_NAV_VTOL_LAND, self.clear_roi, self.ReadOnlyDefaults, + self.FenceRelativePreArms, + self.FenceRelativeToHomeMaxAlt, + self.FenceRelativeToHomeMinAlt, + self.FenceRelativeToHomeMaxAltOriginAbove, + self.FenceRelativeToHomeMinAltOriginAbove, + self.FenceRelativeToHomeCliff, + self.FenceRelativeToOriginMaxAlt, + self.FenceRelativeToOriginMinAlt, + self.FenceRelativeToOriginMaxAltHomeAbove, + self.FenceRelativeToOriginMinAltHomeAbove, + self.FenceRelativeToAMSLMaxAlt, + self.FenceRelativeToAMSLMinAlt, + self.FenceRelativeToAMSLCliff, + self.FenceRelativeToTerrainMaxAlt, + self.FenceRelativeToTerrainMinAlt, ]) return ret @@ -13171,6 +13440,121 @@ def ScriptMountAllModes(self): self.do_RTL() + def ScriptMountDriver(self): + '''test scripting mount driver with all modes and camera''' + self.context_push() + + self.set_parameters({ + "SCR_ENABLE": 1, + "MNT1_TYPE": 9, + "MNT1_PITCH_MIN": -45, + "MNT1_PITCH_MAX": 45, + }) + self.reboot_sitl() + + self.install_example_script_context('mount-driver.lua') + self.context_collect('STATUSTEXT') + self.reboot_sitl() + + self.wait_statustext("MountDriver: started", check_context=True, timeout=30) + self.wait_ready_to_arm() + + self.takeoff(20, mode='GUIDED') + + # test RETRACT mode - exercises angle_converted path + self.start_subtest("RETRACT mode") + retract_pitch = -15 + self.set_parameter("MNT1_RETRACT_Y", retract_pitch) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p7=mavutil.mavlink.MAV_MOUNT_MODE_RETRACT, + ) + self.test_mount_pitch(retract_pitch, 1, mavutil.mavlink.MAV_MOUNT_MODE_RETRACT) + + # test NEUTRAL mode - exercises angle_converted path + self.start_subtest("NEUTRAL mode") + neutral_pitch = -10 + self.set_parameter("MNT1_NEUTRAL_Y", neutral_pitch) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p7=mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL, + ) + self.test_mount_pitch(neutral_pitch, 1, mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL) + + # test MAVLINK_TARGETING with angle + self.start_subtest("MAVLINK_TARGETING") + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p1=20, # pitch + p2=0, # roll + p3=0, # yaw + p7=mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, + ) + self.test_mount_pitch(20, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING) + + # test MAVLINK_TARGETING with rate + self.start_subtest("MAVLINK_TARGETING rate") + # start at pitch 0 + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, + p1=0, # pitch angle + p2=0, # yaw angle + ) + self.test_mount_pitch(0, 5, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING) + # send pitch rate of -30 deg/s for 2 seconds + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, + p1=float('nan'), # pitch angle (NaN = use rate) + p2=float('nan'), # yaw angle (NaN = use rate) + p3=-30, # pitch rate deg/s + p4=0, # yaw rate deg/s + ) + self.delay_sim_time(2) + # expect pitch around -60 + _, mount_pitch, _, _ = self.get_mount_roll_pitch_yaw_deg() + if abs(mount_pitch - (-60)) > 20: + raise NotAchievedException( + "Rate mode pitch incorrect: got=%f want=-60 (+/-20)" % mount_pitch) + self.progress("Rate mode pitch correct: %f degrees (~-60)" % mount_pitch) + + # test GPS_POINT (ROI) + self.start_subtest("GPS_POINT (ROI)") + takeoff_loc = self.mav.location() + t = self.offset_location_ne(takeoff_loc, 20, 0) + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, + p5=int(t.lat * 1e7), + p6=int(t.lng * 1e7), + p7=0, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + ) + # We took off to 20m and the target is 20m North, so pitch should be around -45 degrees + self.test_mount_pitch(-45, 5, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT) + + # Reposition the aircraft 20m South of the takeoff location, + # so it's now 40m from the target, and pitch should be around + # -27 degrees + t = self.offset_location_ne(takeoff_loc, -20, 0) + self.send_set_position_target_global_int(int(t.lat * 1e7), int(t.lng * 1e7), 20) + self.test_mount_pitch(-27, 5, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT, constrained=False) + + # test HOME_LOCATION + self.start_subtest("HOME_LOCATION") + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p7=mavutil.mavlink.MAV_MOUNT_MODE_HOME_LOCATION, + ) + # We are 20m South of home and at 20m altitude, so pitch should be around -45 degrees + self.test_mount_pitch(-45, 5, mavutil.mavlink.MAV_MOUNT_MODE_HOME_LOCATION, constrained=False) + + # Reposition over home again, pitch should move to -90 degrees + self.send_set_position_target_global_int(int(takeoff_loc.lat * 1e7), int(takeoff_loc.lng * 1e7), 20) + self.test_mount_pitch(-90, 5, mavutil.mavlink.MAV_MOUNT_MODE_HOME_LOCATION, constrained=False) + + self.do_RTL() + self.context_pop() + self.reboot_sitl() + def ScriptCopterPosOffsets(self): '''test the copter-posoffset.lua example script''' self.context_push() @@ -13934,7 +14318,7 @@ def PILOT_THR_BHV(self): "DISARM_DELAY": 0, }) self.takeoff(2, mode='GUIDED') - self.set_rc(3, 1500) + self.hover() self.change_mode('LOITER') self.set_rc(3, 1300) @@ -13947,7 +14331,7 @@ def PILOT_THR_BHV(self): }) self.zero_throttle() self.takeoff(2, mode='GUIDED') - self.set_rc(3, 1500) + self.hover() self.change_mode('LOITER') self.set_rc(3, 1300) @@ -13978,6 +14362,7 @@ def CameraLogMessages(self): attitudes.append(self.assert_receive_message('ATTITUDE')) self.set_rc(12, 1000) + self.hover() self.change_mode('LOITER') self.set_rc(1, 1000) self.set_rc(2, 1000) @@ -14027,6 +14412,7 @@ def LoiterToGuidedHomeVSOrigin(self): self.takeoff(10, mode='GUIDED') here = self.mav.location() self.set_home(here) + self.hover() self.change_mode('LOITER') self.wait_altitude(here.alt-1, here.alt+1, minimum_duration=10) self.disarm_vehicle(force=True) @@ -14474,7 +14860,9 @@ def AutoContinueOnRCFailsafe(self): self.assert_mode_is('AUTO') self.change_mode('LOITER') self.wait_groundspeed(0, 0.1, minimum_duration=30, timeout=450) - self.do_RTL() + # do_RTL plays with RC inputs, don't use it. + self.change_mode("RTL") + self.wait_rtl_complete() def MissionRTLYawBehaviour(self): '''check end-of-mission yaw behaviour''' @@ -15228,6 +15616,17 @@ def verify_yaw(mav, m): self.arm_vehicle() self.wait_disarmed() + def RC_OPTIONS_1_FS_THR_ENABLE_0(self): + '''check behaviour with unusual RC parameters''' + self.set_parameters({ + "RC_OPTIONS": 1, + "FS_THR_ENABLE": 0, + }) + self.reboot_sitl() + self.takeoff(10, mode='GUIDED') + self.wait_yaw_speed(0, minimum_duration=10) + self.do_land() + def WaitAndMaintainAttitude_RCFlight(self): '''just test WaitAndMaintainAttitude works''' WaitAndMaintainAttitude(self, 0, 0, epsilon=1).run() @@ -15471,6 +15870,15 @@ def PeriphMultiUARTTunnel(self): if ex is not None: raise ex + def FenceRelative_TakeoffMode(self): + '''method for the FenceRelative test to call''' + return 'LOITER' + + def FenceRelative_params(self): + ret = super().FenceRelative_params() + ret["AVOID_ENABLE"] = 0 + return ret + def RCProtocolFailsafe(self): '''ensure we failsafe when the RC protocol failsafe is set''' self.takeoff(10, mode='LOITER') @@ -15843,8 +16251,14 @@ def tests2b(self): # this block currently around 9.5mins here self.ThrottleGainBoost, self.ScriptMountPOI, self.ScriptMountAllModes, + self.ScriptMountDriver, self.ScriptCopterPosOffsets, self.MountSolo, + self.MountSiyiZT30, + self.MountTopotek, + self.MountViewPro, + self.MountAVTCM62, + self.MountAVTCM62Dual, self.FlyMissionTwice, self.FlyMissionTwiceWithReset, self.MissionIndexValidity, @@ -15898,6 +16312,7 @@ def tests2b(self): # this block currently around 9.5mins here self.AHRSOriginRecorded, self.TestTetherStuck, self.ScriptingFlipMode, + self.RC_OPTIONS_1_FS_THR_ENABLE_0, self.ScriptingFlyVelocity, self.EK3_EXT_NAV_vel_without_vert, self.CompassLearnCopyFromEKF, diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 2d6fde18c644f..6800d3025f751 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3010,7 +3010,13 @@ def TerrainLoiterToCircle(self): self.wait_altitude(alt*0.9, alt*1.1, minimum_duration=10, relative=True) self.fly_home_land_and_disarm() - def fly_external_AHRS(self, sim, eahrs_type, mission): + def fly_generic_mission(self, filename, mission_timeout=60.0, strict=True): + """Fly a mission from the Generic_Missions directory.""" + self.progress("Flying generic mission %s" % filename) + num_wp = self.load_generic_mission(filename, strict=strict) - 1 + self.fly_mission_waypoints(num_wp, mission_timeout=mission_timeout) + + def fly_external_AHRS(self, sim, eahrs_type): """Fly with external AHRS""" self.customise_SITL_commandline(["--serial4=sim:%s" % sim]) @@ -3033,7 +3039,7 @@ def fly_external_AHRS(self, sim, eahrs_type, mission): self.wait_ready_to_arm() self.arm_vehicle() - self.fly_mission(mission) + self.fly_generic_mission("externalahrs.txt") def wait_and_maintain_wind_estimate( self, @@ -3232,19 +3238,69 @@ def test_replay_wind_and_airspeed_bit(self): def VectorNavEAHRS(self): '''Test VectorNav EAHRS support''' - self.fly_external_AHRS("VectorNav", 1, "ap1.txt") + self.fly_external_AHRS("VectorNav", 1) def MicroStrainEAHRS5(self): '''Test MicroStrain EAHRS series 5 support''' - self.fly_external_AHRS("MicroStrain5", 2, "ap1.txt") + self.fly_external_AHRS("MicroStrain5", 2) def MicroStrainEAHRS7(self): '''Test MicroStrain EAHRS series 7 support''' - self.fly_external_AHRS("MicroStrain7", 7, "ap1.txt") + self.fly_external_AHRS("MicroStrain7", 7) def InertialLabsEAHRS(self): '''Test InertialLabs EAHRS support''' - self.fly_external_AHRS("ILabs", 5, "ap1.txt") + self.fly_external_AHRS("ILabs", 5) + + def KebniSensAItionExternalINS(self): + '''Test Kebni SensAItion External INS mode. Or in Ardupilot terminology, ExternalAHRS mode.''' + self.set_parameters({ + "EAHRS_OPTIONS": 4 # INS mode -> Bit 2 set + }) + self.fly_external_AHRS("SensAItionINS", 11) + + def KebniSensAItionExternalIMU(self): + '''Test Kebni SensAItion External IMU-only mode''' + self.customise_SITL_commandline(["--serial4=sim:SensAItion"]) + + self.set_parameters({ + # External AHRS configuration (IMU-only mode at 1000Hz) + "EAHRS_TYPE": 11, # SensAItion External AHRS type + "EAHRS_SENSORS": 14, # IMU(2) + Baro(4) + Compass(8) = 14 + "EAHRS_OPTIONS": 0, # Legacy IMU mode + "SERIAL4_PROTOCOL": 36, # External AHRS protocol + "SERIAL4_BAUD": 460800, # 460800 baud + "GPS1_TYPE": 1, # Use SITL GPS for position data + "AHRS_EKF_TYPE": 3, # Internal EKF3 (fed by external IMU) + "EK3_PRIMARY": 0, # Use external IMU (SensAItion) as primary + "EK3_OPTIONS": 2, # Disable EK3 lane switching + + # Ultra high-rate INS filtering + "INS_GYR_CAL": 1, + }) + self.reboot_sitl() + self.delay_sim_time(10) + + self.progress("Running accelcal") + self.run_cmd( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p5=4, + timeout=10, + ) + + # Wait for EKF3 with external IMU to converge + expected_flags = (mavutil.mavlink.ESTIMATOR_ATTITUDE | + mavutil.mavlink.ESTIMATOR_VELOCITY_HORIZ | + mavutil.mavlink.ESTIMATOR_VELOCITY_VERT | + mavutil.mavlink.ESTIMATOR_POS_HORIZ_REL | + mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS | + mavutil.mavlink.ESTIMATOR_POS_VERT_ABS) + self.wait_ekf_flags(expected_flags, 0, timeout=60) + + self.wait_ready_to_arm(timeout=120) + self.arm_vehicle() + self.fly_mission("ap1.txt", mission_timeout=120) + self.disarm_vehicle(force=True) def GpsSensorPreArmEAHRS(self): '''Test pre-arm checks related to EAHRS_SENSORS using the MicroStrain7 driver''' @@ -7995,6 +8051,8 @@ def tests1b(self): self.MicroStrainEAHRS5, self.MicroStrainEAHRS7, self.InertialLabsEAHRS, + self.KebniSensAItionExternalINS, + self.KebniSensAItionExternalIMU, self.GpsSensorPreArmEAHRS, self.Deadreckoning, self.EKFlaneswitch, diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 6eb6b8c32f04e..4b337fb1fed09 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -2858,6 +2858,10 @@ def ScriptedArmingChecksApplet(self): self.set_parameter("Q_OPTIONS", 1) self.wait_text("ArmCk: note: Q will land", check_context=True) + def FenceRelative_TakeoffMode(self): + '''method for the FenceRelative test to call''' + return 'QLOITER' + def TerrainAvoidApplet(self): '''Terrain Avoidance with CMTC''' self.start_subtest("Terrain Avoidance Load and Start") @@ -3162,5 +3166,20 @@ def tests(self): self.ScriptedArmingChecksApplet, self.TerrainAvoidApplet, self.TakeoffCheck, + self.FenceRelativePreArms, + self.FenceRelativeToHomeMaxAlt, + self.FenceRelativeToHomeMinAlt, + self.FenceRelativeToHomeMaxAltOriginAbove, + self.FenceRelativeToHomeMinAltOriginAbove, + self.FenceRelativeToHomeCliff, + self.FenceRelativeToOriginMaxAlt, + self.FenceRelativeToOriginMinAlt, + self.FenceRelativeToOriginMaxAltHomeAbove, + self.FenceRelativeToOriginMinAltHomeAbove, + self.FenceRelativeToAMSLMaxAlt, + self.FenceRelativeToAMSLMinAlt, + self.FenceRelativeToAMSLCliff, + self.FenceRelativeToTerrainMaxAlt, + self.FenceRelativeToTerrainMinAlt, ]) return ret diff --git a/Tools/autotest/test_build_options.py b/Tools/autotest/test_build_options.py index 3c19164b4fc10..f1b24609c688a 100755 --- a/Tools/autotest/test_build_options.py +++ b/Tools/autotest/test_build_options.py @@ -324,7 +324,6 @@ def define_is_whitelisted_for_feature_in_code(self, target, define): feature_define_whitelist.add('QAUTOTUNE_ENABLED') feature_define_whitelist.add('AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED') feature_define_whitelist.add('HAL_QUADPLANE_ENABLED') - feature_define_whitelist.add('AP_BATTERY_WATT_MAX_ENABLED') feature_define_whitelist.add('MODE_AUTOLAND_ENABLED') feature_define_whitelist.add('AP_PLANE_GLIDER_PULLUP_ENABLED') feature_define_whitelist.add('AP_QUICKTUNE_ENABLED') @@ -343,8 +342,12 @@ def define_is_whitelisted_for_feature_in_code(self, target, define): # required for these bindings: feature_define_whitelist.add('AP_SCRIPTING_BINDING_MOTORS_ENABLED') + if target.lower() not in ["plane", "rover"]: + # only Plane and Rover support battery watt limiting + feature_define_whitelist.add('AP_BATTERY_WATT_MAX_ENABLED') + if target.lower() not in ["rover", "copter"]: - # only Plane and Copter instantiate Beacon + # only Rover and Copter instantiate Beacon feature_define_whitelist.add('AP_BEACON_ENABLED') if target.lower() != "rover": diff --git a/Tools/autotest/tilecache/srtm/N21W157.hgt.zip b/Tools/autotest/tilecache/srtm/N21W157.hgt.zip new file mode 100644 index 0000000000000..4d28e1331721e Binary files /dev/null and b/Tools/autotest/tilecache/srtm/N21W157.hgt.zip differ diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index f8981e35075e5..8c4b08eb7fe4f 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -2816,6 +2816,8 @@ def all_log_format_ids(self): continue if "#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED" in line: continue + if "#if AP_RANGEFINDER_ENABLED" in line: + continue if "#end" in line: continue if "LOG_COMMON_STRUCTURES" in line: @@ -9702,6 +9704,15 @@ def offset_location_ne(self, location, metres_north, metres_east): location.alt, location.heading) + def offset_location_up(self, location, metres_up): + '''return a new location offset from passed-in location''' + return mavutil.location( + location.lat, + location.lng, + location.alt + metres_up, + location.heading + ) + def offset_location_heading_distance(self, location, bearing, distance): (target_lat, target_lng) = mavextra.gps_newpos( location.lat, @@ -11383,7 +11394,7 @@ def test_set_message_interval_basic(self): if ex is not None: raise ex - def send_poll_message(self, message_id, target_sysid=None, target_compid=None, quiet=False, mav=None): + def send_poll_message(self, message_id, target_sysid=None, target_compid=None, quiet=False, mav=None, p2=0): if mav is None: mav = self.mav if isinstance(message_id, str): @@ -11391,13 +11402,14 @@ def send_poll_message(self, message_id, target_sysid=None, target_compid=None, q self.send_cmd( mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, p1=message_id, + p2=p2, target_sysid=target_sysid, target_compid=target_compid, quiet=quiet, mav=mav, ) - def poll_message(self, message_id, timeout=10, quiet=False, mav=None, target_sysid=None, target_compid=None): + def poll_message(self, message_id, timeout=10, quiet=False, mav=None, target_sysid=None, target_compid=None, p2=0): if mav is None: mav = self.mav if target_sysid is None: @@ -11407,7 +11419,7 @@ def poll_message(self, message_id, timeout=10, quiet=False, mav=None, target_sys if isinstance(message_id, str): message_id = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % message_id) tstart = self.get_sim_time() # required for timeout in run_cmd_get_ack to work - self.send_poll_message(message_id, quiet=quiet, mav=mav, target_sysid=target_sysid, target_compid=target_compid) + self.send_poll_message(message_id, quiet=quiet, mav=mav, target_sysid=target_sysid, target_compid=target_compid, p2=p2) self.run_cmd_get_ack( mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, mavutil.mavlink.MAV_RESULT_ACCEPTED, @@ -14901,6 +14913,454 @@ def _MotorTest(self, self.delay_sim_time(10) self.end_subtest("Testing percentage output") + def FenceRelative_fly_north_then_descend(self, north_m, timeout=120): + '''fly north_m metres north in GUIDED mode then descend. + + Uses MAV_CMD_DO_REPOSITION (wpnav) to fly off the KalaupapaCliffs + cliff edge where terrain drops to near sea level. After arriving, + switches back to the vehicle's loiter mode and lowers throttle to + descend below the min altitude fence floor. + Caller must call wait_mode('RTL') to confirm the fence breach. + ''' + current_loc = self.mav.location() + target_loc = self.offset_location_heading_distance(current_loc, 0, north_m) + + # At KalaupapaCliffs the terrain rises ~40 m in the first 100 m + # north (interpolated from the 100 m SRTM grid) before dropping + # off the cliff face to near sea level. 215 m AMSL clears this + # ridge (~206 m AMSL) while staying 10 m below both tests' max + # fence altitude (225 m AMSL), so use the higher of the current + # altitude and 215 m AMSL. + reposition_alt_amsl = max(current_loc.alt, 215.0) + + # fly to target using GUIDED mode waypoint navigation + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_REPOSITION, + -1, # ground speed (-1 = use default) + 1, # flags: change mode to GUIDED + 0, # loiter radius + 0, # yaw (no change) + int(target_loc.lat * 1e7), + int(target_loc.lng * 1e7), + reposition_alt_amsl, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL, + ) + self.wait_location(target_loc, accuracy=50, height_accuracy=None, + timeout=timeout) + + # switch back to loiter mode and descend to breach the fence floor + self.change_mode(self.FenceRelative_TakeoffMode()) + self.set_rc(3, 1200) + + def FenceRelative_params(self): + '''returns a set of base parameters for FenceRelative test''' + return { + "FENCE_ALT_MAX_TP": 1, # 1 is above-home + "FENCE_ENABLE": 1, + "FENCE_TYPE": 9, # ALT_MAX|ALT_MIN + "FENCE_ALT_MAX": 20, + } + + def FenceRelativePreArms(self): + '''test fence altitude types''' + self.customise_SITL_commandline([ + "--home", "KalaupapaCliffs", + ]) + self.set_parameters(self.FenceRelative_params()) + self.wait_ready_to_arm() + original_home = self.home_position_as_mav_location() + + self.start_subtest("Above home-relative fence") + self.set_home(self.offset_location_up(original_home, -2)) + self.set_parameters({ + "FENCE_ALT_MIN": -10, + "FENCE_ALT_MAX": 0, + }) + self.assert_prearm_failure("PreArm: Vehicle breaching Max Alt fence") + self.customise_SITL_commandline([]) + + def FenceRelativeToHome_params(self): + '''returns parameters for FenceRelativeToHome test''' + ret = self.FenceRelative_params() + ret["FENCE_ALT_MIN_TP"] = 1 # above-home (ALT_MAX_TP=1 already in base) + return ret + + def FenceRelativeToOrigin_params(self): + '''returns parameters for FenceRelativeToOrigin test''' + ret = self.FenceRelative_params() + ret["FENCE_ALT_MAX_TP"] = 2 # above EKF origin + ret["FENCE_ALT_MIN_TP"] = 2 # above EKF origin + return ret + + def FenceRelativeToAMSL_params(self): + '''returns parameters for FenceRelativeToAMSL test''' + ret = self.FenceRelative_params() + ret["FENCE_ALT_MAX_TP"] = 0 # AMSL + ret["FENCE_ALT_MIN_TP"] = 0 # AMSL + # disable fence at setup time: with AMSL frame the base FENCE_ALT_MAX=20 + # would immediately breach at KalaupapaCliffs (~165 m AMSL) + ret["FENCE_ENABLE"] = 0 + return ret + + def FenceRelativeToTerrain_params(self): + '''returns parameters for FenceRelativeToTerrain test''' + ret = self.FenceRelative_params() + ret["FENCE_ALT_MAX_TP"] = 3 # above terrain + ret["FENCE_ALT_MIN_TP"] = 3 # above terrain + ret["TERRAIN_ENABLE"] = 1 + return ret + + def FenceRelativeToHomeMaxAlt(self): + '''fence max-alt threshold is measured relative to home, not EKF origin''' + self.set_parameters(self.FenceRelativeToHome_params()) + self.wait_ready_to_arm() + original_home = self.home_position_as_mav_location() + home_ofs = 20 + fence_alt_max = 20 # m above home = 40 m above origin + offset_home = self.offset_location_up(original_home, home_ofs) + self.set_home(offset_home) + self.set_parameters({ + "FENCE_TYPE": 1, # ALT_MAX only + "FENCE_ALT_MAX": fence_alt_max, + }) + self.takeoff(10, mode=self.FenceRelative_TakeoffMode()) + self.do_fence_enable() + self.assert_mode_is(self.FenceRelative_TakeoffMode()) + self.set_rc(3, 1800) + self.wait_mode('RTL', timeout=120) + expected_breach_alt = offset_home.alt + fence_alt_max + self.assert_altitude(expected_breach_alt, accuracy=10) + self.disarm_vehicle(force=True) + self.set_home(original_home) + + def FenceRelativeToHomeMinAlt(self): + '''fence min-alt threshold is measured relative to home, not EKF origin''' + fence_alt_min = 5 # m above home = 25 m above origin + params = copy.copy(self.FenceRelativeToHome_params()) + params.update({ + "FENCE_TYPE": 8, # ALT_MIN only + "FENCE_ALT_MIN": fence_alt_min, + "FENCE_ALT_MAX": 50, # generous ceiling + }) + self.set_parameters(params) + self.wait_ready_to_arm() + original_home = self.home_position_as_mav_location() + home_ofs = 20 + offset_home = self.offset_location_up(original_home, home_ofs) + self.set_home(offset_home) + self.takeoff(10, mode=self.FenceRelative_TakeoffMode()) + self.do_fence_enable() + self.set_rc(3, 1200) + self.wait_mode('RTL', timeout=120) + expected_breach_alt = offset_home.alt + fence_alt_min + self.assert_altitude(expected_breach_alt, accuracy=10) + self.disarm_vehicle(force=True) + self.set_home(original_home) + + def FenceRelativeToHomeMaxAltOriginAbove(self): + '''fence max-alt relative to home when origin is above home''' + self.set_parameters(self.FenceRelativeToHome_params()) + self.wait_ready_to_arm() + original_home = self.home_position_as_mav_location() + home_ofs = -20 + fence_alt_max = 30 # m above home = 10 m above origin + offset_home = self.offset_location_up(original_home, home_ofs) + self.set_home(offset_home) + self.set_parameters({ + "FENCE_TYPE": 1, # ALT_MAX only + "FENCE_ALT_MAX": fence_alt_max, + }) + self.takeoff(25, mode=self.FenceRelative_TakeoffMode()) + self.do_fence_enable() + self.assert_mode_is(self.FenceRelative_TakeoffMode()) + self.set_rc(3, 1800) + self.wait_mode('RTL', timeout=120) + expected_breach_alt = offset_home.alt + fence_alt_max + self.assert_altitude(expected_breach_alt, accuracy=10) + self.disarm_vehicle(force=True) + self.set_home(original_home) + + def FenceRelativeToHomeMinAltOriginAbove(self): + '''fence min-alt relative to home when origin is above home''' + # 26 m above home = 6 m above origin; wrong origin-frame would + # see vehicle (10 m above origin) as below the 26 m fence → breach + fence_alt_min = 26 + params = self.FenceRelativeToHome_params() + params.update({ + "FENCE_TYPE": 8, # ALT_MIN only + "FENCE_ALT_MIN": fence_alt_min, + "FENCE_ALT_MAX": 50, # generous ceiling + }) + self.set_parameters(params) + self.wait_ready_to_arm() + original_home = self.home_position_as_mav_location() + home_ofs = -20 + offset_home = self.offset_location_up(original_home, home_ofs) + self.set_home(offset_home) + self.takeoff(30, mode=self.FenceRelative_TakeoffMode()) + self.do_fence_enable() + self.assert_mode_is(self.FenceRelative_TakeoffMode()) + self.set_rc(3, 1200) + self.wait_mode('RTL', timeout=120) + expected_breach_alt = offset_home.alt + fence_alt_min + self.assert_altitude(expected_breach_alt, accuracy=10) + self.disarm_vehicle(force=True) + self.set_home(original_home) + + def FenceRelativeToHomeCliff(self): + '''home-relative min fence below arming altitude requires cliff to breach''' + self.install_terrain_handlers_context() + self.customise_SITL_commandline(["--home", "KalaupapaCliffs"]) + fence_alt_min = 5 # m above home = ~150 m AMSL, 15 m below arming alt + params = self.FenceRelativeToHome_params() + params.update({ + "FENCE_TYPE": 8, # ALT_MIN only + "FENCE_ALT_MIN": fence_alt_min, + "FENCE_ALT_MAX": 80, # generous ceiling + "FENCE_RADIUS": 10000, # ensure circle fence cannot limit north flight + "TERRAIN_ENABLE": 1, + }) + self.set_parameters(params) + self.wait_ready_to_arm() + original_home = self.home_position_as_mav_location() + home_ofs = -20 + offset_home = self.offset_location_up(original_home, home_ofs) + self.set_home(offset_home) + self.takeoff(25, mode=self.FenceRelative_TakeoffMode()) + self.do_fence_enable() + self.assert_mode_is(self.FenceRelative_TakeoffMode()) + self.FenceRelative_fly_north_then_descend(150) + self.wait_mode('RTL', timeout=120) + expected_breach_alt = offset_home.alt + fence_alt_min + self.assert_altitude(expected_breach_alt, accuracy=10) + self.disarm_vehicle(force=True) + self.customise_SITL_commandline([]) + + def FenceRelativeToOriginMaxAlt(self): + '''fence max-alt threshold is measured relative to EKF origin, not home''' + self.set_parameters(self.FenceRelativeToOrigin_params()) + self.wait_ready_to_arm() + origin_alt_m = self.poll_message("GPS_GLOBAL_ORIGIN").altitude / 1000.0 + fence_alt_max = 10 # m above origin = 30 m above home + original_home = self.home_position_as_mav_location() + self.set_home(self.offset_location_up(original_home, -20)) + self.set_parameters({ + "FENCE_TYPE": 1, # ALT_MAX only + "FENCE_ALT_MAX": fence_alt_max, + }) + self.takeoff(25, mode=self.FenceRelative_TakeoffMode()) + self.do_fence_enable() + self.assert_mode_is(self.FenceRelative_TakeoffMode()) + self.set_rc(3, 1800) + self.wait_mode('RTL', timeout=120) + expected_breach_alt = origin_alt_m + fence_alt_max + self.assert_altitude(expected_breach_alt, accuracy=10) + self.disarm_vehicle(force=True) + self.set_home(original_home) + + def FenceRelativeToOriginMinAlt(self): + '''fence min-alt threshold is measured relative to EKF origin, not home''' + fence_alt_min = 3 # m above origin = 23 m above home + params = self.FenceRelativeToOrigin_params() + params.update({ + "FENCE_TYPE": 8, # ALT_MIN only + "FENCE_ALT_MIN": fence_alt_min, + "FENCE_ALT_MAX": 50, # generous ceiling + }) + self.set_parameters(params) + self.wait_ready_to_arm() + origin_alt_m = self.poll_message("GPS_GLOBAL_ORIGIN").altitude / 1000.0 + original_home = self.home_position_as_mav_location() + self.set_home(self.offset_location_up(original_home, -20)) + self.takeoff(25, mode=self.FenceRelative_TakeoffMode()) + self.do_fence_enable() + self.set_rc(3, 1200) + self.wait_mode('RTL', timeout=120) + expected_breach_alt = origin_alt_m + fence_alt_min + self.assert_altitude(expected_breach_alt, accuracy=10) + self.disarm_vehicle(force=True) + self.set_home(original_home) + + def FenceRelativeToOriginMaxAltHomeAbove(self): + '''fence max-alt relative to origin when home is above origin''' + self.set_parameters(self.FenceRelativeToOrigin_params()) + self.wait_ready_to_arm() + ground_loc = self.home_position_as_mav_location() + origin_alt_m = self.poll_message("GPS_GLOBAL_ORIGIN").altitude / 1000.0 + fence_alt_max = 50 # m above origin = 30 m above home + # take off first from home==origin so relative alt starts at 0 + self.takeoff(10, mode=self.FenceRelative_TakeoffMode()) + # now move home 20 m above origin; vehicle at 10 m above origin + # is safely below the fence max at 50 m above origin + original_home = self.mav.location() + self.set_home(self.offset_location_up(original_home, 10)) + self.set_parameters({ + "FENCE_TYPE": 1, # ALT_MAX only + "FENCE_ALT_MAX": fence_alt_max, + }) + self.do_fence_enable() + self.assert_mode_is(self.FenceRelative_TakeoffMode()) + self.set_rc(3, 1800) + self.wait_mode('RTL', timeout=120) + expected_breach_alt = origin_alt_m + fence_alt_max + self.assert_altitude(expected_breach_alt, accuracy=10) + self.disarm_vehicle(force=True) + self.set_home(ground_loc) + + def FenceRelativeToOriginMinAltHomeAbove(self): + '''fence min-alt relative to origin when home is above origin''' + # 15 m above origin = 180 m AMSL (5 m below home); vehicle at + # 10 m above home = 30 m above origin is above the origin fence. + # A home-frame interpretation would place the fence at 15 m above + # home = 200 m AMSL, above the vehicle at 195 m → immediate breach. + fence_alt_min = 15 + params = self.FenceRelativeToOrigin_params() + params.update({ + "FENCE_TYPE": 8, # ALT_MIN only + "FENCE_ALT_MIN": fence_alt_min, + "FENCE_ALT_MAX": 80, # generous ceiling + }) + self.set_parameters(params) + self.wait_ready_to_arm() + ground_loc = self.home_position_as_mav_location() + origin_alt_m = self.poll_message("GPS_GLOBAL_ORIGIN").altitude / 1000.0 + # take off first from home==origin so relative alt starts at 0 + self.takeoff(30, mode=self.FenceRelative_TakeoffMode()) + # now move home 20 m above origin; vehicle at 30 m above origin + # is safely above the fence min at 15 m above origin + original_home = self.mav.location() + self.set_home(self.offset_location_up(original_home, -10)) + self.do_fence_enable() + self.assert_mode_is(self.FenceRelative_TakeoffMode()) + self.set_rc(3, 1200) + self.wait_mode('RTL', timeout=120) + expected_breach_alt = origin_alt_m + fence_alt_min + self.assert_altitude(expected_breach_alt, accuracy=10) + self.disarm_vehicle(force=True) + self.set_home(ground_loc) + + def FenceRelativeToAMSLMaxAlt(self): + '''fence max-alt threshold is interpreted as AMSL, not home-relative''' + self.set_parameters(self.FenceRelativeToAMSL_params()) + # reboot clears any lingering breach state from prior tests + self.reboot_sitl() + self.wait_ready_to_arm() + ggo = self.poll_message("GPS_GLOBAL_ORIGIN") + origin_alt_m = ggo.altitude / 1000.0 # mm -> m + fence_max_amsl = origin_alt_m + 25 + self.set_parameters({ + "FENCE_TYPE": 1, # ALT_MAX only + "FENCE_ALT_MAX": fence_max_amsl, + }) + self.takeoff(15, mode=self.FenceRelative_TakeoffMode()) + self.do_fence_enable() + self.assert_mode_is(self.FenceRelative_TakeoffMode()) + self.set_rc(3, 1800) + self.wait_mode('RTL', timeout=120) + expected_breach_alt = fence_max_amsl + self.assert_altitude(expected_breach_alt, accuracy=10) + self.disarm_vehicle(force=True) + + def FenceRelativeToAMSLMinAlt(self): + '''fence min-alt threshold is interpreted as AMSL, not home-relative''' + self.set_parameters(self.FenceRelativeToAMSL_params()) + # reboot clears any lingering breach state from prior tests + self.reboot_sitl() + self.wait_ready_to_arm() + ggo = self.poll_message("GPS_GLOBAL_ORIGIN") + origin_alt_m = ggo.altitude / 1000.0 # mm -> m + fence_min_amsl = origin_alt_m + 10 + self.set_parameters({ + "FENCE_TYPE": 8, # ALT_MIN only + "FENCE_ALT_MIN": fence_min_amsl, + "FENCE_ALT_MAX": origin_alt_m + 50, # generous AMSL ceiling + }) + self.takeoff(20, mode=self.FenceRelative_TakeoffMode()) + self.do_fence_enable() + self.assert_mode_is(self.FenceRelative_TakeoffMode()) + self.set_rc(3, 1200) + self.wait_mode('RTL', timeout=120) + expected_breach_alt = fence_min_amsl + self.assert_altitude(expected_breach_alt, accuracy=10) + self.disarm_vehicle(force=True) + + def FenceRelativeToAMSLCliff(self): + '''AMSL min fence below arming altitude requires cliff to breach''' + self.install_terrain_handlers_context() + self.customise_SITL_commandline(["--home", "KalaupapaCliffs"]) + self.set_parameters(self.FenceRelativeToAMSL_params()) + self.wait_ready_to_arm() + ggo = self.poll_message("GPS_GLOBAL_ORIGIN") + origin_alt_m = ggo.altitude / 1000.0 # mm -> m + fence_min_below_arming = origin_alt_m - 15 + self.set_parameters({ + "FENCE_TYPE": 8, # ALT_MIN only + "FENCE_ALT_MIN": fence_min_below_arming, + "FENCE_ALT_MAX": origin_alt_m + 60, # generous AMSL ceiling + "FENCE_RADIUS": 10000, # ensure circle fence cannot limit north flight + "TERRAIN_ENABLE": 1, + }) + self.takeoff(20, mode=self.FenceRelative_TakeoffMode()) + self.do_fence_enable() + self.assert_mode_is(self.FenceRelative_TakeoffMode()) + self.FenceRelative_fly_north_then_descend(150) + self.wait_mode('RTL', timeout=120) + expected_breach_alt = fence_min_below_arming + self.assert_altitude(expected_breach_alt, accuracy=10) + self.disarm_vehicle(force=True) + self.customise_SITL_commandline([]) + + def FenceRelativeToTerrainMaxAlt(self): + '''fence max-alt threshold is interpreted as AGL (terrain-relative)''' + self.install_terrain_handlers_context() + self.set_parameters(self.FenceRelativeToTerrain_params()) + self.wait_ready_to_arm() + original_home = self.home_position_as_mav_location() + # home is placed 20 m below terrain; terrain AMSL ≈ original_home.alt + terrain_alt_amsl = original_home.alt + fence_alt_max = 10 # m AGL = 30 m above home + offset_home = self.offset_location_up(original_home, -20) + self.set_home(offset_home) + self.set_parameters({ + "FENCE_TYPE": 1, # ALT_MAX only + "FENCE_ALT_MAX": fence_alt_max, + }) + self.takeoff(25, mode=self.FenceRelative_TakeoffMode()) + self.do_fence_enable() + self.assert_mode_is(self.FenceRelative_TakeoffMode()) + self.set_rc(3, 1800) + self.wait_mode('RTL', timeout=120) + expected_breach_alt = terrain_alt_amsl + fence_alt_max + self.assert_altitude(expected_breach_alt, accuracy=10) + self.disarm_vehicle(force=True) + self.set_home(original_home) + + def FenceRelativeToTerrainMinAlt(self): + '''fence min-alt threshold is interpreted as AGL (terrain-relative)''' + self.install_terrain_handlers_context() + fence_alt_min = 3 # m AGL = 23 m above home + params = self.FenceRelativeToTerrain_params() + params.update({ + "FENCE_TYPE": 8, # ALT_MIN only + "FENCE_ALT_MIN": fence_alt_min, + "FENCE_ALT_MAX": 50, # generous ceiling + }) + self.set_parameters(params) + self.wait_ready_to_arm() + original_home = self.home_position_as_mav_location() + # home is placed 20 m below terrain; terrain AMSL ≈ original_home.alt + terrain_alt_amsl = original_home.alt + offset_home = self.offset_location_up(original_home, -20) + self.set_home(offset_home) + self.takeoff(25, mode=self.FenceRelative_TakeoffMode()) + self.do_fence_enable() + self.set_rc(3, 1200) + self.wait_mode('RTL', timeout=120) + expected_breach_alt = terrain_alt_amsl + fence_alt_min + self.assert_altitude(expected_breach_alt, accuracy=10) + self.disarm_vehicle(force=True) + self.set_home(original_home) + def MotorTest(self, timeout=60, **kwargs): '''Run Motor Tests''' # common to Copter and QuadPlane self._MotorTest(self.run_cmd, **kwargs) diff --git a/Tools/bootloaders/HWH7_bl.bin b/Tools/bootloaders/HWH7_bl.bin new file mode 100644 index 0000000000000..391cc8c9a2b74 Binary files /dev/null and b/Tools/bootloaders/HWH7_bl.bin differ diff --git a/Tools/bootloaders/HWH7_bl.hex b/Tools/bootloaders/HWH7_bl.hex new file mode 100644 index 0000000000000..8543455103e72 --- /dev/null +++ b/Tools/bootloaders/HWH7_bl.hex @@ -0,0 +1,2482 @@ +:020000040800F2 +:1000000000060020E1020008E3020008E302000805 +:10001000E3020008E3020008E3020008E30200082C +:10002000E3020008E3020008E30200080177000889 +:10003000E3020008E3020008E3020008E30200080C +:10004000E3020008E3020008E3020008E3020008FC +:10005000E3020008E3020008E3020008E3020008EC +:10006000E3020008E3020008E3020008CD460008AE +:10007000F5460008214700084D4700087947000869 +:10008000A1470008CD470008E3020008E30200088A +:10009000E3020008E3020008E3020008E3020008AC +:1000A000E3020008E3020008E3020008E30200089C +:1000B000E3020008E3020008E3020008E30200088C +:1000C000E3020008E3020008E3020008E30200087C +:1000D000E3020008E3020008E3020008E30200086C +:1000E000E3020008E3020008E3020008E30200085C +:1000F000E3020008E3020008E3020008F9470008F1 +:10010000E30200080D86000835860008E3020008B7 +:10011000E3020008E3020008E3020008E30200082B +:10012000254800084D48000879480008A5480008FF +:10013000D1480008E3020008E3020008E3020008D7 +:10014000E3020008E3020008E3020008E3020008FB +:10015000F94800082549000851490008218600088F +:10016000E3020008E3020008E3020008E3020008DB +:10017000E3020008ED820008E3020008E302000841 +:10018000E3020008E3020008E3020008E3020008BB +:10019000E3020008E3020008E3020008E3020008AB +:1001A000E3020008E3020008E3020008E30200089B +:1001B000E3020008E3020008E3020008E30200088B +:1001C000E3020008E3020008E3020008E30200087B +:1001D000E3020008D9820008E3020008E3020008F5 +:1001E000E3020008E3020008E3020008E30200085B +:1001F000E3020008E3020008E3020008E30200084B +:10020000E3020008E3020008E3020008E30200083A +:10021000E3020008E3020008E3020008E30200082A +:10022000E3020008E3020008E3020008E30200081A +:10023000E3020008E3020008E3020008E30200080A +:10024000E3020008E3020008E3020008E3020008FA +:10025000E3020008E3020008E3020008E3020008EA +:10026000E3020008E3020008E3020008E3020008DA +:10027000E3020008E3020008E3020008E3020008CA +:10028000E3020008E3020008E3020008E3020008BA +:10029000E3020008E3020008E3020008E3020008AA +:1002A000E3020008E3020008E3020008E30200089A +:1002B000E3020008E3020008E3020008E30200088A +:1002C000E3020008E3020008E3020008E30200087A +:1002D000E3020008E3020008E3020008E30200086A +:1002E00002E000F000F8FEE772B6374880F30888B5 +:1002F000364880F3098836483649086040F20000E5 +:10030000CCF200004EF63471CEF200010860BFF36B +:100310004F8FBFF36F8F40F20000C0F2F0004EF637 +:100320008851CEF200010860BFF34F8FBFF36F8F8B +:100330004FF00000E1EE100A4EF63C71CEF20001E3 +:100340000860062080F31488BFF36F8F07F03AFA35 +:1003500006F0B6FB4FF055301F491B4A91423CBF97 +:1003600041F8040BFAE71D49184A91423CBF41F895 +:10037000040BFAE71A491B4A1B4B9A423EBF51F83D +:10038000040B42F8040BF8E700201849184A914280 +:100390003CBF41F8040BFAE707F052FA06F014FCF0 +:1003A000144C154DAC4203DA54F8041B8847F9E7A6 +:1003B00000F0C4F9114C124DAC4203DA54F8041B9E +:1003C0008847F9E707F03ABA00060020002200202B +:1003D0000000000808ED00E00000002000060020FA +:1003E000889A0008002200206C2200207022002041 +:1003F000D45D0020E0020008E0020008E0020008EE +:10040000E00200082DE9F04F2DED108AC1F80CD064 +:10041000D0F80CD0BDEC108ABDE8F08F002383F338 +:1004200011882846A047002006F02CF9FEE706F0C8 +:100430007FF800DFFEE7000053B94AB9002908BF82 +: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 +:1007400095FA01F051FC06F0D7FE054606F0CAFF07 +:100750000446D0B90F4B9D4219D001339D4241F25E +:10076000883504BF01240025002006F0CFFE0CB11F +:1007700000F076F801F0D6FB00F02EFD08B100F095 +:100780006FF8284600F01AF9F9E70025ECE705466E +:10079000EAE700BF010007B008B501F033FAA0F1A5 +:1007A00020035842584108BD07B5042101900DEBC4 +:1007B000010001F045FA03B05DF804FB38B53023C1 +:1007C00083F31188174803680BB106F03FF8002344 +:1007D000154A4FF47A71134806F02EF8002383F37C +:1007E0001188124C236813B12368013B23606368AE +:1007F00013B16368013B63600D4D2B7833B96368B7 +:100800007BB9022001F0F8FA322363602B78032BC6 +:1008100007D163682BB9022001F0EEFA4FF47A7326 +:10082000636038BD70220020BD07000890230020BF +:1008300088220020084B187003280CD8DFE800F04D +:1008400008050208022001F0CFBA022001F0C2BA66 +:10085000024B00225A60704788220020902300201B +:10086000F8B5504B504A1C461968013100F0998088 +:1008700004339342F8D162684C4B9A4240F2918023 +:100880004B4B9B6803F1006303F500339A4280F001 +:100890008880002001F010FA0220FFF7CBFF454BC3 +:1008A0000021D3F8E820C3F8E810D3F81021C3F8EA +:1008B0001011D3F81021D3F8EC20C3F8EC10D3F8C2 +:1008C0001421C3F81411D3F81421D3F8F020C3F87D +:1008D000F010D3F81821C3F81811D3F81821D3F861 +:1008E000802042F00062C3F88020D3F8802022F0FC +:1008F0000062C3F88020D3F88020D3F8802042F033 +:100900000072C3F88020D3F8802022F00072C3F870 +:100910008020D3F8803072B64FF0E023C3F8084D42 +:10092000D4E90004BFF34F8FBFF36F8F224AC2F8A0 +:100930008410BFF34F8F536923F480335361BFF3A7 +:100940004F8FD2F8803043F6E076C3F3C905C3F386 +:100950004E335B0103EA060C29464CEA81770139E4 +:10096000C2F87472F9D2203B13F1200FF2D1BFF319 +:100970004F8FBFF36F8FBFF34F8FBFF36F8F5369ED +:1009800023F4003353610023C2F85032BFF34F8F7A +:10099000BFF36F8F302383F31188854680F3088877 +:1009A0002047F8BD0000020820000208FFFF0108F0 +:1009B000002200200044025800ED00E02DE9F04F35 +:1009C00095B0DFF8D8922022FF21029004A8D9F830 +:1009D000085001F051FAA84A1378A3B90121A74899 +:1009E00011700360302383F3118803680BB105F0A5 +:1009F0002DFF0023A24A4FF47A71A04805F01CFF96 +:100A0000002383F31188029B13B19E4B029A1A6054 +:100A10009D4A1378032B03D000231370994A536027 +:100A20004FF0000B029C5E46DA46CDF804B0012080 +:100A300001F0DAF924B1934B1B68002B00F02882F7 +:100A4000002001F0DFF8B0F10008F3DB012001F035 +:100A5000C1F9A8F121031F2BE9D801A252F823F014 +:100A6000E10A0008FD0A00088B0B00082F0A0008A5 +:100A70002F0A00082F0A0008150C0008070E0008AE +:100A8000F90C0008550D00087B0D0008A10D0008A9 +:100A90002F0A0008B30D0008D70D00084F0E0008FC +:100AA000690B00082F0A0008970E0008ED0A0008DD +:100AB000690B00082F0A0008550D00082F0A0008CE +:100AC0002F0A00082F0A00082F0A00082F0A000822 +:100AD0002F0A00082F0A00082F0A00088B0B0008B5 +:100AE0000220FFF759FE002840F0FF81029B0221FF +:100AF0006648BBF1000F08BF1C4640E04FF47A7017 +:100B000001F080F8071EF1DB0220FFF745FE002808 +:100B1000ECD0013F052F00F2EA81DFE807F0030780 +:100B20000A0D10330520FFF73FFE14E0D9F800004E +:100B3000F9E7D9F80400F6E7D9F80800F3E74FF031 +:100B40001C08404608F1040801F0B8F8FFF72CFE35 +:100B5000B8F12C0FF5D10120019B4FF0000A00FAEB +:100B600007F71F43FBB2019301F0D2F92EB1019BAD +:100B700003F00B030B2B08BF00240221444801F0B3 +:100B80005FF854E7D9F80C00CDE7002EAED0019BFA +:100B900003F00B030B2BA9D10220FFF7FDFD074645 +:100BA0000028A3D00120002601F086F80220FFF7DC +:100BB00041FE1FFA86FB584601F08EF8044688B1C4 +:100BC000A8F14002584601365142514101F094F8D3 +:100BD0000028EED1BB46044602212E483E4601F0D5 +:100BE0002FF824E725460120FFF724FED9F8083026 +:100BF000AB4207D9284601F061F8013040F07A8114 +:100C00000435F3E70025204BBB463E461D701D4BC7 +:100C10005D60A9E7002E3FF469AF019B03F00B0371 +:100C20000B2B7FF463AF0220FFF704FE322000F0AD +:100C3000E9FFB0F10008FFF659AF18F003077FF4A1 +:100C400055AF08EB0503D9F8082093423FF64EAFA5 +:100C5000B8F5807F3FF74AAF0F4BB84503931FDDD0 +:100C60004FF47A7000F0CEFF0028FFF63FAF039BF1 +:100C7000013703F8010BF0E78C23002070220020DD +:100C8000BD0700089023002088220020449000081F +:100C90003C900008409000088C2200200022002098 +:100CA000C820FFF779FD074600283FF41FAF1F2D2E +:100CB00011D8C5F1200204AB25F003008F4942454D +:100CC000184428BF4246039201F0B0F8039AFF216E +:100CD0008A4801F0D1F84FEAA803C8F38702874990 +:100CE0002846039301F0D0F8064600283FF474AF7D +:100CF000039B05EB830537E70220FFF74DFD002836 +:100D00003FF4F4AE01F016F800283FF4EFAE0027F0 +:100D1000B846D9F80830BB4218D91F2F11D8049B08 +:100D200001330ED027F0030314AA134453F8403CB8 +:100D30000C93404604220CA9043701F067F9804661 +:100D4000E7E7384600F0BAFF0C90F2E74046FFF7BD +:100D50002BFD09E7002364210CA80C9300F066FF2B +:100D600000287FF4C3AE0220FFF716FD00283FF4F1 +:100D7000BDAE0C9800F0D4FFE9E7002364210CA875 +:100D80000C9300F053FF00287FF4B0AE0220FFF771 +:100D900003FD00283FF4AAAE0C9800F0C3FFD6E78D +:100DA0000220FFF7F9FC00283FF4A0AE00F0D2FFCC +:100DB000CDE70220FFF7F0FC00283FF497AE0CA926 +:100DC000142000F0CDFF0390FFF7EEFC03990CA870 +:100DD00000F036FFC8E60220FFF7DEFC00283FF4F3 +:100DE00085AE474B0CAF03F1100C1868083353F86D +:100DF000041C3A46634503C21746F6D11020FFF79C +:100E0000D3FC1021E3E7322000F0FCFE071EFFF6C2 +:100E10006DAEBB077FF46AAE07EB0A03D9F8082072 +:100E200093423FF663AE0220FFF7B6FC00283FF482 +:100E30005DAE27F003075744BA453FF495AE5046E0 +:100E40000AF1040A00F03AFFFFF7AEFCF4E74FF4B2 +:100E50007A70FFF7A1FC00283FF448AE00F06AFF6B +:100E6000002849D0049B01330BD0082204A900209C +:100E700001F00AF800283FD02022FF2104A800F04A +:100E8000FBFF2048022100F0DBFE1F4805F000FCBC +:100E900015B0BDE8F08F002E3FF428AE019B03F0A3 +:100EA0000B030B2B7FF422AE002364210CA80C93C0 +:100EB00000F0BCFE074600287FF418AE0220FFF7C2 +:100EC0006BFC804600283FF411AE02210D4800F073 +:100ED000B7FE41F2883005F0DBFB0C9801F062F8B8 +:100EE00046463C4601F014F8A1E506463CE64FF0C4 +:100EF000000AFBE5BB466FE637466DE68C22002014 +:100F0000289000083C900008A0860100704700006F +:100F100070470000704700002DE9F04100F5803770 +:100F2000044616463B7C5BB9C0681030204400F094 +:100F30007DFFE5683544B5F5004FE56002D816B190 +:100F4000BDE8F081DEB905F07F0605F11000002153 +:100F5000C6F180062044F6B232462E4400F08CFFE3 +:100F6000A06804F11008324600F10060414600F527 +:100F7000003006F061F830B901233B74E0E74FF42C +:100F800000463546ECE7A26805F1100140463244C0 +:100F90002144A260E268521BE26000F047FF022099 +:100FA000BDE8F04100F028BF183000F0E9BC0000B7 +:100FB00010B5044601F046F8204610BD10B50446B1 +:100FC00001F040F8204610BDC0B2A0F14103052B4E +:100FD00002D83738C0B27047A0F16103052B94BF27 +:100FE00057383038F6E7000070B504461546084615 +:100FF00003220C4900F024FF014688B908346E1C16 +:1010000015F91100FFF7E0FF024616F91100013152 +:10101000FFF7DAFF102940EA021004F8010BEFD1C4 +:1010200070BD00BF489000082DE9F04FADF53F7D41 +:101030000746416801222AA802F0F4FE002840F089 +:1010400087800646824681461125DFF80C81DFF84D +:101050000CB101AB4FF4805241462AA803F042F88C +:10106000002875D1019AB2F5805F71D8002A65D049 +:101070000446019A9442ECD2282D0FD008DC132D9F +:101080002DD01E2D39D0112D13D00134A4B2F0E78C +:10109000322D2DD0372D2FD02D2DF6D13B68121BA0 +:1010A00008EB040138461B692D259847BDF804401C +:1010B000EBE7121B022A09D9594608EB040000F09D +:1010C000BFFE18B902342825A4B2DEE718F80430B0 +:1010D0003A2B3DD00A2B1CBFA1461325D5E718F8A3 +:1010E00004300A2B34D03A2B04BFA2463225CCE779 +:1010F00018F80430202BC8D0264618F804300A2BE4 +:101100001AD1AAEB090208EB090102A811254F2AFE +:1011100028BF4F2207F074FFA21B08EB060116A898 +:101120004F2A28BF4F2207F06BFF3B6816AA02A97F +:10113000DB6838469847A8E71E25A6E73B6838468F +:1011400004491B69984701200DF53F7DBDE8F08FEC +:101150000020F9E74A910008982300204C900008ED +:1011600000F1180110B5044686B00846019100F060 +:10117000F1FB2046FFF758FF60B1019902A800F08B +:1011800049FC102204F1080102A807F0F9FEB0FAA8 +:1011900080F0400906B010BD70B504460025EEB2DF +:1011A000304600F099FD58B100213046013500F07D +:1011B000A3FD08B9002070BD022000F01DFEEEE77F +:1011C0002046FFF731FF0028F4D004F58034207C5E +:1011D00080F00100EFE70000F0B5C9B006F0A4F917 +:1011E00000F04CFF18B90025284649B0F0BD69460B +:1011F0002A4803F039F800284BD1294C204603F047 +:1012000063F8284803F060F8274803F05DF82146AA +:10121000224803F0D5F80028E5D1702000F00CFF3B +:10122000064610B1214B44600360336830469B682A +:101230009847054600282ED01A4F1948394603F022 +:10124000BFF805460028CED1194800F0F5FE044647 +:1012500038B1184B4760036000F58033C0E9025590 +:101260001D74236820469B689847054628B10E499F +:101270000C4803F0A5F80028B5D1336830465B6808 +:1012800098471CB1236820465B68984700F0DEFE53 +:10129000AAE70025FAE70446EFE700BF50900008F0 +:1012A00060900008779000088D900008B0900008CA +:1012B00014000100CC9000082DE9F04FD44A8DB005 +:1012C0000B68D0F804A001931A440368D14E1A4465 +:1012D000D1F81C90DFF8B4C3DFF8B4B3D0E902341E +:1012E000634003EA0A03634013444A6802920AEB2C +:1012F0007363029CC84A2244C468224484688AEA10 +:1013000004051D40654015448A68039203EB35557A +:10131000039CC24A2244846822448AEA03042C4083 +:1013200084EA0A041444CA6805EBF43404921644AF +:1013300083EA0502224056445A4032440E6905961B +:1013400004EBB222059FB64E3E441E4485EA0403D8 +:1013500013406B4033444E69069602EB7363069F5D +:10136000B04E3E442E4484EA02051D40654035449B +:101370008E69079603EB3555079FAB4E3E442644D6 +:1013800082EA03042C4054403444A84E4E4405EBFA +:10139000F434164483EA050222405A4032440E6A6D +:1013A000089604EBB222089FA14E3E441E4485EAF3 +:1013B000040313406B4033444E6A099602EB736397 +:1013C000099F9C4ED1F830E03E44D1F83880F34478 +:1013D0002E4484EA02051D40654035448E6AA6F518 +:1013E000244703EB35550A964F3F274482EA03040E +:1013F0002C4054403C44CF6A0B9705EBF4340B9ED1 +:101400008D4F3744029E174483EA050222405A401A +:101410003A448A4F774404EBB2221F4485EA04031E +:1014200013406B403B444F6BBC4402EB7363654419 +:1014300084EA020C0CEA030C8CEA040C6544DFF825 +:1014400054C2C44403EB3555A44482EA03042C403F +:1014500054406444D1F83CC0794905EBF43461440C +:10146000114483EA050222405A400A44754904EBBC +:10147000B2223144079E194484EA02032B406340A0 +:101480000B44714902EBF36331440B9E0D4482EA35 +:1014900003012140514029446C4D03EBF151354487 +:1014A000019E254483EA010414405C402C44684DAD +:1014B00001EBB4443544069E154481EA04021A4007 +:1014C0004A402A44634D04EB323235440A9E1D449F +:1014D00084EA02030B4063402B445F4D02EBF3634D +:1014E0003544059E0D4482EA0301214051402944C0 +:1014F0005A4D03EBF1516544254483EA010414403D +:101500005C402C44564D01EBB4443544099E1544CF +:1015100081EA04021A404A402A44524D04EB323216 +:101520003544049E1D4484EA02030B4063402B446F +:101530004D4D02EBF36345440D4482EA0301214023 +:1015400051402944494D03EBF1513544089E2C4448 +:1015500083EA010515405D402C44454D01EBB44440 +:101560003544039E2A4481EA04051D404D402A4427 +:10157000404D04EB32323D442B4484EA020593444F +:101580000D4065402B443C4D02EBF3633544069E11 +:10159000294482EA0305254055402944374D03EB91 +:1015A000F1514D442C4483EA010515405D4025442A +:1015B00001EBB54581EA050404EA03024A405A44B6 +:1015C000A6F5B82B089E05EB3232ABF2BE6B544049 +:1015D0005B4423442A4C344402EB33730B9E0C448B +:1015E00085EA020159402144264C344403EB7151F1 +:1015F000029E254482EA03044C402544224C444484 +:1016000001EB3545144483EA01026A40224443E079 +:1016100078A46AD7EECEBDC156B7C7E8DB702024E8 +:10162000AF0F7CF52AC68747134630A8019546FDC3 +:10163000D8988069AFF7448BBED75C892211906B34 +:101640002108B44962251EF640B340C0515A5E26B7 +:10165000AAC7B6E95D102FD65314440281E6A1D87B +:10166000C8FBD3E7E6CDE121D60737C3870DD5F414 +:10167000ED145A4505E9E3A9F8A3EFFCD9026F6719 +:1016800081F6718722619D6D0C38E5FD937198FD9F +:101690008A4C2A8D8E4379A6934C344405EB7222F2 +:1016A000059E1C4481EA0503534023448F4C344477 +:1016B00002EB33730A9E0C4485EA0201594021442F +:1016C0008B4C4C4403EB7151254482EA03044C409B +:1016D0002C44884D354401EB3444019E154483EA83 +:1016E000010262402A44844D3D4404EB72221D44B1 +:1016F00081EA040353402B44804D354402EB33739D +:10170000049E294484EA02055D4029447C4D354409 +:1017100003EB7151079E254482EA03044C402C449C +:10172000784D354401EB3444099E2A4483EA01058F +:1017300065401544744A324404EB7525039E1344F6 +:1017400081EA04026A401A44704B734405EB32721A +:101750000B4484EA0501514019446D4B634402EB8C +:1017600071511C4485EA02034B401C44694B3344CD +:1017700001EB3444019E1D4482EA010363402B4483 +:10178000654D04EB73233544069E154463EA01025C +:1017900062402A44614D03EBB2624D4462EA04099F +:1017A00029445F4D89EA0309454449442C445D4D71 +:1017B00002EBB1513544049E61EA03081D4488EAF6 +:1017C0000208444401EB744464EA02034B402B4496 +:1017D000554D04EBF323754463EA010E15448EEA7C +:1017E000040E0EEB0502514D03EBB262354462EA82 +:1017F000040E29440A9D8EEA030EA5F580164C4D71 +:101800007144A6F6833602EBB151264461EA030423 +:1018100054403444029E01EB7444354464EA0206A9 +:101820001D444E407319089E424D04EBF32335448A +:1018300063EA01061544664072193F4D03EBB2623C +:10184000654462EA040629443C4D5E403144079EEB +:1018500002EBB151354461EA03062C44384D564041 +:101860003D443444059E1D4401EB744464EA020384 +:101870004B402B44334D04EBF32335440B9E15446E +:1018800063EA010262402A442F4D03EBB262354401 +:10189000039E0D4462EA0401594029442B4D02EB9A +:1018A000B15135442A4E254461EA030454402C4486 +:1018B000099D01EB74442E4464EA02051E4485EA46 +:1018C00001039D1903681A440AEB040303EBF52393 +:1018D0000260436083681C44C36819448460C1602B +:1018E0000DB0BDE8F08F00BF44EABEA4A9CFDE4B27 +:1018F000604BBBF670BCBFBEC67E9B28FA27A1EA30 +:101900008530EFD4051D880439D0D4D9E599DBE6BC +:10191000F87CA21F6556ACC4442229F497FF2A43E1 +:10192000A72394AB39A093FCC3595B6592CC0C8F71 +:10193000D15D84854F7EA86FE0E62CFE144301A3A1 +:10194000A111084E827E53F735F23ABDBBD2D72A99 +:1019500091D386EB094B036003F18833436003F1B5 +:101960002943A3F59613A3F68B638360A3F1883311 +:10197000C3600023C0E90433704700BF01234567FB +:101980002DE9F8431446026905460E46E300C2F30A +:10199000C50800F118079B18036122BF4369013392 +:1019A000436112F4FC7F436903EB5473436114D029 +:1019B000C8F1400907EB08004C4504D22246BDE8B7 +:1019C000F84307F0EDBA403C4A464E4407F0E8FAC7 +:1019D000444439462846FFF76FFCA04606EB04094D +:1019E000B8F13F0FA9EB08010AD94022384607F0A9 +:1019F000D7FA39462846A8F14008FFF75DFCEFE723 +:101A0000A1096FF03F02384602FB014206EB81114B +:101A1000D5E7000070B50B6901F1180506460C46C4 +:101A2000C3F3C503EA18501C8022EA54C3F13F02F5 +:101A3000072A1FD8002100F01FFA29462046FFF789 +:101A40003BFC38220021284600F016FA236929467B +:101A50002046236563696365FFF72EFC214610224B +:101A6000304607F09DFA204658220021BDE870401C +:101A700000F002BAC3F137020021E5E72DE9F84F83 +:101A80004FF47A7306460D46002402FB03F7DFF895 +:101A90005080DFF8509098F900305FFA84FA5A1CB1 +:101AA00001D0A34212D159F824002A4631460368D6 +:101AB000D3F820B03B46D847854207D1074B0120D9 +:101AC00083F800A0BDE8F88F0124E4E7002CFBD0E8 +:101AD0004FF4FA7004F0DCFD0020F3E7DC33002063 +:101AE000102200201422002000232DE9F0471A467E +:101AF000054688460120194602F0D2FF04468020A0 +:101B000005F0B8FB0746002861D0002C5FD03B4EA3 +:101B100028464FF0800A736943F0C0637361D6F8BA +:101B2000143143F0C063C6F8143105F025FA05EB13 +:101B3000080200FB05F34FF4D06C50434FF03D0E0C +:101B4000834233D23269520143D4D6F81021510175 +:101B50003FD4216803F100628A602268D76022685E +:101B6000C2F804A02268C2F800C0D4F80090D9F8E6 +:101B7000002042F00102C9F80020D4F80090D9F802 +:101B8000002022F01E02C9F80020D4F80090D9F8F5 +:101B90000020D207FBD494F80C90803361680EFAD1 +:101BA00009F20A60CCE7002005F0C4FA204602F0F2 +:101BB000B7FF384605F09AFB104B5A6942F0C062F5 +:101BC0005A61D3F8142142F0C062C3F81421BDE871 +:101BD000F0870B4B9A6A42F400729A629A6A42F456 +:101BE00000429A6261B60026012005F0A3FAB045D2 +:101BF000D9D9A819013605F0D7F9F8E70020005225 +:101C000000ED00E0002307B5024601210DF10700B9 +:101C10008DF80730FFF732FF20B19DF8070003B0C1 +:101C20005DF804FB4FF0FF30F9E700000A4604219D +:101C300008B5FFF723FF80F00100C0B2404208BDA5 +:101C4000074B0A4630B41978064B53F82140014639 +:101C500023682046DD69044BAC4630BC604700BFBA +:101C6000DC33002014220020A086010070B5104C47 +:101C70000025104E05F084F92080306823888342C7 +:101C80000CD800252088013805F076F92388054412 +:101C9000013BB5F5802F2380F4D370BD05F06CF9BE +:101CA000336805440133B5F5003F3360E5D3E8E719 +:101CB000DE3300209833002005F03CBA00F10060CC +:101CC00000F500300068704700F10060920000F5F8 +:101CD000003005F0B1B90000054B1A68054B1B88B0 +:101CE0009B1A834202D9104405F046B90020704780 +:101CF00098330020DE33002038B50446074D29B163 +:101D000028682044BDE8384005F04EB928682044D2 +:101D100005F038F90028F3D038BD00BF9833002013 +:101D20000020704700F1FF5000F58F10D0F8000838 +:101D300070470000064991F8243033B1002308228F +:101D4000086A81F82430FFF7BFBF0120704700BF49 +:101D50009C330020014B1868704700BF0010005CE6 +:101D6000194B01380322084470B51D68174BC5F3A1 +:101D70000B042D0C1E88A6420BD15C680A46013C60 +:101D8000824213460FD214F9016F4EB102F8016B73 +:101D9000F6E7013A03F10803ECD181420B4602D287 +:101DA0002C2203F8012B0424094A1688AE4204D1E0 +:101DB000984284BF967803F8016B013C02F104025B +:101DC000F3D1581A70BD00BF0010005C1C22002027 +:101DD0000C910008022803D1024B4FF480229A6133 +:101DE000704700BF00040258022802D1014B0422B0 +:101DF0009A61704700040258022804D1024A5369CC +:101E000083F00403536170470004025870B5044620 +:101E10004FF47A764CB1412C254628BF412506FB6C +:101E200005F0641B04F034FCF4E770BD002310B52A +:101E3000934203D0CC5CC4540133F9E710BD0000D9 +:101E4000013810B510F9013F3BB191F900409C42B7 +:101E500003D11AB10131013AF4E71AB191F9002026 +:101E6000981A10BD1046FCE703460246D01A12F934 +:101E7000011B0029FAD1704702440346934202D065 +:101E800003F8011BFAE770472DE9F8431F4D14468C +:101E90000746884695F8242052BBDFF870909CB323 +:101EA00095F824302BB92022FF2148462F62FFF7F6 +:101EB000E3FF95F824004146C0F1080205EB8000DD +:101EC000A24228BF2246D6B29200FFF7AFFF95F894 +:101ED0002430A41B17441E449044E4B2F6B2082EEA +:101EE00085F82460DBD1FFF725FF0028D7D108E073 +:101EF0002B6A03EB82038342CFD0FFF71BFF00283E +:101F0000CBD10020BDE8F8830120FBE79C33002003 +:101F1000024B1A78024B1A70704700BFDC33002066 +:101F20001022002038B51A4C1A4D204603F0A8FEA6 +:101F30002946204603F0D0FE2D684FF47A70D5F87C +:101F40009020D2F8043843F00203C2F80438FFF7B7 +:101F50005DFF1149284603F0CDFFD5F890200F4DC5 +:101F6000D2F80438286823F002030D49A042C2F8D1 +:101F700004384FF4E1330B6001D003F0DFFD6868F3 +:101F8000A04204D00649BDE8384003F0D7BD38BDB3 +:101F900038400020089900081099000814220020F9 +:101FA000C43300200C4B70B50C4D04461E780C4B0E +:101FB00055F826209A420DD00A4B002118221846C7 +:101FC000FFF75AFF0460014655F82600BDE870404F +:101FD00003F0B4BD70BD00BFDC330020142200202C +:101FE00038400020C433002008B571B6054B19886D +:101FF000054B1888FFF778FD02210E20FFF774FDCE +:1020000061B608BDDE3300209833002030B50A44A5 +:10201000084D91420DD011F8013B5840082340F380 +:102020000004013B2C4013F0FF0384EA5000F6D17A +:10203000EFE730BD2083B8ED0121884238BF084664 +:1020400005F050B908B105F051B9704710B5084C0A +:1020500001220849002001F0C1FE23783BB1064867 +:1020600003F0E4FC044803F017FD0023237010BDC7 +:10207000E03300201C910008C03600202DE9F0411B +:10208000234C244E236DBBB901224FF4004104F1CF +:10209000500005F06FFA236D1B686BB901224FF4F5 +:1020A000805104F1500005F065FA236D1B681BB9DF +:1020B00000203070BDE8F08133780BB1FFF7C6FF28 +:1020C0000324154F4FF00008114D13492846C7F857 +:1020D000048003F0D3FC284603F00CFB38B1013C2C +:1020E000284603F0D9FC14F0FF04E1D0EDE70B49DA +:1020F00001220B4801F072FE014618B1284603F098 +:1021000095FCECE7074800F00FF80120D1E700BF8D +:10211000C0360020E0330020342200201C9100084B +:10212000E4330020209100080FB4002004B0704771 +:102130000068704703460068596870470B0A0170D1 +:1021400043700B0C090E8370C1707047110A027046 +:1021500003714170110C120E8170C2701A0A427123 +:102160001A0C1B0E8271C37170470000C36A0239DA +:10217000023B8B4283BF4389006C01FB03000020BC +:1021800070470000C2F307238A76CB760378032BCF +:1021900001BF120C0A75120A4A75704700F10B0153 +:1021A0000022D30143EA520310F8012B52FA83F3C1 +:1021B0008842DAB2F5D110467047000010B5417878 +:1021C00004460020013102464901022A16BFA35CE1 +:1021D000032203EBC03302F101021EBF9BB203EBEB +:1021E000500398B29142F0D810BD000002684AB185 +:1021F000134613F8011B1F290DD93A29F9D1911C57 +:102200008B4202D04FF0FF3070471278302AF9D15C +:10221000036000207047014B18787047303600206B +:1022200038B50D46044618B9092000232B6038BD87 +:102230000368002BF8D01A78002AF5D08188DA8854 +:102240009142F1D1587804F0BFFB10F00100EBD1BE +:102250002368EBE738B50D4640F25231144602F0E0 +:10226000B9F9FF2807D9012C0BD9030A0224687099 +:102270002B70204638BD30B1002CFAD001242870D4 +:10228000F7E70024F5E70446F3E700002DE9F843FB +:102290000026D0F8008005460C468E76836B002B16 +:1022A0004AD098F80030042B4BD1334635464027AE +:1022B00020E0B7F5187F80F0C480F90606F101062A +:1022C00008BF0237D05B02372BB900F5205292B21B +:1022D000B2F5006F0DD305F11A01C5F1FF0240EA16 +:1022E00003402144FFF7B6FF002800F0AA80054410 +:1022F00000200346D8F8102092F82310B142D8D815 +:10230000002B40F09E80002D00F09B800023254490 +:10231000AB766373D8F81020137903F03701DB062E +:1023200021730BD402F13800FFF704FFC4E9000168 +:1023300093896381D3892381BDE8F8830020014616 +:10234000F4E7C36C01335ED1EA6B00232E26551EE1 +:10235000184615F8011F013020290CD0052908BFA7 +:10236000E521092804D10B2B9EBFE71801337E73AA +:10237000E718013379730B28EBD1E118002048737B +:10238000A17E00294BD1002B40D06FF00C0604F148 +:102390000D000825361B331810F8011B002938D012 +:1023A0002E298BB24AD0A3F14101192903D8117B00 +:1023B0000D4200D020330373EDE7B9F1000F05D1D2 +:1023C00000F520539BB2B3F5006F0BD307F11A0150 +:1023D000C7F1FF0240EA09402144FFF73BFF48B143 +:1023E0000744002002368146D8F80C30985B00285C +:1023F000E3D13846B9F1000F4FF0000218BF0020BA +:1024000023189A76A0E7B1463746EDE73F23A37637 +:102410000123234400219976137B03B96373D37A94 +:1024200002F11C0003F03F0323730023FFF780FE3B +:1024300020606360D38A6381138B7CE710250B4691 +:10244000B9E73F230125A37660E7000038B50546CC +:10245000002435F8020B08B9204638BD02F0EEF82A +:102460006308C2B203EBC43312FA83F39AB2C0F327 +:10247000072303EB520303EBC2339CB2E9E70000EE +:1024800037B5C37804461BB90025284603B030BDD4 +:1024900000F14C01826C01234078019104F0BAFAFA +:1024A000054680B9A36BE070A06C226BC31A9342FF +:1024B000EAD2A3780199022BE6D102440123607885 +:1024C00004F0A8FAE1E70125DFE7000038B5836CE6 +:1024D00005460C468B4210D0FFF7D2FF60B922466A +:1024E000012305F14C01687804F070FA00281CBF44 +:1024F0004FF0FF340120AC6438BD0020FCE7000041 +:1025000038B500230446C3704FF0FF338364FFF7F0 +:10251000DDFF00284BD1B4F84A524AF655239D42BC +:1025200007D10B22254904F14C0006F029FD0028B3 +:102530003FD094F84C30EB2B03D01833DBB2012B97 +:102540002ED84AF655239D4206D108221C4904F193 +:102550009E0006F015FD48B3B4F85730B3F5007F80 +:102560001ED194F85930DBB15A1E1A4218D1B4F872 +:102570005A30ABB194F85C30013B012B10D8B4F861 +:102580005D306BB1B4F85F307F2B06D804F16C007E +:10259000FFF7CEFDB0F5803F02D3B4F8623053B9F7 +:1025A0004AF6552085420CBF0220032038BD042086 +:1025B000FCE70120FAE70020F8E700BF4C91000893 +:1025C0005891000802392DE9F04701F007044FF057 +:1025D000010A466C05460AFA04F41746984606EBCB +:1025E0001136C1F3C809E4B2314628460136FFF777 +:1025F0006DFF18B10120BDE8F087994605EB09028F +:1026000092F84C30234214BF01210021414513D0E0 +:102610006340013F82F84C3085F803A0EBD06400A2 +:1026200014F0FF04EAD109F1010301244FF000097D +:10263000B3F5007FE1D1D7E70220DCE701290246AC +:10264000F8B50C4640F28C800668F36A8B4240F283 +:1026500087803378013B032B00F28280DFE803F0B0 +:102660000229384B04EB5405B16B304601EB552180 +:10267000FFF72CFF10B14FF0FF30F8BD6F1CC5F312 +:102680000805B16B3046354401EB572195F84C50A5 +:10269000FFF71CFF0028EED1C7F30807E3073E440D +:1026A00096F84C0045EA00204CBF0009C0F30B002F +:1026B000E3E7B16B304601EB1421FFF707FF002879 +:1026C000D9D1640004F4FF742644B6F84C00D4E772 +:1026D000B16B304601EBD411FFF7F8FE0028CAD1E8 +:1026E000A40006F14C0004F4FE742044FFF720FD22 +:1026F00020F07040C1E7D0E90430D57953EA0001F9 +:1027000001D0916801B95DBB9168022DA4EB010174 +:102710000DD1013B728940F1FF305B0A43EAC0539F +:10272000B3FBF2F399421BD81CD0601CA5E7032D24 +:1027300002D193698B42F8D8D3699BB9B16B30460B +:1027400001EBD411FFF7C2FE002894D1A0004C3653 +:1027500000F4FE703044FFF7EBFC20F000408CE703 +:1027600001208AE76FF0004087E70000F8B50668AF +:1027700004460D463378042B0CBF4FF080524FF4C3 +:1027800000128A4201D80220F8BDCA06FBD1826835 +:102790000163D2B9022B13D83389B3EB551FF2D999 +:1027A000F36BA363A36B6263002BECD003EB5523A5 +:1027B0004C36C5F308050020A3633544E563E3E721 +:1027C000F36BC271002BE7D01A4677897F02BD42B6 +:1027D000114604D23046FFF7C9FCA063E2E7204669 +:1027E000FFF72CFF431C024606D00128CBD9F36A21 +:1027F0008342C8D9ED1BEAE70120C5E701292DE98D +:10280000F04706460C46174608D9C36A8B4205D9DD +:102810000378022B62D003D8012B22D0022552E08C +:10282000033B012BFAD8816B01EBD411FFF74EFE6D +:102830000546002847D1A40006F14C0304F4FE74B9 +:102840001C443378042B07D0204627F07047FFF74D +:102850006FFC00F07040074339462046FFF76EFCDE +:102860002FE001EB5108816B01EB5821FFF72EFEA1 +:10287000054640BB14F0010406F14C0908F1010AB9 +:10288000C8F3080808BFFBB230461FBF19F808306C +:1028900003F00F023B0103F0F00318BF134309F8E4 +:1028A00008300123B16BF37001EB5A21FFF70EFEE4 +:1028B000054640B9CAF3080A44B1C7F3071709F837 +:1028C0000A700123F3702846BDE8F08719F80A3032 +:1028D000C7F3032723F00F031F43F0E7816B01EBDE +:1028E0001421FFF7F3FD05460028ECD1640006F142 +:1028F0004C0304F4FF741F551919C7F307274F70D1 +:10290000DFE70000F8B504460E461746E3690BB949 +:102910001846F8BD012BA6EB0305206814BFAA1CBE +:102920003A46691CFFF76AFF0028F2D1E369013BD0 +:10293000E361EBE701292DE9F84306460C4617460B +:10294000056802D80220BDE8F883EB6A8B42F9D90A +:102950007AB9A14621463046A046FFF76FFE0446ED +:10296000B0B92B78042B02D1002F43D1F77100208E +:10297000E9E72B78042B02D1C379022BE9D04FF081 +:10298000FF3239462846FFF739FF0028E1D0DAE761 +:102990000128D7D0421C01D10120D4E72B78042B89 +:1029A00019D1EA6AAB69023A93421CD308F10102D9 +:1029B000A2420CD02B78042B08D10023A2EB0902F1 +:1029C00049462846FFF7FEFD0028BCD1A146EB6A28 +:1029D000A342BFD8C5E7002241462846FFF70EFFB5 +:1029E0000028DED0AFE70133AB612B7943F0010360 +:1029F0002B71DBE7F3798BB9B468BC4202D10223B7 +:102A0000F371B4E721463046FFF718FE012899D943 +:102A1000431CC1D001348442EFD0A8E7032BA6D1D8 +:102A2000B368BB42A3D8B2691344BB429FD3E6E765 +:102A300070B5C3790446032B06D181688369CD182C +:102A4000A94203D10023E371002070BD4E1C206811 +:102A50003246FFF7D3FE0028F7D13146F0E70000F9 +:102A60002DE9F74305460191FFF70AFD04460028CA +:102A700049D105F14C09019928464FF40072FFF73E +:102A800075FB2146A86407464846FFF7F5F96C89AF +:102A90006402B4F5004F28BF4FF40044B4F5007F42 +:102AA0002FD9204604F026FC804630B12246002172 +:102AB000640A0026FFF7E0F909E06408EEE7234620 +:102AC000BA194146687803F0A5FF18B926446B8906 +:102AD0009E42F4D3404604F01DFC6889801B18BF59 +:102AE000012003B0BDE8F08301366B899E42F4D229 +:102AF0000123BA194946687803F08CFF0028F3D007 +:102B0000EBE70026F1E70120EBE70000F8B504460B +:102B1000FFF7B6FC0546002842D12378032B37D1B6 +:102B20002779012F34D104F14C0601464FF400728D +:102B30003046FFF7A1F955234122722184F84A3229 +:102B4000AA2304F50D7084F84F2084F84B325223E9 +:102B500084F8301284F84C3084F84D30612384F8C6 +:102B6000311284F84E3084F83332A16984F832226D +:102B7000FFF7E4FA616904F50E70FFF7DFFA626BA4 +:102B80003B46314601326078A26403F043FF257171 +:102B900000226078114603F061FF003818BF012061 +:102BA000F8BD000000232DE9F0430B6085B00F460F +:102BB0001546FFF71BFB061EC0F2B281804B53F88F +:102BC0002640002C00F0AE813C6005F0FE05237825 +:102BD0006BB1607803F0F8FEC70708D41DB110F0A0 +:102BE000040500D00A25284605B0BDE8F08300237F +:102BF000F0B22370607003F0D3FEC10700F194813E +:102C00000DB14207EED400212046FFF779FC0228DF +:102C100040F099806E4604F2122304F252213246AB +:102C200018461033FFF784FA42F8040B8B42F7D1B1 +:102C3000002556F8041B00297DD02046FFF760FCD4 +:102C4000012879D80128A26C40F0C08004F1570314 +:102C500004F18C0113F8015B002D7BD18B42F9D17B +:102C6000B4F8B430B3F5807F74D194F8B830092B40 +:102C700070D104F19400FFF75DFA4FF0FF3317189D +:102C800041F10001BB4275EB010363D304F1A000E5 +:102C9000FFF74EFA94F8BA302063012BA37059D194 +:102CA00094F8B99003FA09F91FFA89F36381002BAC +:102CB00050D0444B04F1A800FFF73AFA0646984278 +:102CC00048D8831C626304F1A400E362FFF730FA82 +:102CD00000EB020804F19C00C4F84080FFF728FADA +:102CE00010441FFA89F2A06306FB02F313EB0803FA +:102CF00045EB05029F4271EB02032BD32E4604F1F4 +:102D0000AC00FFF715FAE06365B96389B34221D9D6 +:102D1000E16B2046FFF72AFA81192046FFF7D6FB20 +:102D200098B90136631993F84C30812B14D02035B3 +:102D3000C5F30805E8E703200135042D7FF479AFDA +:102D4000042807D101E0042801D101254BE701281F +:102D50007FF678AF0D2546E705F1140004F14C0627 +:102D60003044FFF7E5F901280546F3D9E36A8342C9 +:102D7000F0D96189821E236C02FB01336364A16B6D +:102D8000204601EBD511FFF7A1FB0028DDD105F0AE +:102D90007F0006EB8000FFF7CBF9431C03D0013521 +:102DA000A842ECD0D6E70425C4E90500064A257000 +:102DB00000251388E56101339BB21380E38012E79D +:102DC00034360020FDFFFF7F38360020B4F857303E +:102DD000B3F5007FBED1B4F8626026B904F170008B +:102DE000FFF7A6F9064694F85C302663591EA370D7 +:102DF0000129AFD894F859506581002DAAD0691ED9 +:102E00002942A7D1B4F85D8018F00F0FA4F808800C +:102E1000A0D1B4F85F0018B904F16C00FFF788F98D +:102E2000B4F85A10002995D006FB03FE01EB181CDC +:102E3000F44460458ED3A0EB0C00A842B0FBF5F340 +:102E400088D33E48834285D84FF6F57083426DD9CA +:102E500003259F1C114402EB0C03032DE762626300 +:102E6000A16323644CD1B4F8763053EA08037FF4AD +:102E700071AFBB0004F17800FFF75AF9E06303F289 +:102E8000FF13B6EB532FFFF465AF4FF0FF33032D65 +:102E9000C4E905334FF08003237187D1B4F87C3047 +:102EA000012B83D1511C2046FFF710FB00287FF433 +:102EB0007DAFB4F84A224AF6552320719A427FF436 +:102EC00075AF1F4B04F14C00FFF732F998427FF4C5 +:102ED0006DAF03F1FF5304F50C70FFF729F903F50B +:102EE0000053203398427FF461AF04F50D70FFF773 +:102EF0001FF9A06104F50E70FFF71AF9606155E73C +:102F0000B8F1000F3FF426AF7144022D4FEA47039A +:102F1000E1631EBFD91907F0010303EB5103AEE7CC +:102F20000B2560E60C255EE603255CE640F6F575AC +:102F3000AB428CBF022501258BE700BFF5FFFF0FD9 +:102F4000525261412DE9F84F07460568884649B954 +:102F50006E69C6B1EB6AB34298BF0126AB69A3B9EB +:102F6000002405E0FFF76AFB0128044603D801248A +:102F70002046BDE8F88F421C00F0D280EB6A834205 +:102F8000F6D84646EAE70126E8E72A78EB6A042AFB +:102F900040F08380A6F1020A023B4FF0010B9A45F4 +:102FA00028BF4FF0000AD146696C284601EB193161 +:102FB000FFF78CFA00283BD109F00703EA6AC9F34E +:102FC000C8010BFA03F3901EDBB26A184C4609F1F4 +:102FD000010992F84C20814502EA030233BF5B00ED +:102FE00000234FF40071DBB228BF9946B2B9023416 +:102FF000631E0333BCD80123214628461A46FFF737 +:10300000E1FA0228B3D0012800F08A80B8F1000F5D +:1030100013D10223FB710028A9D130E0CA450AD0A0 +:10302000002BD2D10131B1F5007FBDD20123CCE715 +:103030004FF0FF34DCE70024DAE7FB79022B07D1FD +:10304000731CA342E7D0BB68F31ABB610323FB7177 +:1030500008F10102FB69A24205D113B10133FB6102 +:10306000D9E70223FBE70BB90123FB612246414666 +:103070003846FFF747FC00284FD10123FB61EA6A7D +:10308000AB69023A6C6193429CBF03F1FF33AB61C1 +:103090002B7943F001032B716AE7464514D1741C68 +:1030A0003846A34298BF02242146FFF7C7FA0128F9 +:1030B0003FF45DAF431C33D0E0B16B69012B03D902 +:1030C000EA6A934238BF1E4634460134EB6AA34293 +:1030D00003D8012E7FF644AF022421463846FFF77D +:1030E000ADFA48B101283FF442AF013018D0B442E4 +:1030F000EBD135E7002CE7D04FF0FF3221462846D0 +:10310000FFF77CFB48B9B8F1000FB8D02246414622 +:103110002846FFF773FB0028B1D001287FF427AFC2 +:103120004FF0FF3424E700002DE9F8430668044619 +:10313000076B894633782037042B0CBF4FF0805340 +:103140004FF40013BB429CBF00238363836B73B1B6 +:10315000C7F30808B8F1000F3CD10133416B83631A +:1031600039B93389B3EB571F34D80023A363042044 +:103170000AE07389013B13EA57232BD1FFF75EFA6C +:103180000128054602D80220BDE8F883421C01D17F +:103190000120F9E7F36A834216D8B9F1000FE4D0B1 +:1031A000616B2046FFF7CEFE0546C8B10128EAD084 +:1031B000431CEDD001463046FFF752FC0028E7D112 +:1031C000E37943F00403E371294630466563FEF773 +:1031D000CDFFA0634C36002027634644E663D3E767 +:1031E0000720D1E7F8B50E46002104460768FFF72F +:1031F000BDFA98B90546A16B3846FFF767F968B97B +:103200003A78E36B042A1B780CD11B060ED50546D1 +:1032100001212046FFF788FF0028ECD0042808BFD2 +:10322000072006E0E52B01D0002BF0D10135B54297 +:10323000EED1F8BDC16C4B1C2DE9F0410446056888 +:10324000066B1FD1E5274FF00108A16B2846FFF759 +:103250003DF998B92A78E36B042A09BF1A781F70E0 +:1032600002F07F021A7085F80380236BB3420DD2FF +:1032700000212046FFF758FF0028E6D0042808BFA9 +:10328000022003E0FFF772FA0028DBD0BDE8F081EE +:103290002DE9F04105460068A96B0669FFF716F9AC +:1032A000044620B9EB6B1A78852A03D00224204605 +:1032B000BDE8F081324603F1200153F8040B8B4244 +:1032C00042F8040BF9D1777801377F01A7F1600349 +:1032D000B3F5007FEAD800212846FFF725FF042830 +:1032E0000446E3D00028E2D1A96B2868FFF7EEF886 +:1032F00004460028DBD1EB6B1A78C02AD6D106F140 +:10330000200203F1200153F8040B8B4242F8040B16 +:10331000F9D196F823300F222C33B3FBF2F3B7EB3D +:10332000431FC3D34FF0400800212846FFF7FCFE9F +:1033300004280446BAD00028B9D1A96B2868FFF741 +:10334000C5F804460028B2D1EB6B1A78C12AADD17A +:10335000B8F5187F09D206EB080203F1200153F8F3 +:10336000040B8B4242F8040BF9D108F120084745C1 +:10337000DAD8B8F5187F9AD83046FEF71FFF738861 +:10338000834294D092E700000B68002210B50360DE +:1033900004460B6A83604B6AC261C37123F0FF036A +:1033A000896AC0E90432C164FFF7E0F920B9204618 +:1033B000BDE81040FFF76CBF10BD0000F8B5036812 +:1033C000054601271C692046FEF7F8FEA070000A9A +:1033D0006678E0702846E96CFFF7C8F920B102284A +:1033E00028BF0220C0B2F8BDA96B2868FFF76EF8AD +:1033F0000028F4D1EB6B04F1200254F8041B944232 +:1034000043F8041BF9D12B68DF70002EE7D00021B0 +:103410002846013EFFF788FEE0E700002DE9F8436B +:103420004FF0FF0806460768042445464FF6FF792B +:10343000B16B11B9002C73D063E03846FFF746F842 +:10344000044600285DD1F06B0378002B6ED03A78EB +:10345000042A11D1852B4DD1336B3046F364FFF72D +:1034600017FF044600284CD13B691B7903F03F034A +:10347000B3712046BDE8F883C27AE52B02F03F0223 +:10348000B27143D02E2B41D022F0200108293DD02B +:103490000F2A40D1590637D503F0BF05336B90F89A +:1034A0000D80F364437B434530D1428B72BB03787C +:1034B0000D21FC6823F04003DFF874E0013B4B432F +:1034C00001211EF801CB30F80CC009B3FF2B1DD829 +:1034D00024F813C06146013301320D2AF1D102787C +:1034E000520605D521B1FF2B10D8002224F8132055 +:1034F000013DEDB200213046FFF716FE04460028DC +:1035000096D00023B363B4E7AB42CBD0FF25F1E7FD +:10351000CC45E1D0FAE72DB9FEF740FE404501D198 +:103520000024A6E74FF0FF33F364A2E70424E8E7A2 +:10353000F49100082DE9F04F002187B00446D0F83F +:103540000090FFF713F9804670B999F80030042B0A +:1035500033D1D9F80C00FEF779FF07462046FFF774 +:103560005DFF054620B18046404607B0BDE8F08FBC +:10357000D9F810309A8CBA42F0D193F823B0402693 +:103580005D4506D1D9F80C3033F81530002BE5D164 +:10359000EAE7F106D9F8103008BF0236985B01F06F +:1035A0004DF8D9F80C30824633F8150001F046F892 +:1035B0008245D3D102360135E2E74FF0FF0A4FF0E2 +:1035C000FF3B5546C4F84CB0A16B4846FEF77EFF62 +:1035D00000285CD1E66B3778002F77D0F27AE52FA0 +:1035E00002F03F03A37103D0120704D50F2B04D0C0 +:1035F000C4F84CB04FE00F2B54D194F84B30580620 +:103600003FD4790645D5236B07F0BF0796F80DA088 +:10361000E364737B53453ED1738B002B3BD13578EC +:103620000121D9F80C3005F03F0501930D23013D30 +:103630005D43284B13F8012BB25A71B3FF2D05934C +:1036400029D81046049200F0F9FF6B1C03900293F6 +:10365000019B33F8150000F0F1FF039981421AD164 +:10366000049A029D1146059B1B4A9342E2D133788E +:103670005A0604D519B1019B33F815305BB97D1E8C +:10368000EDB200212046FFF74FFD00289CD0804678 +:103690006AE7BD42BDD0FF25F3E74FF6FF708242D7 +:1036A000E2D0F8E72DB93046FEF778FD50453FF4FB +:1036B0005BAF94F84B30DB079AD40B2204F1400146 +:1036C000304605F05DFC002892D14DE74FF004082C +:1036D0004AE700BFF4910008019200082DE9F04F7D +:1036E00090F84BB099B004461BF0A00540F06881FB +:1036F0000668F26832F81530002B4AD13378042B73 +:1037000040F087800F230E352046B5FBF3F5A91C4A +:10371000FFF768FD8146002877D1236B0135A3EBC5 +:103720004515E3795A07E56435D523F004032046AF +:10373000E371FFF77DF950BB4FF0FF32616B20461C +:10374000FFF7E0F818BBA3682BB3214604A8FFF7E6 +:103750001BFEE0B970894FF40071D4E90423E0FB4B +:1037600001233069C4E904233830FEF7EFFC3069E7 +:10377000D4E904232830FEF7E9FCE379326904A890 +:1037800043F0010382F82130FFF718FE18B181469B +:103790003BE00135AEE7D6E9035440220021204644 +:1037A000FEF76AFB8523012140222370C0234FF0DE +:1037B000C10C04EB010884F8203000231E469E460D +:1037C000571C04F802C0F0B2023204F807E021B13D +:1037D00035F8131009B10133DBB20F0AA15408F810 +:1037E00002700232D706F2D135F813700136002F7D +:1037F000E6D184F82330831C28466370FEF726FE4A +:1038000084F82400000A84F82500484619B0BDE871 +:10381000F08F04F140070DF1100A1BF0010F97E83B +:1038200007008AE8070000F0D38040234FF001082A +:1038300084F84B30BC46F368B8F1050F9AE80700EE +:10384000ACE803002CF8022B4FEA12428CF800205F +:1038500059D9981E424630F8021F002942D10DF175 +:103860000F0C072102F00F0E914612090EF13000E5 +:10387000392888BF0EF1370001390CF8010902D050 +:10388000B9F10F0FEED818AB7E205A1802F8580C79 +:1038900038460022914206D010F801CB02F1010E09 +:1038A000BCF1200F31D104F13F0C072902F10102D4 +:1038B00097BF18AB20205818013198BF10F8580C4A +:1038C000072A0CF80200F0D92046FFF733FE8146A4 +:1038D000002878D108F10108B8F1640FAAD14FF09F +:1038E000070992E74FF0100C01F0010E49080EEBAA +:1038F0004202D30344BF82F4883282F02102BCF139 +:10390000010CF1D1A7E74246A9E77246C2E7216B55 +:103910002046A1EB4511FEF729FF814600287FF4E0 +:1039200074AF4FF6FF783846FEF738FC0190A16B74 +:103930003046FEF7CBFD814600287FF466AFE36B8F +:10394000E9B2019A4FF00D0CD6F80CE05A734FF023 +:103950000F02DFF8E0A0DA724A1E18730CFB02F2C5 +:1039600084469876D87640451AF8019B0CF1010CF4 +:1039700018BF3EF8120003EB090B18BF013203F821 +:1039800009004FEA1029002808BF4046BCF10D0F7E +:103990008BF80190E7D1404502D03EF812200AB9D9 +:1039A00041F040011970012300212046F370FFF718 +:1039B000BBFB814600287FF428AF013DB7D11BE057 +:1039C0004FF0060921E704287FF41FAF84F84BB0BD +:1039D0001BF0020F20461BBF0C350D210125B5FB46 +:1039E000F1F518BF01352946FFF7FCFB8146002899 +:1039F0007FF40BAF013D8AD1A16B3046FEF766FD27 +:103A0000814600287FF401AF01462022E06BFEF7DB +:103A100033FAE36B03CF18605960BA7839889A7229 +:103A2000198194F84B30E26B03F0180313730123F0 +:103A3000F370EAE6F491000810B504460A46343003 +:103A4000FEF776FB886004F13800FEF773FBC2E9ED +:103A5000040194F8213003F00203D3710023D361F1 +:103A600010BD000003284B8B04BF8A8A43EA02433F +:103A7000184670472DE9F04F0B7899B004468946F7 +:103A80002F2BD0F800B001D05C2B09D14A46137817 +:103A9000914601322F2BFAD05C2BF8D0002301E0A5 +:103AA000DBF81C30A3600023E3619BF80030042B9B +:103AB0001ED1A368E3B1DBF82030214604A82362BD +:103AC000DBF824306362DBF82830A362FFF75CFC8C +:103AD0000346002854D1DBF8102002F13800FEF72D +:103AE00027FBC4E9040392F8213003F00203E371D9 +:103AF00099F800301F2B00F23581802300212046E9 +:103B000084F84B3019B0BDE8F04FFEF72FBE4946A0 +:103B10000B78894601312F2BFAD05C2BF8D01F2B64 +:103B20008CBF00250425012F2FD113882E2B31D1D6 +:103B3000002322F8173004F140029F428CBF2E214F +:103B4000202101330B2B02F8011BF6D145F0200593 +:103B5000204684F84B50FFF7EDFC94F84B300028DA +:103B600000F0E78004280BD1990603F0040240F12D +:103B7000DC80002A00F0F6808023002084F84B309F +:103B800019B0BDE8F08F0425CDE7022F02D153888C +:103B90002E2BCAD0911E87BB002322F81730002F8E +:103BA00000F0118132F81300194601332028F9D0B2 +:103BB00009B92E2801D145F00305901E30F81730C1 +:103BC0002E2B01D0013FF9D14FF020334FF0000AE6 +:103BD0006364D0462364C4F847300823481C32F895 +:103BE00011600090F6B1202E03D02E2E0DD1B842D8 +:103BF00010D045F003050099F0E731F81730202B7D +:103C000001D02E2BC8D1013FC5E79A4505D20099B6 +:103C1000B9423BD10B2B30D101E00B2B27D145F022 +:103C200003050B2394F84020E52A04BF052284F8FD +:103C30004020082B04BF4FEA88085FFA88F808F094 +:103C40000C030C2B03D008F00303032B01D145F028 +:103C50000205A8073FF57CAF18F0010F18BF45F02B +:103C6000100518F0040F18BF45F0080570E700991B +:103C7000B94202D045F00305D4D84FEA88080B2397 +:103C80004FF0080A00975FFA88F8B4E77F2E15D93D +:103C9000304640F25231CDE9022345F00203019350 +:103CA00000F098FC10F0800F0646DDE9022316D0E4 +:103CB00000F07F0646498E5D019D46B3314645487A +:103CC000CDE9012305F086F9DDE90123F8B9A6F174 +:103CD000410189B219291ED848F0020810E0FF28D6 +:103CE000EAD9591E8A4503D345F003059A4682E76F +:103CF00004EB0A01000A0AF1010A019D81F8400063 +:103D000004EB0A010AF1010A81F8406073E745F00B +:103D100003055F26F4E7A6F1610189B219299EBF68 +:103D2000203E48F00108B6B2EAE7002A08BF0520A5 +:103D300026E75A073FF524AFA379DB0645D59BF864 +:103D40000000042835D1A3682146E2792362236963 +:103D5000DBF8100023F0FF0313436362E36CA362FC +:103D6000FFF76AFE23680027DA6819F8010B0028BC +:103D70003FF409AF40F25231009200F04BFC05468F +:103D800008B31F28009A7FF6FEAE2F283FF4BFAE7F +:103D90005C283FF4BCAE7F2805D801460E4805F0EC +:103DA00019F9009A78B9FF2F0DD022F81750013772 +:103DB000DBE7216B0BF14C03C1F308011944FFF75A +:103DC00051FEA060CEE70620DAE60520D8E600BF67 +:103DD000749100086D910008649100081FB5CDE949 +:103DE000001003A814460391FEF700FA002815DB23 +:103DF0000B4A52F820300BB100211970019B0BB116 +:103E00000021197042F820302CB1002201A9684627 +:103E1000FEF7C8FE0446204604B010BD0B24FAE7A6 +:103E2000343600202DE9F04798B0904605460191C0 +:103E3000002800F0528102F03F0603A901A8324693 +:103E4000FEF7B0FE002840F04681039B4FF48C60E3 +:103E5000049303F04FFA0746002800F04081039BCB +:103E600000F500720199D86004A81A61FFF702FEFC +:103E7000044620B99DF95B30002BB8BF062418F02A +:103E80001C0F00F0CD80002C4CD0042C40D104A895 +:103E9000FFF724FC044600283AD146F00806039BAD +:103EA0001A78042A40F08380186929462B60FFF7AE +:103EB000C3FD039B1E22002118690230FDF7DCFFC1 +:103EC000039C00211A2220692630FDF7D5FF2369C3 +:103ED00020221A71246903F047FA014601220834AE +:103EE0002046FEF72BF9039B04A81B6983F82120C9 +:103EF000FFF764FA044658B9A96801B30246284698 +:103F0000FEF718FDAB68039A0446013B5361B4B158 +:103F1000384603F0FFF90CB100232B60204618B09F +:103F2000BDE8F0879DF8163013F0110F40F0848043 +:103F300018F0040F40F0C98018F0080FAFD1039AB1 +:103F400031071399936C48BF46F04006E964AB64AF +:103F50001078042872D1069B9DF817102B62089BDD +:103F6000106923F0FF030B4329466B62179BAB627A +:103F7000FFF762FDDDF80CA00024002205F15008D7 +:103F8000BAF8063021464046C5F800A0AB800023B1 +:103F900085F8306085F831406C64C5E90E234FF434 +:103FA0000072FDF769FFB20653D40024B0E703F0B6 +:103FB000DBF90146009013980E30FEF7BFF8139816 +:103FC00000991630FEF7BAF8039C13992078FFF792 +:103FD00049FD202300228046CB7220461399FEF72C +:103FE000D1F8139B002201211A775A779A77DA7752 +:103FF000039BD970B8F1000FA1D0414604A8D3F8B3 +:104000004890FEF797FC0446002881D14946039862 +:10401000FEF75CFA039B044608F1FF30586176E72F +:10402000002C7FF475AF9DF81630DC064FD418F0E5 +:10403000020F84D0D80782D5072469E7FFF712FD65 +:104040000023A86001F11C00FEF772F86B61286183 +:1040500090E7D5E9046956EA0903A6D0BAF80AA0A0 +:10406000A9684FEA4A2AC5E90E69B24574EB09030B +:104070001BD300242964002C7FF44AAFC6F3080345 +:10408000002B92D0039C2046FEF770F808B3760A06 +:104090000123414646EAC95682196A64607802F0F3 +:1040A00095FC041E18BF012432E72846FEF7C6FA25 +:1040B000B6EB0A06014669F10009012803D9431C41 +:1040C000D3D10124D6E70224D4E7082420E704242E +:1040D0001EE702241CE704461EE709241EE71124FC +:1040E0001CE700002DE9F04F994685B00023884673 +:1040F00003A90446C9F800301646FEF791F80546B4 +:1041000080BB94F831506DBB94F8303013F001034C +:10411000009300F0A68004F1500AD4E90432D4E9F7 +:104120000E011B1A62EB0102B34272F1000238BFAA +:104130001E46BEB1D4E90E10C1F30803002B40F0B7 +:104140008280039B5A894B0A013A43EAC0531A40C2 +:104150001BD151EA000309D1A06801280DD802251E +:1041600084F83150284605B0BDE8F08F216C204618 +:104170000192FEF763FA019AEFE7431C04D1012391 +:10418000009D84F83130EDE72064DDF80CB0216C3F +:104190005846FDF7EBFF0028E1D0B6F5007F02EBB3 +:1041A000000731D3BBF80A1002EB5620730A88428D +:1041B0009BF8010088BF8B1A3A464146019302F0F2 +:1041C00005FC0028DBD194F93020019B002A0BDA92 +:1041D000606CC01B984207D24FF40072514608EB46 +:1041E0004020FDF723FE019B5F02D9F80030F61B4B +:1041F000B8443B44C9F80030D4E90E32DB1942F12F +:104200000002C4E90E3294E7626CBA421AD094F903 +:104210003030002B0DDA012351469BF8010002F0EB +:10422000F9FB0028ABD194F8303003F07F0384F819 +:104230003030039801233A465146407802F0C6FBDD +:1042400000289CD16764A16B4046C1F30801C1F509 +:1042500000775144B74228BF37463A46FDF7E6FD9E +:10426000C3E707257EE7000070B596B00E46002232 +:10427000019002A901A8FEF795FC0446E0B94FF4AD +:104280008C6003F037F80546D8B1029B00F5007248 +:104290000199D86002A81A61FFF7ECFB044640B907 +:1042A0009DF95330002B0ADB1EB1314602A8FDF701 +:1042B000EDFF284603F02EF8204616B070BD062408 +:1042C000F7E71124F8E7000070B5B8B002220190BA +:1042D00003A901A8FEF766FC044608BB039B4FF444 +:1042E0008C60109303F006F80546002866D0039B07 +:1042F00000F500720199D86010A81A61FFF7BAFBA7 +:10430000044650B99DF88B30980655D4190653D4FD +:104310009DF84630DA0706D50724284602F0FAFF52 +:10432000204638B070BD039B04931878042814D13C +:1043300004A91869FFF780FB069E9DF84630DB064E +:1043400010D410A8FEF776FF04460028E5D156BB2E +:104350000398FEF7DBFB0446DFE71F99FFF782FBBC +:104360000646EAE7039BDA69B242D5D024930021DE +:10437000269624A81B78042B01BFDDE90823CDE98C +:1043800028239DF817308DF89730FEF7EFF9044693 +:104390000028C2D124A8FFF741F804460028BBD06A +:1043A0000428BAD1CDE70246314604A8FEF7C2FA86 +:1043B00004460028B1D1CBE70624AEE71124AFE7CD +:1043C000F0B5BDB0CDE900106846FDF70FFF022241 +:1043D00003A901A8FEF7E6FB0446002841D1039B90 +:1043E0004FF48C60149302F085FF0546002800F01E +:1043F000EE80039B00F5007214AE0199D8601A613B +:104400003046FFF737FB044640BB9DF89B3013F066 +:10441000A00F40F0D880039B009F1A78042A68D12F +:104420001B6904AC03F1400C1868083353F8041CF2 +:104430002246634503C21446F6D15022314628A8CD +:10444000FDF7F4FC394628A8FFF714FB04460028C2 +:104450004CD12A9A169B9A4206D00824284602F08C +:1044600059FF20463DB0F0BD349A209B9A42F4D1CA +:1044700028A8FFF733F904460028EFD1039B04AFC7 +:104480001B6993F801E093F823C09C8C3A460833EB +:1044900003CAB24243F8080C43F8041C1746F5D18E +:1044A000039B28A81B6983F801E0039B1A6982F823 +:1044B00023C01A6982F82440240A82F825401A6928 +:1044C0001379D9065CBF43F020031371FEF776FF22 +:1044D00004460028C2D13046FEF7ACFE0446002850 +:1044E000BCD10398FEF712FB0446B7E70428B5D108 +:1044F000BEE7239A04AB02F1200C1068083252F890 +:10450000041C1C46624503C42346F6D150223146A2 +:1045100028A8FDF78BFC394628A8FFF7ABFA04461C +:1045200000284CD12A9A169B9A4296D1349A209B05 +:104530009A4292D128A8FFF7D1F8044600288DD1DD +:1045400037990DF11D030DF12D0001F10D0253F806 +:10455000044B834242F8044BF9D11888012710809C +:104560009B7893709DF81B30039CDA0658BF43F08C +:104570002003CB72E770CB7ADB06ACD5169A2A9B68 +:104580009A42A8D02078FFF76DFA01462046FDF741 +:10459000EDFD0146C8B12046FDF798FF044600280E +:1045A0007FF45CAF039890F86D302E2B93D12A9A4C +:1045B00000F16C01FDF7E6FD039BDF708BE704283B +:1045C0007FF44CAFB6E7062448E7022446E71124FF +:1045D00047E700007F2810B501D880B210BDB0F5C4 +:1045E000803F13D240F2523399420FD10849002242 +:1045F00031F8024B93B2844203D103F18000C0B280 +:10460000ECE70132802AF3D11346F6E70020E5E714 +:10461000B49400087F280DD940F25233994208D152 +:10462000FF2806D800F10040034B803833F8100013 +:1046300070470020704700BFB4940008B0F5803F79 +:10464000F0B522D21F4A83B21F49B0F5805F28BF60 +:104650000A46141D34F8042C2146AAB1934213D300 +:1046600034F8025C2E0AEFB252FA85F5A84222DA3B +:10467000082E09D8DFE806F0050A10121416181AD9 +:104680001C00801A34F810301846F0BD981A00F05B +:1046900001001B1A9BB2F7E7103BFBE7203BF9E751 +:1046A000303BF7E71A3BF5E70833F3E7503BF1E718 +:1046B000A3F5E353EEE70434002ECBD101EB470220 +:1046C000C7E700BF04920008F893000808B5074B3D +:1046D000074A196801F03D01996053680BB1906871 +:1046E0009847BDE8084003F043B800BF000002400F +:1046F0003C36002008B5084B1968890901F03D01D6 +:104700008A019A60054AD3680BB110699847BDE8E1 +:10471000084003F02DB800BF000002403C360020E6 +:1047200008B5084B1968090C01F03D010A049A60AC +:10473000054A53690BB190699847BDE8084003F0FA +:1047400017B800BF000002403C36002008B5084BF7 +:104750001968890D01F03D018A059A60054AD369FF +:104760000BB1106A9847BDE8084003F001B800BFDC +:10477000000002403C36002008B5074B074A596844 +:1047800001F03D01D960536A0BB1906A9847BDE8CA +:10479000084002F0EDBF00BF000002403C360020A0 +:1047A00008B5084B5968890901F03D018A01DA60B2 +:1047B000054AD36A0BB1106B9847BDE8084002F078 +:1047C000D7BF00BF000002403C36002008B5084BB0 +:1047D0005968090C01F03D010A04DA60054A536B7F +:1047E0000BB1906B9847BDE8084002F0C1BF00BF15 +:1047F000000002403C36002008B5084B5968890D7E +:1048000001F03D018A05DA60054AD36B0BB1106CEB +:104810009847BDE8084002F0ABBF00BF000002406F +:104820003C36002008B5074B074A196801F03D01E6 +:104830009960536C0BB1906C9847BDE8084002F04A +:1048400097BF00BF000402403C36002008B5084B6B +:104850001968890901F03D018A019A60054AD36C03 +:104860000BB1106D9847BDE8084002F081BF00BF52 +:10487000000402403C36002008B5084B1968090CBA +:1048800001F03D010A049A60054A536D0BB1906D29 +:104890009847BDE8084002F06BBF00BF000402402B +:1048A0003C36002008B5084B1968890D01F03D0120 +:1048B0008A059A60054AD36D0BB1106E9847BDE822 +:1048C000084002F055BF00BF000402403C36002003 +:1048D00008B5074B074A596801F03D01D960536E8E +:1048E0000BB1906E9847BDE8084002F041BF00BF91 +:1048F000000402403C36002008B5084B596889097D +:1049000001F03D018A01DA60054AD36E0BB1106FE8 +:104910009847BDE8084002F02BBF00BF00040240EA +:104920003C36002008B5084B5968090C01F03D01E0 +:104930000A04DA60054A536F0BB1906F9847BDE8DF +:10494000084002F015BF00BF000402403C360020C2 +:1049500008B5084B5968890D01F03D018A05DA60F8 +:10496000054AD36F13B1D2F880009847BDE80840DC +:1049700002F0FEBE000402403C36002000230C4939 +:1049800010B51A460B4C0B6054F82300026001EB83 +:10499000430004334260402BF6D1074A4FF0FF3307 +:1049A0009360D360C2F80834C2F80C3410BD00BF65 +:1049B0003C360020B4950008000002400F28F8B5EE +:1049C00010D9102810D0112811D0122808D10F2486 +:1049D0000720DFF8C8E00126DEF80050A04208D921 +:1049E000002653E00446F4E70F240020F1E70724F3 +:1049F000FBE706FA00F73D424AD1264C4FEA001C7D +:104A00003D4304EB00160EEBC000CEF80050C0E9A9 +:104A10000123FBB273B12048D0F8D83043F0010332 +:104A2000C0F8D830D0F8003143F00103C0F80031AD +:104A3000D0F8003117F47F4F0ED01748D0F8D83097 +:104A400043F00203C0F8D830D0F8003143F002033D +:104A5000C0F80031D0F8003154F80C00036823F09E +:104A60001F030360056815F00105FBD104EB0C037F +:104A70003D2493F80CC05F6804FA0CF43C602124D8 +:104A80000560446112B1987B00F06AFC3046F8BDC5 +:104A90000130A3E7B4950008004402583C360020DA +:104AA00010B5302484F31188FFF788FF002383F3C7 +:104AB000118810BD10B50446807B00F067FC01230F +:104AC0001549627B03FA02F20B6823EA0203DAB2A9 +:104AD0000B6072B9114AD2F8D81021F00101C2F866 +:104AE000D810D2F8001121F00101C2F80011D2F85B +:104AF000002113F47F4F0ED1084BD3F8D82022F0B9 +:104B00000202C3F8D820D3F8002122F00202C3F831 +:104B10000021D3F8003110BD3C360020004402587B +:104B200008B5302383F31188FFF7C4FF002383F314 +:104B3000118808BD836CC26A8B42506810B506D9D3 +:104B40005A1E4C0002EB4103B3FBF4F3184410BDB2 +:104B500001F001038A0748BF43F002034A0748BF38 +:104B600043F008030A0748BF43F00403CA0648BFDE +:104B700043F010038A06426B48BF43F020031343FF +:104B80004363704710B5074C204600F06BFF064B9F +:104B90000022C4E91023054BA364054BE363054BD6 +:104BA000E36410BDC03600200070005200B4C4049D +:104BB0001437002014390020C36A0BB9104BC362AC +:104BC0000379012B11D10F4B98420ED10E4BD3F824 +:104BD000D42042F48032C3F8D420D3F8FC2042F42D +:104BE0008032C3F8FC20D3F8FC30436C00221A65F5 +:104BF000DA621A605A605A624FF0FF329A63704765 +:104C0000B4960008C0360020004402580379012BF6 +:104C100024D0436C00221A65DA621A605A605A6224 +:104C20004FF0FF329A630E4B984217D10D4BDA6F5B +:104C300042F48032DA67DA6F22F48032DA67DA6FB0 +:104C4000D3F8D42022F48032C3F8D420D3F8FC2047 +:104C500022F48032C3F8FC20D3F8FC30704700BF48 +:104C6000C03600200044025810B5446C0649FFF7D6 +:104C700061FF6060236842F2107043F00303236019 +:104C8000BDE8104001F004BD801A06000129F8B506 +:104C9000466C0B4F09D175680A493D40FFF74AFF42 +:104CA000054345F480557560F8BD746806493C407D +:104CB000FFF740FF044344F480547460F4E700BFFE +:104CC00000ECFFFF80F0FA0240787D01436C002287 +:104CD0005A601A6070470000426C0129536823F43F +:104CE000404304D0022905D001B95360704743F412 +:104CF0008043FAE743F40043F7E70000436C41F4D4 +:104D000080519A60D9605A6B1206FCD580229A6352 +:104D10007047000010B541F48851446CA260E16016 +:104D2000616B11F04502FBD0A26311F0040203D0C5 +:104D3000FFF70EFF012010BD616910461960FAE708 +:104D400010B541F48851446CA260E160616B11F0D0 +:104D50004502FBD0A26311F0050203D0FFF7F8FE75 +:104D6000012010BD616910461960FAE773B513465A +:104D700004460E46302282F31188426CD26B32B167 +:104D80004FF0FF314030019301F08EFC019B606CCD +:104D900000220265C263C262456B15F4807504D1BE +:104DA00085F31188012002B070BD4FF0FF3181639F +:104DB00082F31188012E06D90C21204602B0BDE8ED +:104DC0007040FFF7BDBF1046EDE7000073B5446CBF +:104DD0000E4600250192616BA1632565E562FFF730 +:104DE000B7FE012E07D9019B2A460C2102B0BDE86F +:104DF0007040FFF7A5BF02B070BD000010B541F4D0 +:104E00009851446CA260E160616B11F04502FBD0E7 +:104E1000A26311F03F0203D0FFF79AFE012010BDFC +:104E2000216A10461960E1695960A1699960616958 +:104E3000D960F4E72DE9F74304460191006D01A91B +:104E40001746984602F0AEFB064600284AD0626C30 +:104E50002046DDF804905568C5F3090501356B005F +:104E6000A56CB5FBF3F54FF47A73B5FBF3F55D4331 +:104E7000556200F0BDFD50BB636C4FF0FF32012561 +:104E80004146C3F8589020461D659A634FF49572C9 +:104E9000DA6342F207029F62DA62E36C0A9AFFF772 +:104EA0004FFFA0B9E26C104B11680B407BB929464B +:104EB0002046FFF75BFF054648B92E463A46019962 +:104EC000206D02F0A7FB304603B0BDE8F083E26C32 +:104ED00020460121FFF77AFF3A460199206D02F042 +:104EE00099FBF0E70126EEE708E0FFFD2DE9F74327 +:104EF0001F46436C01924FF47A725D6804468846FF +:104F0000C5F3090501356E00856CB5FBF6F5B5FBFB +:104F1000F2F555435D6200F06BFD20B10125284696 +:104F200003B0BDE8F0837E0201A9206D324602F095 +:104F300039FB05460028F1D0636C019AD4F84C90F7 +:104F40009A6501221A654FF0FF329A634FF4957209 +:104F5000DA639E62236BDB064B4658BF4FEA482854 +:104F6000012F42461BD912212046FFF7E9FEC0B9A6 +:104F7000D9F80020104B13409BB9636C42F29302A6 +:104F800039462046DA62E26CFFF7F0FE804640B90F +:104F900032460199206D454602F03CFBBFE71121E6 +:104FA000E2E7E26C39462046FFF710FF32460199EE +:104FB000206D02F02FFBB2E708E0FFFD2DE9F34181 +:104FC0001F46436C01924FF47A725D68044688462E +:104FD000C5F3090501356E00856CB5FBF6F5B5FB2B +:104FE000F2F555435D6200F003FD20B1012528462E +:104FF00002B0BDE8F0817E0201A9206D324602F0C8 +:1050000021FB05460028F1D0636C019A9A650122C4 +:105010001A654FF0FF329A634FF48D72DA639E6225 +:10502000236BE66CDB06334658BF4FEA4828012F56 +:10503000424619D919212046FFF782FEB0B93268DD +:105040000F4B134093B9636C42F2910239462046EC +:10505000DA62E26CFFF78AFE064638B901993546F6 +:10506000206D02F02BFBC2E71821E4E73946204609 +:10507000E26CFFF7ABFE0199206D02F01FFBB6E773 +:1050800008E0FFFD12F0030F2DE9F04107460C4642 +:1050900015461E4617D00E44B44202D10020BDE88A +:1050A000F0810123FA6B21463846FFF71FFF0028E5 +:1050B000F5D128464FF40072F96B05F500750134FF +:1050C000FCF7B4FEE8E7BDE8F041FFF70FBF0000D2 +:1050D00012F0030F2DE9F04107460C4615461E4617 +:1050E00017D00E44B44202D10020BDE8F081294619 +:1050F0004FF40072F86B05F50075FCF797FE01237D +:10510000FA6B21463846FFF759FF0028EDD10134EC +:10511000E8E7BDE8F041FFF751BF0000002070470D +:10512000302310B583F311880024436C40302146AE +:10513000DC6301F0C7FA84F3118810BD026843688C +:105140001143016003B1184770470000024A136819 +:1051500043F0C003136070470014014013B50E4CB8 +:10516000204600F0DFFC04F1140000234FF400722D +:105170000A49009400F07AF9094B4FF4007209498A +:1051800004F13800009400F0F3F9074A074BC4E932 +:10519000172302B010BD00BF183900208439002049 +:1051A0004D510008843B00200014014000E1F5054A +:1051B000037C30B5244C002918BF0C46012B11D1BB +:1051C000224B98420ED1224BD3F8F02042F020021D +:1051D000C3F8F020D3F8182142F02002C3F81821B8 +:1051E000D3F818312268036EC16D03EB5203846655 +:1051F000B3FBF2F36268150442BF23F0070503F026 +:10520000070343EA4503CB60A36843F040034B60C8 +:10521000E36843F001038B6042F4967343F00103AB +:105220000B604FF0FF330B62510505D512F01022D1 +:1052300005D0B2F1805F04D080F8643030BD7F23A8 +:10524000FAE73F23F8E700BFBC96000818390020B2 +:10525000004402582DE9F047C66D05463768F469E9 +:10526000210734621AD014F0080118BF4FF480717E +:10527000E20748BF41F02001A3074FF0300348BFC9 +:1052800041F04001600748BF41F0800183F311887D +:10529000281DFFF753FF002383F31188E2050AD589 +:1052A000302383F311884FF48061281DFFF746FFF8 +:1052B000002383F311884FF030094FF0000A14F0F7 +:1052C000200838D13B0616D54FF0300905F1380AD1 +:1052D000200610D589F31188504600F07DF900288A +:1052E00036DA0821281DFFF729FF27F080033360F5 +:1052F000002383F31188790614D5620612D5302372 +:1053000083F31188D5E913239A4208D12B6C33B16A +:1053100027F040071021281DFFF710FF37600023FA +:1053200083F31188E30618D5AA6E1369ABB15069EF +:10533000BDE8F047184789F31188736A284695F845 +:105340006410194000F008FC8AF31188F469B6E78C +:10535000B06288F31188F469BAE7BDE8F08700000D +:10536000090100F16043012203F56143C9B283F8EA +:10537000001300F01F039A4043099B0003F16043B0 +:1053800003F56143C3F880211A60704700F01F03E2 +:1053900001229A40430900F160409B0000F5614002 +:1053A00003F1604303F56143C3F88020C3F8802113 +:1053B000002380F800337047F8B51546826804462C +:1053C0000B46AA4200D28568A1692669761AB542C1 +:1053D0000BD218462A46FCF729FDA3692B44A3618A +:1053E0002846A3685B1BA360F8BD0CD9AF1B184609 +:1053F0003246FCF71BFD3A46E1683044FCF716FDE7 +:10540000E3683B44EBE718462A46FCF70FFDE368E8 +:10541000E5E7000083689342F7B50446154600D2DD +:105420008568D4E90460361AB5420BD22A46FCF7E7 +:10543000FDFC63692B4463612846A3685B1BA36082 +:1054400003B0F0BD0DD93246AF1B0191FCF7EEFC65 +:1054500001993A46E0683144FCF7E8FCE3683B44D4 +:10546000E9E72A46FCF7E2FCE368E4E710B50A4402 +:105470000024C361029B8460C16002610362C0E9D1 +:105480000000C0E9051110BD08B5D0E9053293420E +:1054900001D1826882B98268013282605A1C4261FD +:1054A00019700021D0E904329A4224BFC3684361D5 +:1054B00001F022F9002008BD4FF0FF30FBE70000AB +:1054C00070B5302304460E4683F31188A568A5B154 +:1054D000A368A269013BA360531CA36115782269EC +:1054E000934224BFE368A361E3690BB12046984768 +:1054F000002383F31188284607E03146204601F057 +:10550000EBF80028E2DA85F3118870BD2DE9F74F3A +:1055100004460E4617469846D0F81C904FF0300AC5 +:105520008AF311884FF0000B154665B12A463146C3 +:105530002046FFF741FF034660B94146204601F08F +:10554000CBF80028F1D0002383F31188781B03B037 +:10555000BDE8F08FB9F1000F03D001902046C84795 +:10556000019B8BF31188ED1A1E448AF31188DCE746 +:10557000C160C361009B82600362C0E905111144F0 +:10558000C0E9000001617047F8B504460D461646B3 +:10559000302383F31188A768A7B1A368013BA360F8 +:1055A00063695A1C62611D70D4E904329A4224BFB7 +:1055B000E3686361E3690BB120469847002080F3FC +:1055C000118807E03146204601F086F80028E2DA2B +:1055D00087F31188F8BD0000D0E9052310B59A4281 +:1055E00001D182687AB982680021013282605A1C36 +:1055F00082611C7803699A4224BFC368836101F009 +:105600007BF8204610BD4FF0FF30FBE72DE9F74F48 +:1056100004460E4617469846D0F81C904FF0300AC4 +:105620008AF311884FF0000B154665B12A463146C2 +:105630002046FFF7EFFE034660B94146204601F0E1 +:105640004BF80028F1D0002383F31188781B03B0B6 +:10565000BDE8F08FB9F1000F03D001902046C84794 +:10566000019B8BF31188ED1A1E448AF31188DCE745 +:105670000379052B05BF836A002001204B6004BF1E +:105680004FF400730B60704770B55D1E866A044668 +:105690000D44B54205D9436B43F0800343630120B9 +:1056A00070BD06250571FFF7EDFC05232371F7E7B3 +:1056B00070B55D1E866A04460D44B54205D9436B3C +:1056C00043F080034363012070BD07250571FFF798 +:1056D000FFFC05232371F7E738B505790446052D4E +:1056E00005D108230371FFF719FD257138BD01208D +:1056F000FCE700000323F0B5037185B00446FFF713 +:10570000B3FA002220461146FFF7F8FA4FF4D5729B +:1057100003AB08212046FFF713FB0246B8B901236B +:105720002363039BC3F30323012B09D103AB37216D +:105730002046FFF705FB18B9A44B039A1340ABB101 +:1057400020460125FFF7C2FA0223237137E103AB9C +:10575000002237212046FFF7F3FA28B99B4A039B22 +:105760001A40002A00F0A78002232363236B03F072 +:105770000F03022B40F0A9806425954E42F2107071 +:1057800000F086FF03AB324601212046FFF7C2FA44 +:105790000028D5D1039B002B80F293805A0003D5BB +:1057A000236B43F010032363002204F1080302215A +:1057B0002046FFF723FB02460028C1D104F138033D +:1057C00003212046FFF7BCFA0028B9D104F11805DF +:1057D000A26B092120462B46FFF710FB0028AFD112 +:1057E00002ABA26B07212046FFF7AAFA0646002863 +:1057F000A6D1236B03F00F03022B40F08F807E2293 +:105800007F21284603F010F9012840F28780E76BDA +:1058100042F2107000F03CFF08234FF4007239464A +:1058200020460096FFF706FB002889D1384603F092 +:1058300049F9236BA06203F00F03022B72D103AB73 +:10584000644A06212046FFF77BFA002871D15F49A0 +:10585000039B1940B1FA81F149092046FFF716FA76 +:1058600002AB4FF4007210212046FFF769FA05469B +:1058700000287FF465AF554E029B33427FF460AF42 +:10588000236B13F00E0F03F00F0273D0022A7FF484 +:1058900057AFE36A1978012900F09480022900F0DB +:1058A0009380002900F089804B4F2046FFF714FABF +:1058B00003AB3A4676E0114620462263FFF71EFA14 +:1058C00054E7013D7FF45AAF3AE7444D6426444A19 +:1058D0003E4F012B18BF154603AB0022372120464F +:1058E000FFF72EFA00287FF42BAF039B3B427FF497 +:1058F00027AF03AB2A4629212046FFF70BFA0028E1 +:105900007FF41EAF039B002BFFF648AF013E3FF430 +:1059100017AF42F2107000F0BBFEDDE7284603F03F +:10592000A5F886E77E227F212846E66B03F07CF807 +:1059300008B9002191E70023402231462046009318 +:105940000623FFF777FA0028F3D1B3895BBA9B07E8 +:10595000EFD5244B40223146204600930623FFF723 +:1059600069FA0028E5D1317C01F00F010F3918BF29 +:10597000012172E7E36A1978F9B101297FF4E0AEF9 +:105980002046FFF7A9F903ABA26B37212046FFF7AA +:10599000D7F900287FF4D4AE039B33427FF4D0AE16 +:1059A00003AB022206212046FFF7CAF900287FF444 +:1059B000C7AE039B33427FF4C3AE05232371284651 +:1059C00005B0F0BD084F70E7084F6EE708E0FFFD37 +:1059D0000080FFC00001B9030000B7030080FF5042 +:1059E00000001080F1FFFF800001B7030002B70341 +:1059F00037B504460C4D01ABA26B0D212046FFF7D5 +:105A00009FF978B9019B2B420BD1C3F34323042B9D +:105A100008D0053B022B04D84FF47A7000F038FE12 +:105A2000E9E7012003B030BD08E0FFFD70B5302389 +:105A3000054683F3118803790024022B03D184F3F4 +:105A40001188204670BD0423037184F31188022657 +:105A5000FFF7CEFF04462846FFF738F92E71F0E72E +:105A6000FFF790B8044B03600123037100234363E5 +:105A7000C0E90A33704700BFD496000810B5302340 +:105A8000044683F31188C162FFF796F802230020D1 +:105A9000237180F3118810BD10B53023044683F3C1 +:105AA0001188FFF7B3F800230122E362227183F328 +:105AB000118810BD026843681143016003B11847A3 +:105AC000704700001430FFF721BD00004FF0FF3396 +:105AD0001430FFF71BBD00003830FFF797BD000002 +:105AE0004FF0FF333830FFF791BD00001430FFF75F +:105AF000E7BC00004FF0FF311430FFF7E1BC0000BD +:105B00003830FFF741BD00004FF0FF323830FFF76B +:105B10003BBD0000012914BF6FF013000020704747 +:105B2000FFF71CBB044B036000234360C0E9023352 +:105B300001230374704700BFF896000810B53023A6 +:105B4000044683F31188FFF733FB022300202374FC +:105B500080F3118810BD000038B5C36904460D46B6 +:105B60001BB904210844FFF7A5FF294604F11400DE +:105B7000FFF78AFC002806DA201D4FF40061BDE81B +:105B80003840FFF797BF38BD026843681143016092 +:105B900003B118477047000013B5406B00F58054FF +:105BA000D4F8A4381A681178042914D1017C022988 +:105BB00011D11979012312898B4013420BD101A90C +:105BC0004C3002F05FFED4F8A4480246019B2179D4 +:105BD000206800F0DFF902B010BD0000143002F0C0 +:105BE000E1BD00004FF0FF33143002F0DBBD0000D8 +:105BF0004C3002F0B3BE00004FF0FF334C3002F0E7 +:105C0000ADBE0000143002F0AFBD00004FF0FF3118 +:105C1000143002F0A9BD00004C3002F07FBE00003D +:105C20004FF0FF324C3002F079BE00000020704788 +:105C300010B500F58054D4F8A4381A6811780429F6 +:105C400017D1017C022914D15979012352898B4043 +:105C500013420ED1143002F041FD024648B1D4F88F +:105C6000A4484FF4407361792068BDE8104000F00B +:105C70007FB910BD406BFFF7DBBF0000704700002D +:105C80007FB5124B012504260446036000230574EA +:105C900000F1840243602946C0E902330C4B0290B4 +:105CA000143001934FF44073009602F0F3FC094B5B +:105CB00004F69442294604F14C000294CDE90063B5 +:105CC0004FF4407302F0BAFD04B070BD2097000895 +:105CD000755C0008995B00080A68302383F311881B +:105CE0000B790B3342F823004B79133342F823002E +:105CF0008B7913B10B3342F8230000F58053C3F8BE +:105D0000A41802230374002080F311887047000058 +:105D100038B5037F044613B190F85430ABB9012570 +:105D2000201D0221FFF730FF04F114006FF0010184 +:105D3000257700F0EDFC04F14C0084F854506FF02E +:105D40000101BDE8384000F0E3BC38BD10B50121C9 +:105D500004460430FFF718FF0023237784F85430FB +:105D600010BD000038B504460025143002F0AAFC2E +:105D700004F14C00257702F079FD201D84F8545081 +:105D80000121FFF701FF2046BDE83840FFF750BF73 +:105D900090F8803003F06003202B06D190F881202A +:105DA0000023212A03D81F2A06D800207047222A60 +:105DB000FBD1C0E91D3303E0034A42670722826733 +:105DC000C3670120704700BF3C22002037B500F5B3 +:105DD0008055D5F8A4381A68117804291AD1017CA5 +:105DE000022917D11979012312898B40134211D14D +:105DF00000F14C04204602F0F9FD58B101A92046FB +:105E000002F040FDD5F8A4480246019B21792068A4 +:105E100000F0C0F803B030BD01F10B03F0B550F84D +:105E2000236085B004460D46FEB1302383F311880C +:105E300004EB8507301D0821FFF7A6FEFB6806F17D +:105E40004C005B691B681BB1019002F029FD0198B1 +:105E500003A902F017FD024648B1039B29462046DC +:105E600000F098F8002383F3118805B0F0BDFB68BB +:105E70005A691268002AF5D01B8A013B1340F1D100 +:105E800004F18002EAE70000133138B550F82140F0 +:105E9000ECB1302383F3118804F58053D3F8A428A0 +:105EA0001368527903EB8203DB689B695D6845B137 +:105EB00004216018FFF768FE294604F1140002F07F +:105EC00017FC2046FFF7B4FE002383F3118838BD8A +:105ED0007047000001F04ABD01234022002110B5A7 +:105EE000044600F8303BFBF7C7FF0023C4E9013349 +:105EF00010BD000010B53023044683F3118824221E +:105F0000416000210C30FBF7B7FF204601F050FD47 +:105F100002230020237080F3118810BD70B500EBC0 +:105F20008103054650690E461446DA6018B1102206 +:105F30000021FBF7A1FFA06918B110220021FBF797 +:105F40009BFF31462846BDE8704001F037BE000097 +:105F500083682022002103F0011310B504468360FA +:105F60001030FBF789FF2046BDE8104001F0B2BEBB +:105F7000F0B4012500EB810447898D40E4683D437E +:105F8000A469458123600023A2606360F0BC01F036 +:105F9000CFBE0000F0B4012500EB810407898D40DD +:105FA000E4683D436469058123600023A260636067 +:105FB000F0BC01F045BF000070B502230025044687 +:105FC000242203702946C0F888500C3040F8045C45 +:105FD000FBF752FF204684F8705001F083FD6368A0 +:105FE0001B6823B129462046BDE87040184770BDA4 +:105FF0000378052B10B504460AD080F88C300523B1 +:10600000037043681B680BB1042198470023A36009 +:1060100010BD00000178052906D190F88C20436856 +:1060200002701B6803B118477047000070B590F804 +:106030007030044613B1002380F8703004F1800200 +:10604000204601F06BFE63689B68B3B994F880301A +:1060500013F0600535D00021204602F05DF90021E3 +:10606000204602F04DF963681B6813B106212046F3 +:106070009847062384F8703070BD20469847002862 +:10608000E4D0B4F88630A26F9A4288BFA36794F92F +:106090008030A56F002B4FF0300380F20381002D7C +:1060A00000F0F280092284F8702083F31188002127 +:1060B0002046D4E91D23FFF76DFF002383F31188E9 +:1060C000DAE794F8812003F07F0343EA022340F2E9 +:1060D0000232934200F0C58021D8B3F5807F48D0CA +:1060E0000DD8012B3FD0022B00F09380002BB2D1B2 +:1060F00004F1880262670222A267E367C1E7B3F591 +:10610000817F00F09B80B3F5407FA4D194F882306A +:10611000012BA0D1B4F8883043F0020332E0B3F58C +:10612000006F4DD017D8B3F5A06F31D0A3F5C06381 +:10613000012B90D86368204694F882205E6894F81A +:106140008310B4F88430B047002884D04368636774 +:106150000368A3671AE0B3F5106F36D040F6024229 +:1061600093427FF478AF5C4B63670223A3670023FD +:10617000C3E794F88230012B7FF46DAFB4F8883018 +:1061800023F00203A4F88830C4E91D55E56778E7D9 +:10619000B4F88030B3F5A06F0ED194F88230204669 +:1061A00084F88A3001F0FCFC63681B6813B101219C +:1061B00020469847032323700023C4E91D339CE73E +:1061C00004F18B0363670123C3E72378042B10D109 +:1061D000302383F311882046FFF7BAFE85F3118838 +:1061E0000321636884F88B5021701B680BB1204633 +:1061F000984794F88230002BDED084F88B3004234B +:10620000237063681B68002BD6D002212046984774 +:10621000D2E794F8843020461D0603F00F010AD51A +:1062200001F06EFD012804D002287FF414AF2B4B3F +:106230009AE72B4B98E701F055FDF3E794F882308D +:10624000002B7FF408AF94F8843013F00F01B3D023 +:106250001A06204602D502F077F8ADE702F068F89A +:10626000AAE794F88230002B7FF4F5AE94F88430DE +:1062700013F00F01A0D01B06204602D502F04CF807 +:106280009AE702F03DF897E7142284F8702083F330 +:1062900011882B462A4629462046FFF769FE85F3DA +:1062A0001188E9E65DB1152284F8702083F3118826 +:1062B00000212046D4E91D23FFF75AFEFDE60B22FC +:1062C00084F8702083F311882B462A4629462046FD +:1062D000FFF760FEE3E700BF50970008489700080B +:1062E0004C97000838B590F870300446002B3ED02B +:1062F000063BDAB20F2A34D80F2B32D8DFE803F08E +:1063000037313108223231313131313131313737A2 +:10631000856FB0F886309D4214D2C3681B8AB5FBE6 +:10632000F3F203FB12556DB9302383F311882B462A +:106330002A462946FFF72EFE85F311880A2384F8A2 +:1063400070300EE0142384F87030302383F311880A +:10635000002320461A461946FFF70AFE002383F35E +:10636000118838BDC36F03B198470023E7E70021C8 +:10637000204601F0D1FF0021204601F0C1FF6368F3 +:106380001B6813B10621204698470623D7E7000073 +:1063900010B590F870300446142B29D017D8062B6E +:1063A00005D001D81BB110BD093B022BFBD8002141 +:1063B000204601F0B1FF0021204601F0A1FF6368F3 +:1063C0001B6813B1062120469847062319E0152BB8 +:1063D000E9D10B2380F87030302383F31188002338 +:1063E0001A461946FFF7D6FD002383F31188DAE732 +:1063F000C3689B695B68002BD5D1C36F03B1984715 +:10640000002384F87030CEE70023826880F82430BF +:1064100083691B6899689142FBD25A6803604260A5 +:1064200010605860704700000023826880F82430B4 +:1064300083691B6899689142FBD85A68036042607F +:10644000106058607047000008B50846302383F399 +:10645000118891F82430032B05D0042B0DD02BB9D3 +:1064600083F3118808BD8B6A00221A604FF0FF3356 +:106470008362FFF7C9FF0023F2E7D1E9003213601E +:106480005A60F3E7034610B51B68984203D09C6836 +:106490008A689442F8D25A680B604A601160596069 +:1064A00010BD0000FFF7B0BF38B50B4B1C68DD68AE +:1064B00085F82400226853601A600122DC60A3681A +:1064C00084F82420934201D101F016F92946204690 +:1064D000BDE83840F9F796BF883D00200C4B30B539 +:1064E000DD684B1C87B004460FD02B46094A68462E +:1064F00000F0A2F92046FFF7D7FF009B13B16846D2 +:1065000000F0A4F9A86A07B030BDFFF7CDFFF9E7A6 +:10651000883D002049640008044B1A68DB689068D5 +:106520009B68984294BF002001207047883D00205E +:1065300038B50C4B1C68DD68226853601A60012274 +:10654000DC6084F82420AB68934201D101F0D6F8D6 +:106550002846FFF769FF01462046BDE83840F9F7B5 +:1065600051BF00BF883D0020044B1A68DB68926869 +:106570009B689A4201D9FFF7DBBF7047883D002036 +:1065800038B50123084C00252370656002F0C8FA75 +:1065900002F0EEFA0549064802F0C4FB022323701C +:1065A00085F3118838BD00BF30400020589700089F +:1065B000883D002000F080B9034A516853685B1A97 +:1065C0009842FBD8704700BF001000E08B604B631F +:1065D0000023CA6100F128020B6302230A618B8445 +:1065E0000123886181F8263001F11003C26A4A61F3 +:1065F0001360C36201F12C030846CB6270470000B0 +:10660000D0E90131026841F8183CA1F19C03383906 +:10661000CB60036941F8243C436941F8203C034BBB +:1066200041F8043CC3680248FFF7D0BF1D040008CE +:10663000883D002008B5FFF7E3FFBDE80840FFF7FD +:1066400031BF000038B50E4BDC6804F12C05A062A8 +:10665000E06AA8420FD194F826303BB994F825306F +:106660009B0702BFD4E9043213605A600F20BDE8D3 +:106670003840FFF719BF0368E362FFF713FFE7E74E +:10668000883D0020302383F31188FFF7DBBF000033 +:1066900008B50146302383F311880820FFF71EFF59 +:1066A000002383F3118808BD054BDB6821B103602B +:1066B00098620320FFF712BF4FF0FF30704700BF12 +:1066C000883D002003682BB10022026018469962C1 +:1066D000FFF7E8BE70470000064BDB6839B142683F +:1066E00018605A60136043600420FFF7F7BE4FF054 +:1066F000FF307047883D00200368984206D01A6832 +:106700000260506018469962FFF7CCBE70470000E7 +:1067100038B504460D462068844200D138BD036870 +:1067200023605C608562FFF7BDFEF4E7036810B587 +:106730009C68A2420CD85C688A600B604C60216047 +:10674000596099688A1A9A604FF0FF33836010BDD0 +:10675000121B1B68ECE700000A2938BF0A2170B53C +:1067600004460D460A26601902F0D4F902F0BCF97D +:10677000041BA54203D8751C04462E46F3E70A2ED7 +:1067800004D90120BDE8704002F094BB70BD000048 +:10679000F8B5144B0D460A2A4FF00A07D96103F1E8 +:1067A0001001826038BF0A224160196914460160F5 +:1067B00048601861A81802F09DF902F095F9431B92 +:1067C0000646A34206D37C1C28192746354602F00C +:1067D000A1F9F2E70A2F04D90120BDE8F84002F040 +:1067E00069BBF8BD883D0020F8B506460D4602F0AD +:1067F0007BF90F4A134653F8107F9F4206D12A4671 +:1068000001463046BDE8F840FFF7C2BFD169BB681A +:10681000441A2C1928BF2C46A34202D92946FFF757 +:106820009BFF224631460348BDE8F840FFF77EBF94 +:10683000883D0020983D0020C0E90323002310B4C8 +:106840005DF8044B4361FFF7CFBF000010B5194C52 +:10685000236998420DD08168D0E9003213605A60F4 +:106860009A680A449A60002303604FF0FF33A361E3 +:1068700010BD0268234643F8102F536000220260C7 +:1068800022699A4203D1BDE8104002F03DB99368F5 +:1068900081680B44936002F027F92269E1699268EC +:1068A000441AA242E4D91144BDE81040091AFFF786 +:1068B00053BF00BF883D00202DE9F047DFF8BC80C2 +:1068C00008F110072C4ED8F8105002F00DF9D8F846 +:1068D0001C40AA68031B9A423ED814444FF000099A +:1068E000D5E90032C8F81C4013605A60C5F8009022 +:1068F000D8F81030B34201D102F006F989F31188BB +:10690000D5E9033128469847302383F311886B6912 +:10691000002BD8D002F0E8F86A69A0EB040982469F +:106920004A450DD2022002F0C5FA0022D8F81030F4 +:10693000B34208D151462846BDE8F047FFF728BFCB +:10694000121A2244F2E712EB09092946384638BFE9 +:106950004A46FFF7EBFEB5E7D8F81030B34208D04F +:106960001444C8F81C00211AA960BDE8F047FFF7DD +:10697000F3BEBDE8F08700BF983D0020883D0020B1 +:1069800038B502F0B1F8054AD2E90845031B1819D9 +:1069900045F10001C2E9080138BD00BF883D002073 +:1069A00010B560B9074804790368053C9B6818BFB7 +:1069B0000124984708B144F00404204610BD012486 +:1069C000FBE700BFC0360020FFF7EABF2DE9F04724 +:1069D000884617469A460446B0B90D4E3579052DBE +:1069E00005D003240DE0013D15F0FF050ED03268FF +:1069F000534639463046D2F814904246C8470028DC +:106A0000F1D12046BDE8F0870424FAE70124F8E735 +:106A1000C03600202DE9F047884617469A460446BE +:106A2000B0B90D4E3579052D05D003240DE0013D9B +:106A300015F0FF050ED03268534639463046D2F87D +:106A400018904246C8470028F1D12046BDE8F0879B +:106A50000424FAE70124F8E7C036002037B50C46D5 +:106A6000154670B951B101290BD107486946036831 +:106A70001B6A984710B9019B04462B60204603B05F +:106A800030BD0424FAE700BFC03600200020704764 +:106A9000FEE70000704700004FF0FF307047000035 +:106AA0004B6843608B688360CB68C3600B6943614C +:106AB0004B6903628B6943620B6803607047000097 +:106AC00008B53C4B40F2FF713B48D3F888200A439D +:106AD000C3F88820D3F8882022F4FF6222F007024E +:106AE000C3F88820D3F88820D3F8E0200A43C3F8FD +:106AF000E020D3F808210A43C3F808212F4AD3F82D +:106B000008311146FFF7CCFF00F5806002F11C014F +:106B1000FFF7C6FF00F5806002F13801FFF7C0FF04 +:106B200000F5806002F15401FFF7BAFF00F58060C4 +:106B300002F17001FFF7B4FF00F5806002F18C01F3 +:106B4000FFF7AEFF00F5806002F1A801FFF7A8FF94 +:106B500000F5806002F1C401FFF7A2FF00F580603C +:106B600002F1E001FFF79CFF00F5806002F1FC01FB +:106B7000FFF796FF02F58C7100F58060FFF790FF3C +:106B800001F07AFC0E4BD3F8902242F00102C3F8D8 +:106B90009022D3F8942242F00102C3F894220522F5 +:106BA000C3F898204FF06052C3F89C20054AC3F800 +:106BB000A02008BD00440258000002586C9700084D +:106BC00000ED00E01F00080308B501F04FFDFFF7DE +:106BD000D7FC104BD3F8DC2042F04002C3F8DC2095 +:106BE000D3F8042122F04002C3F80421D3F8043181 +:106BF000094B1A6842F008021A601A6842F004024F +:106C00001A6000F063FD00F039FBBDE8084000F0B9 +:106C1000B5B800BF00440258001802480120704770 +:106C2000002070477047000002290CD0032904D0CF +:106C30000129074818BF00207047032A05D80548D6 +:106C400000EBC2007047044870470020704700BF47 +:106C50006C9900084C2200202099000870B59AB069 +:106C600005460846144601A900F0C2F801A8FBF742 +:106C7000FBF8431C0022C6B25B001046C5E9003495 +:106C800023700323023404F8013C01ABD1B2023477 +:106C90008E4201D81AB070BD13F8011B013204F8FE +:106CA000010C04F8021CF1E708B5302383F31188C6 +:106CB0000348FFF78BF8002383F3118808BD00BF5A +:106CC0003840002090F8803003F01F02012A07D1DD +:106CD00090F881200B2A03D10023C0E91D3315E071 +:106CE00003F06003202B08D1B0F884302BB990F862 +:106CF0008120212A03D81F2A04D8FFF749B8222A65 +:106D0000EBD0FAE7034A426707228267C367012094 +:106D1000704700BF4322002007B5052917D8DFE8D8 +:106D200001F0191603191920302383F31188104A32 +:106D300001210190FFF7F2F8019802210D4AFFF7B7 +:106D4000EDF80D48FFF70EF8002383F3118803B028 +:106D50005DF804FB302383F311880748FEF7D8FF62 +:106D6000F2E7302383F311880348FEF7EFFFEBE7E8 +:106D7000C0980008E49800083840002038B50C4D51 +:106D80000C4C2A460C4904F10800FFF767FF05F197 +:106D9000CA0204F110000949FFF760FF05F5CA7245 +:106DA00004F118000649BDE83840FFF757BF00BF9F +:106DB000105900204C220020A0980008AA98000832 +:106DC000B598000870B5044608460D46FBF74CF828 +:106DD000C6B22046013403780BB9184670BD32465E +:106DE0002946FBF72DF80028F3D10120F6E7000033 +:106DF0002DE9F84F05460C46FBF736F82849C6B290 +:106E00002846FFF7DFFF08B1033EF6B225492846C2 +:106E1000FFF7D8FF08B11036F6B2632E0DD8DFF8B1 +:106E20008080DFF88090204FDFF884A0DFF884B006 +:106E30002E7846B92670BDE8F88F29462046BDE871 +:106E4000F84F02F0D5B8252E25D107224146284615 +:106E5000FAF7F6FF28B9DBF80030073544F8043BB1 +:106E6000E6E7082249462846FAF7EAFF98B9A21C45 +:106E70000E4B197802320909C95D02F8041C13F897 +:106E8000011B01F00F015345C95D02F8031CF0D14D +:106E900018340835CCE7013504F8016BC8E700BFAA +:106EA0008C990008B59800089999000800E8F11F2E +:106EB0000CE8F11F94990008BFF34F8F044B1A6937 +:106EC0005107FCD1D3F810215207F8D1704700BF09 +:106ED0000020005208B50D4B1B78ABB9FFF7ECFF53 +:106EE0000B4BDA68D10704D50A4A5A6002F188329E +:106EF0005A60D3F80C21D20706D5064AC3F80421FC +:106F000002F18832C3F8042108BD00BF6E5B002087 +:106F1000002000522301674508B5114B1B78F3B9D7 +:106F2000104B1A69510703D5DA6842F04002DA6063 +:106F3000D3F81021520705D5D3F80C2142F04002B6 +:106F4000C3F80C21FFF7B8FF064BDA6842F00102E4 +:106F5000DA60D3F80C2142F00102C3F80C2108BD1D +:106F60006E5B0020002000520F289ABF00F5806061 +:106F700040040020704700004FF4003070470000CC +:106F8000102070470F2808B50BD8FFF7EDFF00F56C +:106F900000330268013204D104308342F9D1012068 +:106FA00008BD0020FCE700000F2838B505463FD893 +:106FB000FFF782FF1F4CFFF78DFF4FF0FF330728CD +:106FC0006361C4F814311DD82361FFF775FF030214 +:106FD00043F02403E360E36843F08003E360236944 +:106FE0005A07FCD42846FFF767FFFFF7BDFF4FF4B1 +:106FF000003100F09BFA2846FFF78EFFBDE83840CD +:10700000FFF7C0BFC4F81031FFF756FFA0F1080327 +:107010001B0243F02403C4F80C31D4F80C3143F0C4 +:107020008003C4F80C31D4F810315B07FBD4D9E7E6 +:10703000002038BD002000522DE9F84F04460D46CF +:10704000104644EA0203DE0602D00020BDE8F88FB5 +:1070500020F01F00DFF8D4B0DFF8D4A0FFF73AFF2C +:1070600005EB0008454503D10120FFF755FFEDE78B +:1070700020222946204601F083FF10B92034203514 +:10708000F0E7234604F120021F68791CDDD10433A8 +:107090009342F9D104F178432048214EB3F5801F83 +:1070A000204B38BF184603F1F80332BFD946D1460A +:1070B0001E46FFF701FF0760A4EB050C336805F1DE +:1070C0001C0143F0020333602B1FD9F8007017F046 +:1070D0000507FAD153F8042F8B424CF80320F4D162 +:1070E000BFF34F8FFFF7E8FE4FF0FF3303603368C5 +:1070F00023F002033360BFF34F8F0B4BC3F85C42A6 +:10710000BFF34F8FBFF36F8F20222946204601F037 +:1071100037FF0028B2D03846A7E700BF142100523D +:107120000C2000521420005200ED00E0102000520C +:107130001021005210B5084C237828B11BB9FFF775 +:10714000C9FE0123237010BD002BFCD02070BDE8C8 +:107150001040FFF7E1BE00BF6E5B00202DE9F04F4D +:107160000D4685B0814658B111F00D0614BF20229E +:10717000082211F00803019304D0431E034269D092 +:10718000002435E0002E37D009F11F0121F01F093E +:107190004FF00108314F05F00403DFF8CCA005EAF9 +:1071A000080BBBF1000F32D07869C0072FD408F16B +:1071B00001080C37B8F1060FF3D19EB9284D4946A6 +:1071C000A819019201F0F6FC0446002839D12036B6 +:1071D000019AA02EF3D1494601F0ECFC04460028A8 +:1071E0002FD1019A49461F4801F0E4FC044660BBD8 +:1071F000204605B0BDE8F08F0029C9D101462846D8 +:10720000029201F0D7FC0446D8B9029AC0E713B144 +:1072100078694107CBD5AC0702D578698007C6D518 +:10722000019911B178690107C1D549460AEB4810A7 +:10723000CDE9022301F0BEFC0446DDE9022300286B +:10724000B5D04A460021204601E04A460021FAF71F +:1072500013FECDE70246002E96D199E7AC990008BF +:10726000B05B0020705B0020905B00200021FFF7E6 +:1072700075BF00000121FFF771BF000000231A460F +:1072800070B51A600433B3F5806FFAD1104D012444 +:10729000104E06EB441001342A6955F80C1F01F01A +:1072A00075FC062CF5D137254FF4C0542046FFF766 +:1072B000E1FF014628B122460748BDE8704001F0D1 +:1072C00065BCC4EBC404013D4FEAD404EED170BDEB +:1072D000AC990008905B0020705B00200421FFF750 +:1072E0003DBF00004843FFF7C1BF000008B101F0F7 +:1072F000D3BC7047B0F5805F10B5044607D8FFF7E0 +:10730000EDFF28B92046BDE81040FFF7AFBF0020D1 +:1073100010BD0000FFF7EABF70B5AAB140EA010353 +:1073200013F01F030FD1094C0144A5686D0706D562 +:107330002568A84203D366683544A94204D90333BB +:107340000C34122BF1D10022104670BDAC9900080C +:1073500008B501F0B7FD034AD2E90032C01842EB8C +:10736000010108BD505C0020434BD3E900232DE907 +:10737000F34113437CD0FFF7EBFF404A0023002783 +:10738000F9F75AF806460D463D4A0023F9F754F836 +:107390000023144630462946394AF9F74DF84FF490 +:1073A00061613C23ADF80170B4FBF1F5B4FBF3F679 +:1073B00001FB154103FB16464624B1FBF3F1314BAB +:1073C000F6B28DF8004098423CD84FF0640C4FF470 +:1073D000C87EA30704F26C7225D1B2FBFCF30CFB50 +:1073E000132313BBB2FBFEF30EFB1322B2FA82F39C +:1073F0005B0903F26D18621C8045D2B217D90FB138 +:107400008DF800400022204C4FF00C0C17460CFB6E +:107410000343D4B2013213F804C084450CD8A0EB66 +:107420000C000127F5E70023E3E70123E1E7A0EBE8 +:10743000080014460127CCE70FB18DF80140431C2A +:107440008DF802309DF80100431C9DF80000503873 +:10745000400640EA43509DF8023040EA034040EACB +:10746000560040EAC52040EA411002B0BDE8F08174 +:107470004FF40410F9E700BF505C002040420F00B9 +:107480008051010090230B00F49900080244074B3F +:10749000D2B210B5904200D110BD441C00B253F8D6 +:1074A000200041F8040BE0B2F4E700BF5040005860 +:1074B0000E4B30B51C6F240405D41C6F1C671C6F69 +:1074C00044F400441C670A4C02442368D2B243F4DB +:1074D00080732360074B904200D130BD441C51F8AB +:1074E000045B00B243F82050E0B2F4E700440258D5 +:1074F000004802585040005807B5012201A9002059 +:10750000FFF7C4FF019803B05DF804FB13B5044610 +:10751000FFF7F2FFA04205D0012201A9002001944B +:10752000FFF7C6FF02B010BD704700000144BFF373 +:107530004F8F064B884204D3BFF34F8FBFF36F8F3B +:107540007047C3F85C022030F4E700BF00ED00E0B4 +:107550000144BFF34F8F064B884204D3BFF34F8FD4 +:10756000BFF36F8F7047C3F870022030F4E700BF9D +:1075700000ED00E070B5054616460C4601201021CE +:10758000FFF7B0FE286046733CB1204636B1FFF7E6 +:10759000A5FE2B68186000B19C6070BDFFF76AFE05 +:1075A000F7E70000F8B50F461546044648B905F15F +:1075B0001F010126386821F01F01FFF7B7FF304691 +:1075C000F8BD427B29463868FFF7A6FE064600282C +:1075D000EDD13B686360A368AB4210D213B1206861 +:1075E000FFF784FE637B28462BB1FFF777FE206010 +:1075F00020B9A060E3E7FFF73DFEF8E7A56020684B +:1076000005F11F01012621F01F013860FFF78EFFF1 +:107610002673D4E770B50E4602F11F0104461546E5 +:1076200021F01F013046FFF793FF4CB12168B142B2 +:1076300006D1606810B12A46FAF7F8FB00232373DD +:1076400070BD0000F8B50F461446054648B904F170 +:107650001F010126386821F01F01FFF779FF30462E +:10766000F8BD427B21463868FFF756FE06460028E3 +:10767000EDD1AB68A34210D213B12868FFF736FEF4 +:107680006B7B20462BB1FFF729FE286020B9A8604C +:10769000E5E7FFF7EFFDF8E7AC60396819B122467E +:1076A0002868FAF7C3FB286804F11F01012621F0BE +:1076B0001F013860FFF74CFF2E73D0E720B103683D +:1076C0008B4204BF0023037370470000034B1A680A +:1076D0001AB9034AD2F8D0241A607047585C0020C7 +:1076E0000040025808B5FFF7F1FF024B1868C0F3DD +:1076F000806008BD585C00207047000070470000A3 +:10770000EFF30983054968334A6B22F001024A63AB +:1077100083F30988002383F31188704700EF00E0AA +:10772000302080F3118862B60D4B0E4AD96821F4DF +:10773000E0610904090C0A430B49DA60D3F8FC2024 +:1077400042F08072C3F8FC20084AC2F8B01F1168EA +:1077500041F0010111602022DA7783F8220070479E +:1077600000ED00E00003FA0555CEACC5001000E0C6 +:10777000302310B583F311880E4B5B6813F400635C +:1077800014D0F1EE103AEFF309844FF08073683CA7 +:10779000E361094BDB6B236684F30988FEF7BCFECB +:1077A00010B1064BA36110BD054BFBE783F31188B5 +:1077B000F9E700BF00ED00E000EF00E02F04000853 +:1077C0003204000870B5BFF34F8FBFF36F8F1A4AB2 +:1077D0000021C2F85012BFF34F8FBFF36F8F536970 +:1077E00043F400335361BFF34F8FBFF36F8FC2F881 +:1077F0008410BFF34F8FD2F8803043F6E074C3F3A8 +:10780000C900C3F34E335B0103EA0406014646EAAE +:1078100081750139C2F86052F9D2203B13F1200F73 +:10782000F2D1BFF34F8F536943F480335361BFF3F9 +:107830004F8FBFF36F8F70BD00ED00E0FEE70000DB +:10784000214B2248224A70B5904237D3214BC11EAA +:10785000DA1C121A22F003028B4238BF00220021E8 +:10786000FAF70AFB1C4A0023C2F88430BFF34F8F9B +:10787000D2F8803043F6E074C3F3C900C3F34E334B +:107880005B0103EA0406014646EA81750139C2F844 +:107890006C52F9D2203B13F1200FF2D1BFF34F8F7E +:1078A000BFF36F8FBFF34F8FBFF36F8F0023C2F80B +:1078B0005032BFF34F8FBFF36F8F70BD53F8041B6F +:1078C00040F8041BC0E700BFF49A0008D45D002014 +:1078D000D45D0020D45D002000ED00E0074BD3F81C +:1078E000D81021EA0001C3F8D810D3F8002122EA09 +:1078F0000002C3F80021D3F8003170470044025859 +:1079000070B5D0E9244300224FF0FF359E6804EBA8 +:1079100042135101D3F80009002805DAD3F8000911 +:1079200040F08040C3F80009D3F8000B002805DAC6 +:10793000D3F8000B40F08040C3F8000B013263180D +:107940009642C3F80859C3F8085BE0D24FF0011320 +:10795000C4F81C3870BD0000890141F020010161AC +:1079600003699B06FCD41220FEF726BE10B50A4C14 +:107970002046FEF7B1FA094BC4F89030084BC4F822 +:107980009430084C2046FEF7A7FA074BC4F8903015 +:10799000064BC4F8943010BD5C5C00200000084029 +:1079A000309A0008F85C0020000004403C9A00086F +:1079B00070B503780546012B5CD1434BD0F890405D +:1079C000984258D1414B0E216520D3F8D82042F07F +:1079D0000062C3F8D820D3F8002142F00062C3F857 +:1079E0000021D3F80021D3F8802042F00062C3F8D0 +:1079F0008020D3F8802022F00062C3F88020D3F8E2 +:107A00008030FDF7ADFC324BE360324BC4F80038F8 +:107A10000023D5F89060C4F8003EC02323604FF4E3 +:107A20000413A3633369002BFCDA01230C203361B8 +:107A3000FEF7C2FD3369DB07FCD41220FEF7BCFD64 +:107A40003369002BFCDA00262846A660FFF758FFB2 +:107A50006B68C4F81068DB68C4F81468C4F81C6864 +:107A600083BB1D4BA3614FF0FF336361A36843F0F9 +:107A70000103A36070BD194B9842C9D1134B4FF05D +:107A80008060D3F8D82042F00072C3F8D820D3F831 +:107A9000002142F00072C3F80021D3F80021D3F88E +:107AA000802042F00072C3F88020D3F8802022F0BA +:107AB0000072C3F88020D3F88030FFF70FFF0E214B +:107AC0004D209EE7064BCDE75C5C00200044025849 +:107AD0004014004003002002003C30C0F85C00204D +:107AE000083C30C0F8B5D0F89040054600214FF072 +:107AF00000662046FFF730FFD5F8941000234FF0C2 +:107B000001128F684FF0FF30C4F83438C4F81C28D5 +:107B100004EB431201339F42C2F80069C2F8006BC4 +:107B2000C2F80809C2F8080BF2D20B68D5F8902009 +:107B3000C5F89830636210231361166916F01006B9 +:107B4000FBD11220FEF738FDD4F8003823F4FE6391 +:107B5000C4F80038A36943F4402343F01003A36141 +:107B60000923C4F81038C4F814380B4BEB604FF0FD +:107B7000C043C4F8103B094BC4F8003BC4F810697B +:107B8000C4F80039D5F8983003F1100243F480139B +:107B9000C5F89820A362F8BD0C9A00084080001038 +:107BA000D0F8902090F88A10D2F8003823F4FE63C1 +:107BB00043EA0113C2F80038704700002DE9F8438A +:107BC00000EB8103D0F890500C468046DA680FFA3B +:107BD00081F94801166806F00306731E022B05EBB7 +:107BE00041134FF0000194BFB604384EC3F8101B88 +:107BF0004FF0010104F1100398BF06F1805601FA1D +:107C000003F3916998BF06F5004600293AD0578AD8 +:107C100004F15801374349016F50D5F81C180B4344 +:107C20000021C5F81C382B180127C3F81019A740EC +:107C30005369611E9BB3138A928B9B08012A88BFEC +:107C40005343D8F89820981842EA034301F14002C0 +:107C50002146C8F89800284605EB82025360FFF7DA +:107C60007BFE08EB8900C3681B8A43EA84534834CF +:107C70001E4364012E51D5F81C381F43C5F81C78EB +:107C8000BDE8F88305EB4917D7F8001B21F4004144 +:107C9000C7F8001BD5F81C1821EA0303C0E704F15C +:107CA0003F030B4A2846214605EB83035A60FFF742 +:107CB00053FE05EB4910D0F8003923F40043C0F817 +:107CC0000039D5F81C3823EA0707D7E700800010F1 +:107CD00000040002D0F894201268C0F89820FFF742 +:107CE0000FBE00005831D0F8903049015B5813F4B2 +:107CF000004004D013F4001F0CBF02200120704785 +:107D00004831D0F8903049015B5813F4004004D05A +:107D100013F4001F0CBF02200120704700EB81010B +:107D2000CB68196A0B6813604B685360704700009A +:107D300000EB810330B5DD68AA691368D36019B917 +:107D4000402B84BF402313606B8A1468D0F89020C6 +:107D50001C4402EB4110013C09B2B4FBF3F4634351 +:107D6000033323F0030343EAC44343F0C043C0F8A2 +:107D7000103B2B6803F00303012B0ED1D2F8083817 +:107D800002EB411013F4807FD0F8003B14BF43F0A6 +:107D9000805343F00053C0F8003B02EB4112D2F88D +:107DA000003B43F00443C2F8003B30BD2DE9F041F5 +:107DB000D0F8906005460C4606EB4113D3F8087BDB +:107DC0003A07C3F8087B08D5D6F814381B0704D542 +:107DD00000EB8103DB685B689847FA071FD5D6F88C +:107DE0001438DB071BD505EB8403D968CCB98B6944 +:107DF000488A5A68B2FBF0F600FB16228AB9186866 +:107E0000DA6890420DD2121AC3E90024302383F3BA +:107E1000118821462846FFF78BFF84F31188BDE8BF +:107E2000F081012303FA04F26B8923EA02036B81D8 +:107E3000CB68002BF3D021462846BDE8F041184717 +:107E400000EB81034A0170B5DD68D0F890306C69B1 +:107E50002668E66056BB1A444FF40020C2F81009A9 +:107E60002A6802F00302012A0AB20ED1D3F80808E8 +:107E700003EB421410F4807FD4F8000914BF40F0E3 +:107E8000805040F00050C4F8000903EB4212D2F8D1 +:107E9000000940F00440C2F800090122D3F8340878 +:107EA00002FA01F10143C3F8341870BD19B9402E2C +:107EB00084BF4020206020681A442E8A8419013C27 +:107EC000B4FBF6F440EAC44040F00050C6E70000BE +:107ED0002DE9F843D0F8906005460C464F0106EBBB +:107EE0004113D3F8088918F0010FC3F808891CD092 +:107EF000D6F81038DB0718D500EB8103D3F80CC097 +:107F0000DCF81430D3F800E0DA68964530D2A2EB02 +:107F10000E024FF000091A60C3F80490302383F377 +:107F20001188FFF78DFF89F3118818F0800F1DD09D +:107F3000D6F834380126A640334217D005EB840327 +:107F40000134D5F89050D3F80CC0E4B22F44DCF8DB +:107F5000142005EB0434D2F800E05168714514D3C5 +:107F6000D5F8343823EA0606C5F83468BDE8F88346 +:107F7000012303FA01F2038923EA02030381DCF8F7 +:107F80000830002BD1D09847CFE7AEEB0103BCF807 +:107F90001000834228BF0346D7F8180980B2B3EB1C +:107FA000800FE3D89068A0F1040959F8048FC4F851 +:107FB0000080A0EB09089844B8F1040FF5D81844E4 +:107FC0000B4490605360C8E72DE9F84FD0F890500B +:107FD00004466E69AB691E4016F480586E6103D08A +:107FE000BDE8F84FFDF7E8BF002E12DAD5F8003EE5 +:107FF0009B0705D0D5F8003E23F00303C5F8003EEB +:10800000D5F80438204623F00103C5F80438FEF7FC +:1080100001F8370505D52046FFF772FC2046FDF72D +:10802000E7FFB0040CD5D5F8083813F0060FEB685D +:1080300023F470530CBF43F4105343F4A053EB608C +:1080400031071BD56368DB681BB9AB6923F00803F4 +:10805000AB612378052B0CD1D5F8003E9A0705D0EB +:10806000D5F8003E23F00303C5F8003E2046FDF797 +:10807000D1FF6368DB680BB120469847F30200F13B +:10808000BA80B70226D5D4F8909000274FF0010AA5 +:1080900009EB4712D2F8003B03F44023B3F5802FDD +:1080A00011D1D2F8003B002B0DDA62890AFA07F3EE +:1080B00022EA0303638104EB8703DB68DB6813B107 +:1080C0003946204698470137D4F89430FFB29B6870 +:1080D0009F42DDD9F00619D5D4F89000026AC2F3A8 +:1080E0000A1702F00F0302F4F012B2F5802F00F02D +:1080F000CA80B2F5402F09D104EB8303002200F5BA +:108100008050DB681B6A974240F0B0803003D5F89E +:10811000185835D5E90303D500212046FFF746FE60 +:10812000AA0303D501212046FFF740FE6B0303D5C8 +:1081300002212046FFF73AFE2F0303D503212046F4 +:10814000FFF734FEE80203D504212046FFF72EFE98 +:10815000A90203D505212046FFF728FE6A0203D5B0 +:1081600006212046FFF722FE2B0203D507212046D9 +:10817000FFF71CFEEF0103D508212046FFF716FE8E +:10818000700340F1A780E90703D500212046FFF7DF +:108190009FFEAA0703D501212046FFF799FE6B0732 +:1081A00003D502212046FFF793FE2F0703D50321B5 +:1081B0002046FFF78DFEEE0603D504212046FFF78B +:1081C00087FEA80603D505212046FFF781FE690634 +:1081D00003D506212046FFF77BFE2A0603D507219B +:1081E0002046FFF775FEEB0574D520460821BDE853 +:1081F000F84FFFF76DBED4F890904FF0000B4FF0A2 +:10820000010AD4F894305FFA8BF79B689F423FF6DF +:1082100038AF09EB4713D3F8002902F44022B2F536 +:10822000802F20D1D3F80029002A1CDAD3F80029A6 +:1082300042F09042C3F80029D3F80029002AFBDB62 +:108240003946D4F89000FFF787FB22890AFA07F332 +:1082500022EA0303238104EB8703DB689B6813B1E5 +:108260003946204698470BF1010BCAE7910701D127 +:10827000D0F80080072A02F101029CBF03F8018BAD +:108280004FEA18283FE704EB830300F58050DA68D3 +:10829000D2F818C0DCF80820DCE9001CA1EB0C0CBB +:1082A00000218F4208D1DB689B699A683A449A6042 +:1082B0005A683A445A6029E711F0030F01D1D0F807 +:1082C00000808C4501F1010184BF02F8018B4FEA67 +:1082D0001828E6E7BDE8F88F08B50348FFF774FEF5 +:1082E000BDE80840FFF744BA5C5C002008B50348CD +:1082F000FFF76AFEBDE80840FFF73ABAF85C0020D5 +:10830000D0F8903003EB4111D1F8003B43F4001357 +:10831000C1F8003B70470000D0F8903003EB4111EA +:10832000D1F8003943F40013C1F800397047000058 +:10833000D0F8903003EB4111D1F8003B23F4001347 +:10834000C1F8003B70470000D0F8903003EB4111BA +:10835000D1F8003923F40013C1F800397047000048 +:10836000064BD3F8DC200243C3F8DC20D3F8042109 +:108370001043C3F80401D3F8043170470044025895 +:1083800008B53C4B4FF0FF31D3F8802062F000423B +:10839000C3F88020D3F8802002F00042C3F8802088 +:1083A000D3F88020D3F88420C3F88410D3F8842035 +:1083B0000022C3F88420D3F88400D86F40F0FF4037 +:1083C00040F4FF0040F4DF4040F07F00D867D86FF2 +:1083D00020F0FF4020F4FF0020F4DF4020F07F0079 +:1083E000D867D86FD3F888006FEA40506FEA5050D2 +:1083F000C3F88800D3F88800C0F30A00C3F88800E7 +:10840000D3F88800D3F89000C3F89010D3F8900008 +:10841000C3F89020D3F89000D3F89400C3F89410D8 +:10842000D3F89400C3F89420D3F89400D3F89800BC +:10843000C3F89810D3F89800C3F89820D3F89800A0 +:10844000D3F88C00C3F88C10D3F88C00C3F88C20C0 +:10845000D3F88C00D3F89C00C3F89C10D3F89C1080 +:10846000C3F89C20D3F89C30FCF788FABDE808409C +:1084700000F0E8B80044025808B50122534BC3F895 +:108480000821534BD3F8F42042F00202C3F8F42041 +:10849000D3F81C2142F00202C3F81C210222D3F8B7 +:1084A0001C314C4BDA605A689104FCD54A4A1A6078 +:1084B00001229A60494ADA6000221A614FF4404270 +:1084C0009A61444B9A699204FCD51A6842F480720E +:1084D0001A603F4B1A6F12F4407F04D04FF4803281 +:1084E0001A6700221A671A6842F001021A60384BB4 +:1084F0001A685007FCD500221A611A6912F0380276 +:10850000FBD1012119604FF0804159605A67344A0C +:10851000DA62344A1A611A6842F480321A602C4BCB +:108520001A689103FCD51A6842F480521A601A68DE +:108530009204FCD52C4A2D499A6200225A63196391 +:1085400001F57C01DA6301F2E71199635A64284A64 +:108550001A64284ADA621A6842F0A8521A601C4B60 +:108560001A6802F02852B2F1285FF9D148229A61C4 +:108570004FF48862DA6140221A621F4ADA641F4AA5 +:108580001A651F4A5A651F4A9A6532231E4A1360AC +:10859000136803F00F03022BFAD10D4A136943F05D +:1085A00003031361136903F03803182BFAD14FF05A +:1085B0000050FFF7D5FE4FF08040FFF7D1FE4FF09F +:1085C0000040BDE80840FFF7CBBE00BF008000516F +:1085D000004402580048025800C000F002000001A8 +:1085E0000000FF01008890081210200063020901BA +:1085F000470E0508DD0BBF01200000200000011020 +:108600000910E000000101100020005208B50348E5 +:10861000FCF786FDBDE80840FFF7AAB8C036002089 +:1086200008B50348FCF716FEBDE80840FFF7A0B800 +:108630001839002008B500F08DFCBDE80840FFF7B0 +:1086400097B8000008B509217A20FCF789FE0921B6 +:108650003120FCF785FE07213220FCF781FE0C213A +:108660004720BDE80840FCF77BBE000008B5FFF7D7 +:1086700087FE00F043FCFDF753FAFDF7F1F9FDF733 +:1086800029FCFDF7FBFAFEF7CDFABDE8084000F043 +:1086900029BA000030B50433039C0172002104FBA9 +:1086A0000325C160C0E90653049B0363059BC0E931 +:1086B0000000C0E90422C0E90842C0E90A1143638E +:1086C00030BD00000022416AC260C0E90411C0E967 +:1086D0000A226FF00101FEF71BB80000D0E9043256 +:1086E000934201D1C2680AB9181D70470020704733 +:1086F000036919600021C2680132C260C269134473 +:1087000082699342036124BF436A0361FDF7F4BFAA +:1087100038B504460D46E3683BB162690020131D7D +:108720001268A3621344E36207E0237A33B929464F +:108730002046FDF7D1FF0028EDDA38BD6FF00100CB +:10874000FBE70000C368C269013BC360436913448F +:1087500082699342436124BF436A43610023836279 +:10876000036B03B11847704770B53023044683F399 +:108770001188866A3EB9FFF7CBFF054618B186F32C +:108780001188284670BDA36AE26A13F8015B934220 +:10879000A36202D32046FFF7D5FF002383F311889D +:1087A000EFE700002DE9F84F04460E4617469846BD +:1087B0004FF0300989F311880025AA46D4F828B073 +:1087C000BBF1000F09D141462046FFF7A1FF20B1C0 +:1087D0008BF311882846BDE8F88FD4E90A12A7EB7D +:1087E000050B521A934528BF9346BBF1400F1BD986 +:1087F000334601F1400251F8040B914243F8040B57 +:10880000F9D1A36A403640354033A362D4E90A2344 +:108810009A4202D32046FFF795FF8AF31188BD42A2 +:10882000D8D289F31188C9E730465A46F9F7FEFADB +:10883000A36A5E445D445B44A362E7E710B5029C13 +:108840000433017203FB0421C460C0E90613002352 +:10885000C0E90A33039B0363049BC0E90000C0E93D +:108860000422C0E90842436310BD0000026A6FF0B1 +:108870000101C260426AC0E904220022C0E90A2262 +:10888000FDF746BFD0E904239A4201D1C26822B95C +:10889000184650F8043B0B60704700231846FAE76F +:1088A000C3680021C2690133C3604369134482690C +:1088B0009342436124BF436A4361FDF71DBF00003B +:1088C00038B504460D46E3683BB1236900201A1D04 +:1088D000A262E2691344E36207E0237A33B92946CE +:1088E0002046FDF7F9FE0028EDDA38BD6FF00100F3 +:1088F000FBE7000003691960C268013AC260C269FF +:10890000134482699342036124BF436A03610023D5 +:108910008362036B03B118477047000070B53023C2 +:108920000D460446114683F31188866A2EB9FFF777 +:10893000C7FF10B186F3118870BDA36A1D70A36ACA +:10894000E26A01339342A36204D3E1692046043909 +:10895000FFF7D0FF002080F31188EDE72DE9F84FF5 +:1089600004460D46904699464FF0300A8AF3118826 +:108970000026B346A76A4FB949462046FFF7A0FF35 +:1089800020B187F311883046BDE8F88FD4E90A0793 +:108990003A1AA8EB0607974228BF1746402F1BD963 +:1089A00005F1400355F8042B9D4240F8042BF9D102 +:1089B000A36A40364033A362D4E90A239A4204D31F +:1089C000E16920460439FFF795FF8BF3118846458E +:1089D000D9D28AF31188CDE729463A46F9F726FA23 +:1089E000A36A3D443E443B44A362E5E7D0E9042347 +:1089F0009A4217D1C3689BB1836A8BB1043B9B1A1F +:108A00000ED01360C368013BC360C3691A44836915 +:108A10009A42026124BF436A0361002383620123F7 +:108A2000184670470023FBE701F01F03F0B502F082 +:108A30001F0456095A1C0123B6EB511F50F826504B +:108A400003FA02F34FEA511703F1FF333DBF50F829 +:108A50002720C4F12000134003EA05003BBF03FABE +:108A600000F225FA04F0E0401043F0BD70B57E221C +:108A70007F210546FFF7D8FF18B1012819D0002043 +:108A800070BD3E2249212846FFF7CEFF2F22044623 +:108A900031212846FFF7C8FF06460134502202362E +:108AA00053212846B440FFF7BFFF093804FA00F00D +:108AB000E6E7302245212846FFF7B6FF0130800265 +:108AC000DEE7000090F8D63090F8D7201B0403EBC7 +:108AD000026390F8D42090F8D500134403EB0020F3 +:108AE0007047000000F00CBA014B586A704700BF95 +:108AF000000C0040034B002258631A610222DA6026 +:108B0000704700BF000C0040014B0022DA60704744 +:108B1000000C0040014B5863704700BF000C004040 +:108B2000024B034A1A60034A5A607047AC5D00204A +:108B3000D85D002000000220074B494210B55C6858 +:108B4000201A08401968821A8A4203D3A24201D827 +:108B50005A6010BD0020FCE7AC5D002008B5302352 +:108B600083F31188FFF7E8FF002383F3118808BD22 +:108B70000448054B03600023C0E901330C3000F0CA +:108B800017B900BFB45D00205D8B0008CB1D083A0B +:108B900023F00703591A521A10B4D2080024C0E96E +:108BA000004384600C301C605A605DF8044B00F098 +:108BB000FFB800002DE9F74F364FCD1D884600283D +:108BC00018BF0746082A4FEAD50538BF082207F123 +:108BD0000C003C1D9146019000F02CF9019809F120 +:108BE0000701C9F1000E2246246864B900F02CF98F +:108BF0003B68CBB308224946E8009847044698B33F +:108C000040E9027830E004EB010CD4F804A00CEA4F +:108C10000E0C0AF10106ACF1080304EBC6069E42F5 +:108C2000E1D9A6EB0C0CB5EBEC0F4FEAEC0BDAD864 +:108C30009C421DD204F10802AB45A3EB02024FEAAD +:108C4000E202626009D9691CED4303EBC1025D4495 +:108C50005560256843F8315022601C46C3F80480F3 +:108C600044F8087B00F0F0F8204603B0BDE8F08F30 +:108C7000AA45216802D111602346EEE7013504EBD5 +:108C8000C50344F8351003F10801761AF6105E604A +:108C90001360F1E7B45D002073B50446A0F1080548 +:108CA00050F8080C54F8043C061D0C3007330190B2 +:108CB000DB0844F8043C00F0BDF8334601989E42BE +:108CC0001A6801D0AB4228D20AB1954225D244F8A5 +:108CD000082C54F8042C1D60013254F8081C05EBD4 +:108CE000C206B14206D14E68324444F8042C0A68E8 +:108CF00044F8082C5E68711C03EBC1018D4207D15A +:108D000054F8042C013232445A6054F8082C1A608A +:108D100002B0BDE8704000F097B81346CFE70000FE +:108D2000FEE7000070B51E4B0025044686B0586073 +:108D30000E4605638163FEF7F3FC04F12803A5608A +:108D40006563C4E90A3304F11003C4E904334FF046 +:108D5000FF33C4E90044C4E90635FFF7C5FE2B46DE +:108D6000024604F13C012046C4E9082380230D4A51 +:108D70006567FDF72BFC7368E0600B4A0362012313 +:108D8000009280F824306846F26801923269CDE999 +:108D90000223064BCDE90435FDF74CFC06B070BD4F +:108DA00030400020509A0008489A0008218D0008A1 +:108DB0000023C0E9000083600361704770B51C4B5D +:108DC00005468468DE685CB3B44213D1036901339D +:108DD000036170BDA36094F8243083B1062B15D1D4 +:108DE000A06A2146D4E9003213605A60FDF74AFBBD +:108DF000A36A9C68B368A2689A42EBD306E0D4E900 +:108E00000032204613605A60FDF74CFB284631467D +:108E1000FDF738FBB5620620BDE87040FDF744BBA6 +:108E20000369866001330361336BC3603063D0E74D +:108E3000883D002008B5302383F31188FFF7BEFF7B +:108E4000002383F3118808BD194BD96883688B42CE +:108E500010B520D1302383F311880269013A0261F1 +:108E6000B2B90468C368A0420B631ED04A6B9BB9B9 +:108E700001238A60036103681A68026050601A6BFC +:108E80008360C26018631846FDF70CFBFDF76CFBAE +:108E9000002383F3118810BD1C68A34203D0A4688B +:108EA000A24238BF2246DB68E1E78260F0E700BFFC +:108EB000883D0020024A536B18435063704700BF3F +:108EC000883D002038B5EFF311859DB9EFF3058497 +:108ED000C4F30804302334B183F31188FDF750FD47 +:108EE00085F3118838BD83F31188FDF749FD84F3BC +:108EF000118838BDBDE83840FDF742BD704700001D +:108F0000114BD3F8E82042F00802C3F8E820D3F868 +:108F1000102142F00802C3F810210C4AD3F8103196 +:108F2000D36B43F00803D363C722094B9A624FF017 +:108F3000FF32DA6200229A615A63DA605A600122D3 +:108F40005A611A60704700BF004402580010005C6C +:108F5000000C0040094A08B51169D3680B40D9B22A +:108F60009B076FEA0101116107D5302383F3118854 +:108F7000FDF720FB002383F3118808BD000C00409F +:108F800010B501390244904201D1002005E0037878 +:108F900011F8014FA34201D0181B10BD0130F2E7B8 +:108FA000884210B501EB020402D98442234607D857 +:108FB000431EA14208D011F8012B03F8012FF8E756 +:108FC000024401468A4200D110BD13F8014D02F857 +:108FD000014DF7E7C9B2034610F8012B1AB18A42D6 +:108FE000F9D118467047002918BF0023F9E700009F +:108FF000034611F8012B03F8012B002AF9D1704721 +:1090000010B50139034632B111F8014F03F8014B95 +:10901000013A002CF7D11A440021934200D110BD2F +:1090200003F8011BF9E70000613036643765353815 +:109030003232303734656131000000001210000018 +:1090400012110000121300004D4435002D2D0A00AE +:109050002F6172647570696C6F742E6162696E0045 +:109060002F6172647570696C6F742D7665726966B4 +:10907000792E6162696E002F6172647570696C6F20 +:10908000742D666C6173682E6162696E002F617267 +:10909000647570696C6F742D666C61736865642E9D +:1090A0006162696E00000000000000000000000026 +:1090B000150F0008B10F000861110008E90F000842 +:1090C000A90F00080000000000000000110F0008B8 +:1090D000BD0F0008991100080D0F0008190F0008B6 +:1090E00053544D333248373F3F3F0053544D333292 +:1090F000483733782F3732780053544D333248375E +:1091000034332F3735332F373530000001105A00F4 +:109110000310590001205800032056002F000000C2 +:109120005375636365737366756C6C79206D6F75C9 +:109130006E746564205344436172642028736C6FBD +:1091400077646F776E3D2575290A0000EB769045B0 +:1091500058464154202020004641543332202020DC +:10916000000000002A3A3C3E7C223F7F002B2C3B33 +:109170003D5B5D00435545414141414345454549BE +:10918000494941414592924F4F4F5555594F554F7F +:109190009C4F9E9F41494F55A5A5A6A7A8A9AAAB3C +:1091A000ACADAEAFB0B1B2B3B4414141B8B9BABBE6 +:1091B000BCBDBEBFC0C1C2C3C4C54141C8C9CACB82 +:1091C000CCCDCECFD1D145454549494949D9DADB46 +:1091D000DCDD49DF4FE14F4F4F4FE6E8E85555558D +:1091E0005959EEEFF0F1F2F3F4F5F6F7F8F9FAFB6E +:1091F000FCFDFEFF01030507090E10121416181CD2 +:109200001E00000061001A03E0001703F8000703C6 +:10921000FF0001007801000130013201060139012F +:1092200010014A012E017901060180014D0043021F +:109230008101820182018401840186018701870105 +:1092400089018A018B018B018D018E018F019001B3 +:109250009101910193019401F60196019701980102 +:1092600098013D029B019C019D0120029F01A001EC +:10927000A001A201A201A401A401A601A701A701C6 +:10928000A901AA01AB01AC01AC01AE01AF01AF0174 +:10929000B101B201B301B301B501B501B701B80124 +:1092A000B801BA01BB01BC01BC01BE01F701C0019C +:1092B000C101C201C301C401C501C401C701C80184 +:1092C000C701CA01CB01CA01CD011001DD010100B6 +:1092D0008E01DE011201F3010300F101F401F4013A +:1092E000F8012801220212013A020900652C3B0212 +:1092F0003B023D02662C3F0240024102410246020F +:109300000A015302400081018601550289018A0148 +:1093100058028F015A0290015C025D025E025F02F8 +:1093200093016102620294016402650266026702AF +:10933000970196016A02622C6C026D026E029C011A +:10934000700271029D01730274029F01760277021E +:10935000780279027A027B027C02642C7E027F0210 +:10936000A60181028202A901840285028602870287 +:10937000AE014402B101B20145028D028E028F029C +:1093800090029102B7017B030300FD03FE03FF037C +:10939000AC0304008603880389038A03B103110325 +:1093A000C2030200A303A303C4030803CC03030006 +:1093B0008C038E038F03D8031801F2030A00F9030C +:1093C000F303F403F503F603F703F703F903FA03D2 +:1093D000FA033004200350041007600422018A04B9 +:1093E0003601C1040E01CF040100C004D0044401C1 +:1093F00061052604000000007D1D0100632C001E95 +:109400009601A01E5A01001F0806101F0606201F05 +:109410000806301F0806401F0606511F0700591F87 +:10942000521F5B1F541F5D1F561F5F1F601F0806E2 +:10943000701F0E00BA1FBB1FC81FC91FCA1FCB1F3A +:10944000DA1FDB1FF81FF91FEA1FEB1FFA1FFB1FB4 +:10945000801F0806901F0806A01F0806B01F040002 +:10946000B81FB91FB21FBC1FCC1F0100C31FD01FE4 +:109470000206E01F0206E51F0100EC1FF31F0100BA +:10948000FC1F4E21010032217021100284210100B5 +:109490008321D0241A05302C2F04602C0201672C64 +:1094A0000601752C0201802C6401002D260841FF65 +:1094B0001A030000C700FC00E900E200E400E0003D +:1094C000E500E700EA00EB00E800EF00EE00EC004A +:1094D000C400C500C900E600C600F400F600F200B2 +:1094E000FB00F900FF00D600DC00F800A300D80064 +:1094F000D7009201E100ED00F300FA00F100D10085 +:10950000AA00BA00BF00AE00AC00BD00BC00A100C4 +:10951000AB00BB0091259225932502252425C1008F +:10952000C200C000A9006325512557255D25A20072 +:10953000A5001025142534252C251C2500253C25A7 +:10954000E300C3005A25542569256625602550256A +:109550006C25A400F000D000CA00CB00C800310187 +:10956000CD00CE00CF0018250C2588258425A60027 +:10957000CC008025D300DF00D400D200F500D50058 +:10958000B500FE00DE00DA00DB00D900FD00DD00E2 +:10959000AF00B400AD00B1001720BE00B600A700B8 +:1095A000F700B800B000A800B700B900B300B200DF +:1095B000A025A00010000240080002400008024060 +:1095C00000000B002800024008000240040802408E +:1095D00006010C004000024008000240080802405A +:1095E00010020D0058000240080002400C08024022 +:1095F00016030E00700002400C00024010080240EA +:1096000000040F00880002400C00024014080240D1 +:1096100006051000A00002400C000240180802409D +:1096200010061100B80002400C0002401C08024065 +:1096300016072F00100402400804024020080240D0 +:1096400000083800280402400804024024080240B0 +:10965000060939004004024008040240280802407C +:10966000100A3A0058040240080402402C08024044 +:10967000160B3B00700402400C040240300802400C +:10968000000C3C00880402400C04024034080240F4 +:10969000060D4400A00402400C04024038080240B9 +:1096A000100E4500B80402400C0402403C08024081 +:1096B000160F4600010000000000000000960000A8 +:1096C000000000000000000000000000000000009A +:1096D00000000000000000001D6C0008216C000864 +:1096E000F55600082D5A000889560008B1560008A2 +:1096F000D95600087156000800000000E15A000821 +:10970000CD5A0008095B0008F55A0008015B000803 +:10971000ED5A0008D95A0008C55A0008155B000820 +:1097200000000000F95B0008E55B0008215C000810 +:109730000D5C0008195C0008055C0008F15B00087E +:10974000DD5B00082D5C0008000000000100000047 +:109750000000000063300000549700080000000083 +:1097600000000000003E0020304000200001806A20 +:1097700000000000AAAAAAAA00010064FFFF0000DE +:109780000000000000A00A0010000001000000001E +:10979000AAAAAAAA00000001FBFF00000000000026 +:1097A0000000000000A0AA0200000000AAAAAAAAC5 +:1097B00000505500FFFF000000000077CCCC0C00EB +:1097C0002000000000000000AAAAAAAA10000000C1 +:1097D000FFFF0000000C000000000000000000007F +:1097E00000000000AAAAAAAA00000000FFFF0000D3 +:1097F0000000000000000000000000000000000069 +:10980000AAAAAAAA00000000FFFF000000000000B2 +:10981000000000000000000000000000AAAAAAAAA0 +:1098200000000000FFFF000000000000000000003A +:109830000000000000000000AAAAAAAA0000000080 +:10984000FFFF00000000000000000000000000001A +:1098500000000000AAAAAAAA00000000FFFF000062 +:1098600000000000000000000000000000000000F8 +:10987000AAAAAAAA00000000FFFF00000000000042 +:10988000000000000000000000000000AAAAAAAA30 +:1098900000000000FFFF00000000000000000000CA +:1098A0004172647550696C6F740025424F41524497 +:1098B000252D424C002553455249414C25000000BE +:1098C0000200000000000000195E0008895E000828 +:1098D00040004000E0580020F05800200200000046 +:1098E000000000000300000000000000D15E00083E +:1098F00000000000100000000059002000000000DF +:1099000001000000000000005C5C0020010102007A +:10991000196D0008296C0008C56C0008A96C0008C6 +:10992000430000002899000809024300020100C01A +:1099300032090400000102020100052400100105A3 +:109940002401000104240202052406000107058207 +:10995000030800FF09040100020A000000070501D6 +:1099600002400000070581024000000012000000D4 +:1099700074990008120110010200004009124157B9 +:1099800000020102030100000403090425424F41C3 +:109990005244250048574837003031323334353689 +:1099A000373839414243444546000000000000205A +:1099B000000002000200000000000030000004006F +:1099C000080000000000002400000800040000005F +:1099D0000004000000FC0000020000000000043051 +:1099E00000800000080000000000003800000100B6 +:1099F000010000001F1C1F1E1F1E1F1F1E1F1E1FF9 +:109A00001F1D1F1E1F1E1F1F1E1F1E1F00000000E8 +:109A10002D600008E56200089163000840004000E6 +:109A2000945D0020945D002001000000A45D0020F2 +:109A3000800000004001000008000000000100005C +:109A4000001000000800000069646C650000000060 +:109A50006D61696E002C0438040438080C10141C65 +:109A600020242526000000000000640401000400FA +:109A700000000000000C001028303400EC69FF7F6B +:109A800001000000FFFFFFFFC7040000000000000E +:109A900000001A0000000000FF0000003840002015 +:109AA0001839002000000000E09000088304000046 +:109AB000EB90000850040000F9900008010000003D +:109AC0000000000000960000000008009600000062 +:109AD0000008000004000000889900080000000051 +:109AE0000000000000000000000000000000000076 +:049AF0000000000072 +:00000001FF diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 54c03026f9700..88f0ad5122efc 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -49,6 +49,8 @@ def config_option(self): Feature('AHRS', 'AHRS_EXT_VECTORNAV', 'AP_EXTERNAL_AHRS_VECTORNAV_ENABLED', 'Enable VectorNav external AHRS', 0, "AHRS_EXT"), # noqa Feature('AHRS', 'InertialLabs', 'AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED', 'Enable InertialLabs external AHRS', 0, "AHRS_EXT"), # noqa Feature('AHRS', 'SBG', 'AP_EXTERNAL_AHRS_SBG_ENABLED', 'Enable SBG external AHRS', 0, "AHRS_EXT"), + Feature('AHRS', 'AHRS_EXT_SENSAITION', 'AP_EXTERNAL_AHRS_SENSAITION_ENABLED', + 'Enable SensAItion external AHRS', 0, "AHRS_EXT"), Feature('AHRS', 'VISUALODOM', 'HAL_VISUALODOM_ENABLED', 'Enable Visual Odometry', 0, None), Feature('AHRS', 'EKF3_EXTNAV', 'EK3_FEATURE_EXTERNAL_NAV', 'Enable External navigation for EKF3', 0, 'EKF3'), Feature('AHRS', 'EKF3_WINDEST', 'EK3_FEATURE_DRAG_FUSION', 'Enable Wind estimation for EKF3', 0, 'EKF3'), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index 17e59812b80ad..ff2daac996f80 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -101,7 +101,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_BATTERY_{type}_ENABLED', r'AP_BattMonitor_(?P.*)::init\b',), ('AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED', r'AP_BattMonitor_Backend::update_esc_telem_outbound\b',), - ('AP_BATTERY_WATT_MAX_ENABLED', 'Plane::throttle_watt_limiter',), + ('AP_BATTERY_WATT_MAX_ENABLED', r'Plane::throttle_watt_limiter|AP_MotorsUGV::get_power_limit_max_throttle',), ('HAL_MOUNT_ENABLED', 'AP_Mount::AP_Mount',), ('HAL_MOUNT_{type}_ENABLED', r'AP_Mount_(?P.*)::update\b',), diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index b2cccb99119e6..d0668767bd751 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -8,12 +8,12 @@ extern const AP_HAL::HAL& hal; #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) // default gains for Plane - # define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.2f // Soft + # define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.15f // Medium #define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN 5.0 // Min lean angle so that vehicle can maintain limited control #define AC_ATTITUDE_CONTROL_AFTER_RATE_CONTROL 0 #else // default gains for Copter and Sub - # define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.15f // Medium + # define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.10f // Crisp #define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN 10.0 // Min lean angle so that vehicle can maintain limited control #define AC_ATTITUDE_CONTROL_AFTER_RATE_CONTROL 1 #endif diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 4557cb2a632af..df6db48b6514e 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -433,17 +433,21 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c AC_Fence *fence = AP::fence(); if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && fence) { // calculate distance from vehicle to safe altitude - float veh_alt_m; - _ahrs.get_relative_position_D_home(veh_alt_m); if ((fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) > 0) { - // fence.get_safe_alt_max_m() is UP, veh_alt_m is DOWN: - min_alt_diff_m = -(fence->get_safe_alt_min_m() + veh_alt_m); - limit_min_alt = true; + float veh_alt_m; + if (fence->get_alt_in_alt_min_frame_m(veh_alt_m)) { + // fence.get_safe_alt_min_m() is UP, veh_alt_m is UP: + min_alt_diff_m = veh_alt_m - fence->get_safe_alt_min_m(); + limit_min_alt = true; + } } if ((fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) { - // fence.get_safe_alt_max_m() is UP, veh_alt_m is DOWN: - max_alt_diff_m = fence->get_safe_alt_max_m() + veh_alt_m; - limit_max_alt = true; + float veh_alt_m; + if (fence->get_alt_in_alt_max_frame_m(veh_alt_m)) { + // fence.get_safe_alt_max_m() is UP, veh_alt_m is UP: + max_alt_diff_m = fence->get_safe_alt_max_m() - veh_alt_m; + limit_max_alt = true; + } } } #endif diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.cpp b/libraries/AC_Avoidance/AP_OABendyRuler.cpp index efba4253be69e..be17102d5f8cb 100644 --- a/libraries/AC_Avoidance/AP_OABendyRuler.cpp +++ b/libraries/AC_Avoidance/AP_OABendyRuler.cpp @@ -501,18 +501,18 @@ bool AP_OABendyRuler::calc_margin_from_alt_fence(const Location &start, const Lo return false; } - int32_t alt_above_home_cm_start, alt_above_home_cm_end; - if (!start.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_above_home_cm_start)) { + int32_t alt_above_frame_cm_start, alt_above_frame_cm_end; + if (!start.get_alt_cm(fence->get_alt_max_frame(), alt_above_frame_cm_start)) { return false; } - if (!end.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_above_home_cm_end )) { + if (!end.get_alt_cm(fence->get_alt_max_frame(), alt_above_frame_cm_end )) { return false; } // safe max alt = fence alt - fence margin const float max_fence_alt = fence->get_safe_alt_max_m(); - const float margin_start = max_fence_alt - alt_above_home_cm_start * 0.01f; - const float margin_end = max_fence_alt - alt_above_home_cm_end * 0.01f; + const float margin_start = max_fence_alt - alt_above_frame_cm_start * 0.01f; + const float margin_end = max_fence_alt - alt_above_frame_cm_end * 0.01f; // margin is minimum distance to fence from either start or end location margin = MIN(margin_start,margin_end); diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 0915a502d9d02..156544c199ab3 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -14,6 +14,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -75,7 +76,7 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { // @Param{Copter, Plane, Sub}: ALT_MAX // @DisplayName: Fence Maximum Altitude - // @Description: Maximum altitude allowed before geofence triggers + // @Description: Maximum altitude allowed before geofence triggers. See FENCE_ALT_MAX_TP for reference frame. // @Units: m // @Range: 10 1000 // @Increment: 1 @@ -107,7 +108,7 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { // @Param{Copter, Plane, Sub}: ALT_MIN // @DisplayName: Fence Minimum Altitude - // @Description: Minimum altitude allowed before geofence triggers + // @Description: Minimum altitude allowed before geofence triggers. See FENCE_ALT_MIN_TP for reference frame. // @Units: m // @Range: -100 100 // @Increment: 1 @@ -164,6 +165,20 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { // @User: Standard AP_GROUPINFO("MARGIN_XY", 13, AC_Fence, _margin_ne_m, 0), + // @Param: ALT_MAX_TP + // @DisplayName: Altitude max frame type + // @Description: The altitude reference frame for the maximum altitude fence. Can be AMSL, home-relative, origin relative or AGL. + // @User: Advanced + // @Values: 0:Above sea level, 1:Above Home, 2: Above Origin, 3: Above Terrain + AP_GROUPINFO_FRAME("ALT_MAX_TP", 14, AC_Fence, _alt_max_type, float(Location::AltFrame::ABOVE_HOME), AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_SUB | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_PLANE), + + // @Param: ALT_MIN_TP + // @DisplayName: Altitude min frame type + // @Description: The altitude reference frame for the minimum altitude fence. Can be AMSL, home-relative, origin relative or AGL. + // @User: Advanced + // @Values: 0:Above sea level, 1:Above Home, 2: Above Origin, 3: Above Terrain + AP_GROUPINFO_FRAME("ALT_MIN_TP", 15, AC_Fence, _alt_min_type, float(Location::AltFrame::ABOVE_HOME), AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_SUB | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_PLANE), + AP_GROUPEND }; @@ -298,6 +313,11 @@ uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_enabl if (!value) { clear_breach(fences_to_change); + if (update_auto_enable) { + // explicit user disable resets the manual-recovery suppression + // window so that a fresh fence enable will check immediately + _manual_recovery_start_ms = 0; + } } return fences_to_change; @@ -455,6 +475,10 @@ bool AC_Fence::pre_arm_check_alt(char *failure_msg, const uint8_t failure_msg_le return true; } +bool AC_Fence::terrain_database_required() const +{ + return _alt_min_type == Location::AltFrame::ABOVE_TERRAIN || _alt_max_type == Location::AltFrame::ABOVE_TERRAIN; +} /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const @@ -550,6 +574,54 @@ bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) c return true; } +/* + get our altitude in the supplied frame + return false if not available + */ +bool AC_Fence::get_alt_in_frame_m(Location::AltFrame alt_frame, float &alt) const +{ + const auto &ahrs = AP::ahrs(); + + if (alt_frame == Location::AltFrame::ABOVE_ORIGIN) { + // try and get it relative origin + if (!ahrs.get_relative_position_D_origin_float(alt)) { + // treat as a breach if origin not known + return false; + } + alt = -alt; + return true; + } + + if (alt_frame == Location::AltFrame::ABOVE_HOME) { + // try and get it relative origin + ahrs.get_relative_position_D_home(alt); + alt = -alt; + return true; + } + + Location loc; + if (AP::ahrs().get_location(loc)) { + if (loc.get_alt_m(alt_frame, alt)) { + return true; + } + } + + return false; +} + +// update safe alt min - home *must* be set before this function is called with absolute reference frames +void AC_Fence::update_safe_alt_min() +{ + // update safe alt min, failure to get home will have already breached above + if (_alt_min_type == Location::AltFrame::ABSOLUTE) { + float home_alt; + UNUSED_RESULT(AP::ahrs().get_home().get_alt_m(Location::AltFrame::ABSOLUTE, home_alt)); + _safe_relhome_alt_min_m = _alt_min_m - home_alt - _margin_m; + } else { + _safe_relhome_alt_min_m = _alt_min_m - _margin_m; + } +} + /// returns true if we have freshly breached the maximum altitude /// fence; also may set up a fallback fence which, if breached, will /// cause the altitude fence to be freshly breached @@ -561,13 +633,24 @@ bool AC_Fence::check_fence_alt_max() return false; } - float curr_alt_d_m; - AP::ahrs().get_relative_position_D_home(curr_alt_d_m); - const float _curr_alt_u_m = -curr_alt_d_m; // translate Down to Up + float _curr_alt_u_m; + if (!get_alt_in_alt_max_frame_m(_curr_alt_u_m)) { + // if we can't get the alt then it is a breach + return true; + } // record distance above/below breach _alt_max_breach_distance_m = _curr_alt_u_m - _alt_max_m; + // update safe alt max, failure to get home will have already breached above + if (_alt_max_type == Location::AltFrame::ABSOLUTE) { + float home_alt; + UNUSED_RESULT(AP::ahrs().get_home().get_alt_m(Location::AltFrame::ABSOLUTE, home_alt)); + _safe_relhome_alt_max_m = _alt_max_m - home_alt - _margin_m; + } else { + _safe_relhome_alt_max_m = _alt_max_m - _margin_m; + } + // check if we are over the altitude fence if (_curr_alt_u_m >= _alt_max_m) { // check for a new breach or a breach of the backup fence @@ -612,13 +695,18 @@ bool AC_Fence::check_fence_alt_min() return false; } - float curr_alt_d_m; - AP::ahrs().get_relative_position_D_home(curr_alt_d_m); - const float _curr_alt_u_m = -curr_alt_d_m; // translate Down to Up + float _curr_alt_u_m; + if (!get_alt_in_alt_min_frame_m(_curr_alt_u_m)) { + // if we can't get the alt then it is a breach + return true; + } // record distance above/below breach _alt_min_breach_distance_m = _alt_min_m - _curr_alt_u_m; + // update safe alt min, failure to get home will have already breached above + update_safe_alt_min(); + // check if we are under the altitude fence if (_curr_alt_u_m <= _alt_min_m) { @@ -668,9 +756,14 @@ bool AC_Fence::auto_enable_fence_floor() return false; } - float _curr_alt_d_m; - AP::ahrs().get_relative_position_D_home(_curr_alt_d_m); - const float _curr_alt_u_m = -_curr_alt_d_m; // translate Down to Up + float _curr_alt_u_m; + if (!get_alt_in_alt_min_frame_m(_curr_alt_u_m)) { + // if we can't get the alt then don't enable yet + return true; + } + + // update safe alt min, failure to get home will have already breached above + update_safe_alt_min(); // check if we are over the altitude fence if (!floor_enabled() && _curr_alt_u_m >= get_safe_alt_min_m()) { @@ -892,9 +985,9 @@ bool AC_Fence::check_destination_within_fence(const Location& loc) { // Altitude fence check - Fence Ceiling if ((get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX)) { - int32_t alt_above_home_cm; - if (loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_above_home_cm)) { - if ((alt_above_home_cm * 0.01f) > _alt_max_m) { + float alt_m; + if (loc.get_alt_m(_alt_max_type, alt_m)) { + if (alt_m > _alt_max_m) { return false; } } @@ -902,9 +995,9 @@ bool AC_Fence::check_destination_within_fence(const Location& loc) // Altitude fence check - Fence Floor if ((get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN)) { - int32_t alt_above_home_cm; - if (loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_above_home_cm)) { - if ((alt_above_home_cm * 0.01f) < _alt_min_m) { + float alt_m; + if (loc.get_alt_m(_alt_min_type, alt_m)) { + if (alt_m < _alt_min_m) { return false; } } @@ -1065,6 +1158,11 @@ void AC_Fence::manual_recovery_start() return; } + // Not armed so return + if (!hal.util->get_soft_armed()) { + return; + } + // record time pilot began manual recovery _manual_recovery_start_ms = AP_HAL::millis(); @@ -1131,7 +1229,7 @@ uint8_t AC_Fence::present() const { return 0; } uint8_t AC_Fence::get_enabled_fences() const { return 0; } bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const { return true; } - +bool AC_Fence::terrain_database_required() const { return false; } uint8_t AC_Fence::check(bool disable_auto_fences) { return 0; } bool AC_Fence::check_destination_within_fence(const Location& loc) { return true; } float AC_Fence::get_breach_distance(uint8_t fence_type) const { return 0.0; } diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 59a5a3cdabc35..6251e667e4b92 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -113,6 +113,9 @@ class AC_Fence /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const; + // used by AP_Arming for pre-arm checks + bool terrain_database_required() const; + /// /// methods to check we are within the boundaries and recover /// @@ -151,12 +154,30 @@ class AC_Fence /// get_action - getter for user requested action on limit breach Action get_action() const { return _action; } - /// get_safe_alt - returns maximum safe altitude (i.e. alt_max - margin) + /// get_relative_safe_alt_max_m - returns maximum safe altitude in relative frame + float get_relative_safe_alt_max_m() const { return _safe_relhome_alt_max_m ; } + + /// get_relative_safe_alt_min_m - returns the minimum safe altitude in relative frame + float get_relative_safe_alt_min_m() const { return _safe_relhome_alt_min_m ; } + + /// get_safe_alt_max_m - returns maximum safe altitude in alt max frame (i.e. alt_max - margin) float get_safe_alt_max_m() const { return _alt_max_m - _margin_m; } - /// get_safe_alt_min_m - returns the minimum safe altitude (i.e. alt_min + margin) + /// get_safe_alt_min_m - returns the minimum safe altitude in alt min frame (i.e. alt_min + margin) float get_safe_alt_min_m() const { return _alt_min_m + _margin_m; } + /// get_safe_alt_max_m_and_frame - returns maximum safe altitude and its frame via references + void get_safe_alt_max_m_and_frame(float &alt, uint8_t &frame) const { + alt = _alt_max_m - _margin_m; + frame = (uint8_t)_alt_max_type.get(); + } + + /// get_safe_alt_min_m_and_frame - returns minimum safe altitude and its frame via references + void get_safe_alt_min_m_and_frame(float &alt, uint8_t &frame) const { + alt = _alt_min_m + _margin_m; + frame = (uint8_t)_alt_min_type.get(); + } + /// get_radius_m - returns the fence radius in meters float get_radius_m() const { return _circle_radius_m.get(); } @@ -208,6 +229,15 @@ class AC_Fence } #endif + // get altitude in alt max frame + bool get_alt_in_alt_max_frame_m(float &alt) const { return get_alt_in_frame_m(_alt_max_type, alt); } + // get altitude in alt min frame + bool get_alt_in_alt_min_frame_m(float &alt) const { return get_alt_in_frame_m(_alt_min_type, alt); } + // get alt max frame + Location::AltFrame get_alt_max_frame() const { return (Location::AltFrame)_alt_max_type.get(); } + // get alt min frame + Location::AltFrame get_alt_min_frame() const { return (Location::AltFrame)_alt_min_type.get(); } + private: static AC_Fence *_singleton; @@ -223,6 +253,9 @@ class AC_Fence /// check_fence_circle - true if circle fence has been newly breached bool check_fence_circle(); + /// update safe alt min + void update_safe_alt_min(); + /// record_breach - update breach bitmask, time and count void record_breach(uint8_t fence_type); @@ -238,6 +271,9 @@ class AC_Fence /// retrieve the current NED position relative to home bool get_current_position_NED(Vector3f& currpos) const; + // get altitude in the supplied frame + bool get_alt_in_frame_m(Location::AltFrame alt_frame, float &alt) const; + // additional checks for the different fence types: bool pre_arm_check_polygon(char *failure_msg, const uint8_t failure_msg_len) const; bool pre_arm_check_circle(char *failure_msg, const uint8_t failure_msg_len) const; @@ -263,6 +299,8 @@ class AC_Fence AP_Int16 _ret_altitude; // return to this altitude AP_Int16 _options; // options bitmask, see OPTIONS enum AP_Float _notify_freq; // margin notification frequency + AP_Enum _alt_max_type; // altitude max frame type + AP_Enum _alt_min_type; // altitude min frame type // backup fences float _alt_max_backup_m; // backup altitude upper limit in meters used to refire the breach if the vehicle continues to move further away @@ -282,6 +320,8 @@ class AC_Fence // other internal variables float _home_distance_m; // distance from home in meters (provided by main code) float _fence_distance_m; // distance to the nearest fence + float _safe_relhome_alt_max_m ; // calculated safe alt max + float _safe_relhome_alt_min_m ; // calculated safe alt min // breach information uint8_t _breached_fences; // bitmask holding the fence types that were breached (i.e. AC_FENCE_TYPE_ALT_MIN, AC_FENCE_TYPE_CIRCLE) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index f2e1233bc6c56..e38547e4e0446 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -1549,8 +1549,11 @@ void AP_AHRS::use_recorded_origin_maybe() return; } - // only set if not using GPS - if (using_gps()) { + // don't use recorded origin if the configured EKF uses GPS for + // position — GPS will set a correct origin when it gets + // a fix. Using the recorded origin here would prevent GPS from + // setting it later (EKF origin is immutable once set). + if (using_gps_for_pos()) { return; } @@ -3601,6 +3604,34 @@ bool AP_AHRS::using_gps(void) const return true; } +// check if GPS is configured as the position source for +// the configured EKF type +bool AP_AHRS::using_gps_for_pos(void) const +{ + switch (active_EKF_type()) { +#if HAL_NAVEKF2_AVAILABLE + case EKFType::TWO: + return EKF2.configuredToUseGPSForPosXY(); +#endif +#if HAL_NAVEKF3_AVAILABLE + case EKFType::THREE: + return EKF3.configuredToUseGPSForPos(); +#endif +#if AP_AHRS_DCM_ENABLED + case EKFType::DCM: + return _gps_use != GPSUse::Disable; +#endif +#if AP_AHRS_SIM_ENABLED + case EKFType::SIM: +#endif +#if AP_AHRS_EXTERNAL_ENABLED + case EKFType::EXTERNAL: +#endif + return true; + } + return true; +} + // set and save the alt noise parameter value void AP_AHRS::set_alt_measurement_noise(float noise) { diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 348c969d88705..ccb6ed0abcc74 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -454,6 +454,11 @@ class AP_AHRS { // always returns true for External and SIM EKF types bool using_gps(void) const; + // check if GPS is configured as the horizontal position source + // for the configured EKF type. Used to decide whether GPS will + // set the EKF origin (which is immutable once set). + bool using_gps_for_pos(void) const; + // set and save the ALT_M_NSE parameter value void set_alt_measurement_noise(float noise); diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 54239d349df3c..0ce5f86815ac2 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -1227,11 +1227,13 @@ bool AP_Arming::terrain_database_required() const { #if AP_MISSION_ENABLED AP_Mission *mission = AP::mission(); - if (mission == nullptr) { - // no mission support? - return false; + if (mission != nullptr && mission->contains_terrain_alt_items()) { + return true; } - if (mission->contains_terrain_alt_items()) { +#endif +#if AP_FENCE_ENABLED + const AC_Fence* fence = AP::fence(); + if (fence != nullptr && fence->terrain_database_required()) { return true; } #endif diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 41daa356a65bd..06a903d28286d 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -475,7 +475,13 @@ bool AP_Camera::send_mavlink_message(GCS_MAVLINK &link, const enum ap_message ms break; case MSG_CAMERA_INFORMATION: CHECK_PAYLOAD_SIZE2(CAMERA_INFORMATION); - send_camera_information(chan); + if (_camera_information_send_instance >= 0) { + const int16_t instance = _camera_information_send_instance; + _camera_information_send_instance = -1; + send_camera_information((uint8_t)instance, chan); + } else { + send_camera_information(chan); + } break; case MSG_CAMERA_SETTINGS: CHECK_PAYLOAD_SIZE2(CAMERA_SETTINGS); @@ -616,6 +622,18 @@ void AP_Camera::send_camera_information(mavlink_channel_t chan) } } +// send camera information for a specific instance to GCS +void AP_Camera::send_camera_information(uint8_t instance, mavlink_channel_t chan) +{ + WITH_SEMAPHORE(_rsem); + + auto *backend = get_instance(instance); + if (backend == nullptr) { + return; + } + backend->send_camera_information(chan); +} + #if AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED // send video stream information message to GCS void AP_Camera::send_video_stream_information(mavlink_channel_t chan) diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 1f91c0cad7d12..2a724fffb8da6 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -96,6 +96,12 @@ class AP_Camera { // send the message, true otherwise bool send_mavlink_message(class GCS_MAVLINK &link, const enum ap_message id); + // send camera information for a specific instance (0-based) to GCS + void send_camera_information(uint8_t instance, mavlink_channel_t chan); + + // select which instance to send on the next deferred MSG_CAMERA_INFORMATION send + void set_camera_information_send_instance(int16_t instance) { _camera_information_send_instance = instance; } + // configure camera void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time); void configure(uint8_t instance, float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time); @@ -275,6 +281,10 @@ class AP_Camera { bool _is_in_auto_mode; // true if in AUTO mode uint32_t log_camera_bit; // logging bit (from LOG_BITMASK) to enable camera logging AP_Camera_Backend *_backends[AP_CAMERA_MAX_INSTANCES]; // pointers to instantiated backends + // Stashes the 0-based instance requested by MAV_CMD_REQUEST_MESSAGE(CAMERA_INFORMATION, param2). + // Used to pass the target instance through the deferred-message path so that COMMAND_ACK is + // transmitted before the CAMERA_INFORMATION response. -1 means send for all instances. + int16_t _camera_information_send_instance = -1; }; namespace AP { diff --git a/libraries/AP_Camera/AP_Camera_Servo.cpp b/libraries/AP_Camera/AP_Camera_Servo.cpp index b5308f156c29e..86dd9ec8c4e02 100644 --- a/libraries/AP_Camera/AP_Camera_Servo.cpp +++ b/libraries/AP_Camera/AP_Camera_Servo.cpp @@ -9,6 +9,9 @@ extern const AP_HAL::HAL& hal; // initialize the AP_Camera_Servo driver void AP_Camera_Servo::init() { + // call parent init to set default camera info (flags, focal length, etc.) + AP_Camera_Backend::init(); + // set the zoom and focus to the trim point SRV_Channels::set_output_scaled(SRV_Channel::k_cam_zoom, 500); SRV_Channels::set_output_scaled(SRV_Channel::k_cam_focus, 500); diff --git a/libraries/AP_Camera/AP_RunCam.cpp b/libraries/AP_Camera/AP_RunCam.cpp index 6e770fb6d7416..6cb597550d0d3 100644 --- a/libraries/AP_Camera/AP_RunCam.cpp +++ b/libraries/AP_Camera/AP_RunCam.cpp @@ -51,13 +51,13 @@ const AP_Param::GroupInfo AP_RunCam::var_info[] = { // @User: Advanced AP_GROUPINFO("BT_DELAY", 3, AP_RunCam, _boot_delay_ms, 7000), - // @Param: BTN_DELAY + // @Param: BTN_DELY // @DisplayName: RunCam button delay before allowing further button presses // @Description: Time it takes for the a RunCam button press to be actived in ms. If this is too short then commands can get out of sync. // @User: Advanced AP_GROUPINFO("BTN_DELY", 4, AP_RunCam, _button_delay_ms, RUNCAM_DEFAULT_BUTTON_PRESS_DELAY), - // @Param: MDE_DELAY + // @Param: MDE_DELY // @DisplayName: RunCam mode delay before allowing further button presses // @Description: Time it takes for the a RunCam mode button press to be actived in ms. If a mode change first requires a video recording change then double this value is used. If this is too short then commands can get out of sync. // @User: Advanced diff --git a/libraries/AP_DAL/AP_DAL.cpp b/libraries/AP_DAL/AP_DAL.cpp index 9c5398112f630..0e1df8b442258 100644 --- a/libraries/AP_DAL/AP_DAL.cpp +++ b/libraries/AP_DAL/AP_DAL.cpp @@ -420,6 +420,8 @@ void AP_DAL::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vec _RBOH.delAng = delAng; _RBOH.delTime = delTime; _RBOH.timeStamp_ms = timeStamp_ms; + _RBOH.posOffset = posOffset; + _RBOH.delay_ms = delay_ms; WRITE_REPLAY_BLOCK_IFCHANGED(RBOH, _RBOH, old); } diff --git a/libraries/AP_DAL/LogStructure.h b/libraries/AP_DAL/LogStructure.h index 26c14c3dde360..2aa3d284c5baf 100644 --- a/libraries/AP_DAL/LogStructure.h +++ b/libraries/AP_DAL/LogStructure.h @@ -404,10 +404,10 @@ struct log_RMGI { // @LoggerMessage: RBCH // @Description: Replay Data Beacon Header -// @Field: PX: zero, unused -// @Field: PY: zero, unused -// @Field: PZ: zero, unused -// @Field: AE: zero, unused +// @Field: PX: beacon system estimated vehicle position, North +// @Field: PY: beacon system estimated vehicle position, East +// @Field: PZ: beacon system estimated vehicle position, Down +// @Field: AE: beacon system estimated vehicle position accuracy // @Field: OLat: origin latitude // @Field: OLng: origin longitude // @Field: OAlt: origin altitude @@ -566,10 +566,10 @@ struct log_RWOH { // @Field: DAZ: delta-angle-Z // @Field: DT: delta-time // @Field: TS: data timestamp -// @Field: OX: zero, unused -// @Field: OY: zero, unused -// @Field: OZ: zero, unused -// @Field: D: zero, unused +// @Field: OX: pos-offset-X +// @Field: OY: pos-offset-Y +// @Field: OZ: pos-offset-Z +// @Field: D: delay in body odometry data struct log_RBOH { float quality; Vector3f delPos; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 387810dc4e7f9..47ce3e6a93933 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -28,6 +28,7 @@ #include "AP_ExternalAHRS_InertialLabs.h" #include "AP_ExternalAHRS_SBG.h" #include "AP_ExternalAHRS_GSOF.h" +#include "AP_ExternalAHRS_SensAItion.h" #include #include @@ -60,7 +61,7 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = { // @Param: _TYPE // @DisplayName: AHRS type // @Description: Type of AHRS device - // @Values: 0:None,1:VectorNav,2:MicroStrain5,5:InertialLabs,6:Trimble GSOF,7:MicroStrain7,8:SBG + // @Values: 0:None,1:VectorNav,2:MicroStrain5,5:InertialLabs,6:Trimble GSOF,7:MicroStrain7,8:SBG,11:SensAItion // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_ExternalAHRS, devtype, HAL_EXTERNAL_AHRS_DEFAULT, AP_PARAM_FLAG_ENABLE), @@ -74,8 +75,7 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = { // @Param: _OPTIONS // @DisplayName: External AHRS options // @Description: External AHRS options bitmask - // @Bitmask: 0:Vector Nav use uncompensated values for accel gyro and mag. - // @Bitmask: 1:SBG uses EKF as GNSS. + // @Bitmask: 0:Vector Nav use uncompensated values for accel gyro and mag, 1:SBG uses EKF as GNSS, 2:SensAItion used as AHRS // @User: Standard AP_GROUPINFO("_OPTIONS", 3, AP_ExternalAHRS, options, 0), @@ -138,6 +138,11 @@ void AP_ExternalAHRS::init(void) backend = NEW_NOTHROW AP_ExternalAHRS_InertialLabs(this, state); return; #endif +#if AP_EXTERNAL_AHRS_SENSAITION_ENABLED + case DevType::SensAItion: + backend = NEW_NOTHROW AP_ExternalAHRS_SensAItion(this, state); + return; +#endif #if AP_EXTERNAL_AHRS_SBG_ENABLED case DevType::SBG: diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index a9789bfbbcf17..c4097738e4d87 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -36,6 +36,7 @@ class AP_ExternalAHRS { friend class AP_ExternalAHRS_backend; friend class AP_ExternalAHRS_SBG; friend class AP_ExternalAHRS_VectorNav; + friend class AP_ExternalAHRS_SensAItion; AP_ExternalAHRS(); @@ -68,6 +69,9 @@ class AP_ExternalAHRS { #endif // 9 reserved for EulerNav // 10 reserved for Aeron +#if AP_EXTERNAL_AHRS_SENSAITION_ENABLED + SensAItion = 11, +#endif }; static AP_ExternalAHRS *get_singleton(void) { @@ -141,7 +145,7 @@ class AP_ExternalAHRS { } baro_data_message_t; typedef struct { - Vector3f field; + Vector3f field; // Magnetic flux density (mgauss) } mag_data_message_t; typedef struct { @@ -188,6 +192,7 @@ class AP_ExternalAHRS { enum class OPTIONS { VN_UNCOMP_IMU = 1U << 0, SBG_EKF_AS_GNSS = 1U << 1, + SENSAITION_INS = 1U << 2, }; bool option_is_set(OPTIONS option) const { return (options.get() & int32_t(option)) != 0; } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_SensAItion.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_SensAItion.cpp new file mode 100644 index 0000000000000..9c639228fc80b --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_SensAItion.cpp @@ -0,0 +1,416 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +/* + Support for SensAItion serial connected INS and IMU + Implements SensAItion protocol with ArduPilot-specific adaptations +*/ + +#include "AP_ExternalAHRS_SensAItion.h" + +#if AP_EXTERNAL_AHRS_SENSAITION_ENABLED + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace +{ +const float MINIMUM_INTERESTING_BAROMETER_CHANGE_p = 1.0f; +const float MINIMUM_INTERESTING_TEMP_CHANGE_degc = 0.1f; +const uint32_t BARO_UPDATE_TIMEOUT_ms = 100U; +} + +extern const AP_HAL::HAL &hal; + +AP_ExternalAHRS_SensAItion::AP_ExternalAHRS_SensAItion(AP_ExternalAHRS *_frontend, AP_ExternalAHRS::state_t &_state) : + AP_ExternalAHRS_backend(_frontend, _state), + parser(AP_ExternalAHRS_SensAItion_Parser::ConfigMode::IMU) +{ + ins_mode_enabled = option_is_set(AP_ExternalAHRS::OPTIONS::SENSAITION_INS); + + auto mode = ins_mode_enabled ? + AP_ExternalAHRS_SensAItion_Parser::ConfigMode::INTERLEAVED_INS : + AP_ExternalAHRS_SensAItion_Parser::ConfigMode::IMU; + + if (ins_mode_enabled) { + parser = AP_ExternalAHRS_SensAItion_Parser(mode); + } + + auto &sm = AP::serialmanager(); + uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0); + baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0); + port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0); + if (!uart || baudrate == 0 || port_num == -1) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "KEBNI: Serial Port Not Found!"); + return; + } + + if (ins_mode_enabled) { + set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::IMU) | + uint16_t(AP_ExternalAHRS::AvailableSensor::GPS) | + uint16_t(AP_ExternalAHRS::AvailableSensor::BARO) | + uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS)); + } else { + set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::IMU)); + } + + if (!hal.scheduler->thread_create( + FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_SensAItion::update_thread, void), + "AHRS_SensAItion", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "KEBNI: Failed to create thread!"); + } +} + +int8_t AP_ExternalAHRS_SensAItion::get_port() const +{ + return uart ? port_num : -1; +} + +const char* AP_ExternalAHRS_SensAItion::get_name() const +{ + return "Kebni SensAItion"; +} + +bool AP_ExternalAHRS_SensAItion::healthy() const +{ + const uint32_t now_ms = AP_HAL::millis(); + + WITH_SEMAPHORE(driver_state.semaphore); + + if ((now_ms - driver_state.last_imu_pkt_ms) > 160) { + // IMU is required at high rate for health + return false; + } + + if (ins_mode_enabled) { + if ((now_ms - driver_state.last_ins_pkt_ms) > 400) { + // INS packets must also have high enough rate + return false; + } + + if (!(driver_state.last_sensor_valid & 0x01)) { + // IMU not available + return false; + } + + if (driver_state.last_gnss1_fix < 3) { + // No 3D satellite fix + return false; + } + } + + return true; +} + +bool AP_ExternalAHRS_SensAItion::initialised() const +{ + return setup_complete; +} + +bool AP_ExternalAHRS_SensAItion::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const +{ + if (!healthy()) { + hal.util->snprintf(failure_msg, failure_msg_len, "SensAItion Unhealthy"); + return false; + } + + if (ins_mode_enabled) { + uint8_t last_alignment_status; + { + WITH_SEMAPHORE(driver_state.semaphore); + last_alignment_status = driver_state.last_alignment_status; + } + if (last_alignment_status != 1) { + hal.util->snprintf(failure_msg, failure_msg_len, "SensAItion Aligning"); + return false; + } + } + + return true; +} + +void AP_ExternalAHRS_SensAItion::get_filter_status(nav_filter_status &status) const +{ + memset(&status, 0, sizeof(status)); + + status.flags.initalized = initialised(); + + if (healthy()) { + WITH_SEMAPHORE(driver_state.semaphore); + if (ins_mode_enabled && driver_state.last_alignment_status == 1) { + status.flags.attitude = true; + status.flags.horiz_pos_abs = true; + status.flags.vert_pos = true; + status.flags.horiz_vel = true; + status.flags.vert_vel = true; + status.flags.using_gps = true; + status.flags.horiz_pos_rel = true; + status.flags.pred_horiz_pos_abs = true; + status.flags.pred_horiz_pos_rel = true; + } + } +} + +bool AP_ExternalAHRS_SensAItion::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const +{ + WITH_SEMAPHORE(driver_state.semaphore); + if (ins_mode_enabled && driver_state.last_alignment_status == 1) { + posVar = driver_state.last_h_pos_quality * pos_gate_scale; + velVar = driver_state.last_vel_quality * vel_gate_scale; + hgtVar = driver_state.last_v_pos_quality * hgt_gate_scale; + tasVar = 0; //not used + return true; + } + + return false; +} + +uint8_t AP_ExternalAHRS_SensAItion::num_gps_sensors() const +{ + return ins_mode_enabled ? 1 : 0; +} + +// --------------------------------------------------------------------------- +// THREAD & PROFILER +// --------------------------------------------------------------------------- +void AP_ExternalAHRS_SensAItion::update_thread() +{ + while (true) { + if (!check_uart()) { + hal.scheduler->delay_microseconds(500); + } + } +} + +bool AP_ExternalAHRS_SensAItion::check_uart() +{ + WITH_SEMAPHORE(sem_handle); + + if (!uart) { + return false; + } + + if (!setup_complete) { + uart->begin(baudrate); + setup_complete = true; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "KEBNI: INIT. Mode:%d Baud:%u", + (int)ins_mode_enabled, (unsigned)baudrate); + } + + const auto nread = uart->read(buffer, sizeof(buffer)); + if (nread <= 0) { + return false; + } + + const uint32_t now_ms = AP_HAL::millis(); + + AP_ExternalAHRS_SensAItion_Parser::Measurement meas; + bool found_measurement = false; + ssize_t parsed_bytes = 0; + while (parsed_bytes < nread) { + const size_t max_bytes_to_parse = nread - parsed_bytes; + parsed_bytes += parser.parse_stream(&buffer[parsed_bytes], max_bytes_to_parse, meas); + + switch (meas.type) { + case AP_ExternalAHRS_SensAItion_Parser::MeasurementType::UNINITIALIZED: + // The parser is done parsing the whole buffer, but didn't find any + // more complete message. (We might be in the middle of a message + // that will be finished later.) + break; + case AP_ExternalAHRS_SensAItion_Parser::MeasurementType::IMU: + handle_imu(meas, now_ms); + found_measurement = true; + break; + case AP_ExternalAHRS_SensAItion_Parser::MeasurementType::AHRS: + handle_ahrs(meas, now_ms); + found_measurement = true; + break; + case AP_ExternalAHRS_SensAItion_Parser::MeasurementType::INS: + handle_ins(meas, now_ms); + found_measurement = true; + break; + } + } + + return found_measurement; +} + +void AP_ExternalAHRS_SensAItion::handle_imu(const AP_ExternalAHRS_SensAItion_Parser::Measurement& meas, uint32_t now_ms) +{ + { + WITH_SEMAPHORE(state.sem); + state.accel = meas.acceleration_mss; + state.gyro = meas.angular_velocity_rads; + } + + // INS + ins.accel = meas.acceleration_mss; + ins.gyro = meas.angular_velocity_rads; + ins.temperature = meas.temperature_degc; + AP::ins().handle_external(ins); + +#if AP_COMPASS_EXTERNALAHRS_ENABLED + // COMPASS + mag.field = meas.magnetic_field_mgauss; + AP::compass().handle_external(mag); +#endif + +#if AP_BARO_EXTERNALAHRS_ENABLED + bool baro_updated = false; +#endif + + { + WITH_SEMAPHORE(driver_state.semaphore); + driver_state.last_imu_pkt_ms = now_ms; + +#if AP_BARO_EXTERNALAHRS_ENABLED + // BARO + // ArduPlane has an internal check that triggers an error if there are too many barometer + // readings with the same value. At high sampling rates, we triggered that check because + // the barometer is internally sampled at a lower rate. To avoid that, we only update the value + // if it changes OR a certain minimum time has passed. + const bool pressure_changed = fabsf(baro.pressure_pa - meas.air_pressure_p) > MINIMUM_INTERESTING_BAROMETER_CHANGE_p; + const bool temp_changed = fabsf(baro.temperature - meas.temperature_degc) > MINIMUM_INTERESTING_TEMP_CHANGE_degc; + const bool timeout = now_ms > driver_state.last_baro_update_ms + BARO_UPDATE_TIMEOUT_ms; + + if (pressure_changed || temp_changed || timeout) { + driver_state.last_baro_update_ms = now_ms; + baro.instance = 0; + baro.pressure_pa = meas.air_pressure_p; + baro.temperature = meas.temperature_degc; + baro_updated = true; + } +#endif + + } + +#if AP_BARO_EXTERNALAHRS_ENABLED + // Do the update after releasing the semaphore, for speed + if (baro_updated) { + AP::baro().handle_external(baro); + } +#endif +} + +void AP_ExternalAHRS_SensAItion::handle_ahrs(const AP_ExternalAHRS_SensAItion_Parser::Measurement& meas, uint32_t now_ms) +{ + { + WITH_SEMAPHORE(driver_state.semaphore); + driver_state.last_quat_pkt_ms = now_ms; + } + + { + WITH_SEMAPHORE(state.sem); + state.quat = meas.orientation; + state.have_quaternion = true; + } +} + +void AP_ExternalAHRS_SensAItion::handle_ins(const AP_ExternalAHRS_SensAItion_Parser::Measurement& meas, uint32_t now_ms) +{ + // STATE + { + WITH_SEMAPHORE(state.sem); + state.location = Location( + meas.location.lat, + meas.location.lng, + meas.location.alt, + Location::AltFrame::ABSOLUTE + ); + state.velocity = meas.velocity_ned; + state.have_location = true; + state.have_velocity = true; + state.last_location_update_us = AP_HAL::micros(); + + if (!state.have_origin && meas.alignment_status) { + state.origin = Location( + meas.location.lat, + meas.location.lng, + meas.location.alt, + Location::AltFrame::ABSOLUTE + ); + state.have_origin = true; + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "KEBNI: Origin Set."); + } + } + + { + WITH_SEMAPHORE(driver_state.semaphore); + + // Local data + driver_state.last_ins_pkt_ms = now_ms; + driver_state.last_alignment_status = meas.alignment_status; + driver_state.last_sensor_valid = meas.sensor_valid; + driver_state.last_gnss1_fix = meas.gnss1_fix; + driver_state.last_gnss2_fix = meas.gnss2_fix; + driver_state.last_error_flags = meas.error_flags; + driver_state.last_h_pos_quality = meas.pos_accuracy.xy().length(); + driver_state.last_v_pos_quality = meas.pos_accuracy.z; + driver_state.last_vel_quality = meas.vel_accuracy.length(); + + // GPS + gps.horizontal_pos_accuracy = driver_state.last_h_pos_quality; + gps.vertical_pos_accuracy = driver_state.last_v_pos_quality; + } + + // GPS - continued + gps.gps_week = meas.gps_week; + gps.ms_tow = meas.time_itow_ms; + gps.fix_type = AP_GPS_FixType(meas.gnss1_fix); + gps.satellites_in_view = meas.num_sats_gnss1; + gps.horizontal_vel_accuracy = meas.vel_accuracy.xy().length(); + gps.latitude = meas.location.lat; + gps.longitude = meas.location.lng; + + // Note: SensAItion reports altitude relative to WGS84, not MSL. + // But we expect the user to reset the altitude to 0 at start, + // so the absolute reference should not matter. + gps.msl_altitude = meas.location.alt; + gps.ned_vel_north = meas.velocity_ned.x; + gps.ned_vel_east = meas.velocity_ned.y; + gps.ned_vel_down = meas.velocity_ned.z; + + // 3. Estimate DOPs (Unitless) using assumed UERE of 3.0m + // This answers "What is HDOP/VDOP?" + const float ASSUMED_UERE = 3.0f; + + float est_hdop = gps.horizontal_pos_accuracy / ASSUMED_UERE; + float est_vdop = gps.vertical_pos_accuracy / ASSUMED_UERE; + + // 4. Sanity Clamping (DOP cannot be 0, and rarely < 0.6) + if (est_hdop < 0.7f) { + est_hdop = 0.7f; + } + if (est_vdop < 0.7f) { + est_vdop = 0.7f; + } + gps.hdop = est_hdop; + gps.vdop = est_vdop; + + // Handle + uint8_t instance; + if (AP::gps().get_first_external_instance(instance)) { + AP::gps().handle_external(gps, instance); + } +} + +#endif // AP_EXTERNAL_AHRS_SENSAITION_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_SensAItion.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_SensAItion.h new file mode 100644 index 0000000000000..578a78c2b52d8 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_SensAItion.h @@ -0,0 +1,114 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#pragma once + +#include "AP_ExternalAHRS_config.h" + +#if AP_EXTERNAL_AHRS_SENSAITION_ENABLED + +#include "AP_ExternalAHRS_backend.h" +#include "AP_ExternalAHRS_SensAItion_Parser.h" + +/* +This class is the interface to Kebni's SensAItion range of inertial navigation +sensors, which can provide raw sensor data and/or a sensor fusion solution. +*/ +class AP_ExternalAHRS_SensAItion : public AP_ExternalAHRS_backend +{ +public: + AP_ExternalAHRS_SensAItion(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &_state); + + // Hardware Identification + int8_t get_port() const override; + const char* get_name() const override; + + // Health & Status Interface + bool healthy() const override; + bool initialised() const override; + bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override; + void get_filter_status(nav_filter_status &status) const override; + bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override; + + // GPS Interface + uint8_t num_gps_sensors() const override; + + // Main Loop + void update() override + { + check_uart(); + } + +private: + mutable HAL_Semaphore sem_handle; + + // The member variables below are accessed both from our own + // thread and the main thread and should be protected + // by sem_handle! + // ======================================================= + AP_ExternalAHRS::ins_data_message_t ins; + AP_ExternalAHRS::mag_data_message_t mag; + AP_ExternalAHRS::baro_data_message_t baro; + AP_ExternalAHRS::gps_data_message_t gps; + AP_ExternalAHRS_SensAItion_Parser parser; + + // UART + AP_HAL::UARTDriver *uart; + uint8_t buffer[AP_ExternalAHRS_SensAItion_Parser::MAX_PACKET_SIZE]; + + bool setup_complete; + + // End of member variables protected by sem_handle + // ======================================================= + + // These states are accessed by the public accessor functions, + // so to avoid slowing them down by waiting for sem_handle, + // we use the included semaphore to protect access to them. + // USAGE: + // - If taking both semaphores, always take sem_handle first! + // - Hold this semaphore only while accessing the driver state + // to minimize the waiting time for it! + struct { + mutable HAL_Semaphore semaphore; + + uint32_t last_imu_pkt_ms; + uint32_t last_ins_pkt_ms; // Only used in INS mode + uint32_t last_quat_pkt_ms; // Only used in INS mode + uint32_t last_baro_update_ms; + + // Last known values from INS packet (Packet 2) + uint8_t last_alignment_status; + uint8_t last_gnss1_fix; + uint8_t last_gnss2_fix; + uint8_t last_sensor_valid; + float last_h_pos_quality = 999.9f; + float last_v_pos_quality = 999.9f; + float last_vel_quality = 999.9f; + uint32_t last_error_flags; + } driver_state; + + // Read-only after construction, do not need semaphore + bool ins_mode_enabled = false; + uint32_t baudrate = 460800; + int8_t port_num = -1; + + void update_thread(); + bool check_uart(); + + void handle_imu(const AP_ExternalAHRS_SensAItion_Parser::Measurement& meas, uint32_t now_ms); + void handle_ahrs(const AP_ExternalAHRS_SensAItion_Parser::Measurement& meas, uint32_t now_ms); + void handle_ins(const AP_ExternalAHRS_SensAItion_Parser::Measurement& meas, uint32_t now_ms); +}; +#endif // AP_EXTERNAL_AHRS_SENSAITION_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_SensAItion_Parser.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_SensAItion_Parser.cpp new file mode 100644 index 0000000000000..5ddcb28974111 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_SensAItion_Parser.cpp @@ -0,0 +1,401 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +#include +#include // Required for memchr, memmove +#include "AP_ExternalAHRS_SensAItion_Parser.h" +#include +#include +#include +#include + +// Constructor +AP_ExternalAHRS_SensAItion_Parser::AP_ExternalAHRS_SensAItion_Parser(ConfigMode mode) : + config_mode(mode) +{ + reset_parser(); +} + +size_t AP_ExternalAHRS_SensAItion_Parser::parse_stream(const uint8_t* data, size_t data_size, Measurement& meas) +{ + for (size_t i = 0; i < data_size; i++) { + if (parse_single_byte(data[i])) { + decode_packet(meas); + reset_parser(); // Ready for more packets in same buffer + return i + 1; // No of bytes parsed + } + } + + meas = Measurement(); // Uninitialized + return data_size; +} + +void AP_ExternalAHRS_SensAItion_Parser::reset_parser() +{ + parse_state = ParseState::WAITING_HEADER; + packet_buffer_len = 0; + target_payload_len = 0; + current_packet_id = PacketID::UNKNOWN; +} + +// Error Handler +void AP_ExternalAHRS_SensAItion_Parser::handle_invalid_packet() +{ + // Fallback if called with empty buffer, which should never happen + if (packet_buffer_len == 0) { + return; + } + + // Look for a new header byte starting from index 1 to resync + uint8_t *p = (uint8_t *)memchr(&packet_buffer[1], HEADER_BYTE, packet_buffer_len - 1); + + if (p == nullptr) { + // No header found, reset completely + reset_parser(); + return; + } + + const size_t bytes_to_discard = p - packet_buffer; + const size_t bytes_to_keep = packet_buffer_len - bytes_to_discard; + + memmove(&packet_buffer[0], p, bytes_to_keep); + packet_buffer_len = bytes_to_keep; + + // Determine State based on Mode and remaining data + if (config_mode == ConfigMode::INTERLEAVED_INS) { + if (packet_buffer_len >= 2) { + // We have Header + Potential ID. Process it immediately. + uint8_t id = packet_buffer[1]; + switch (static_cast(id)) { + case PacketID::IMU: + target_payload_len = PAYLOAD_SIZE_IMU; + parse_state = ParseState::COLLECTING_PAYLOAD; + current_packet_id = PacketID::IMU; + break; + case PacketID::AHRS: + target_payload_len = PAYLOAD_SIZE_QUAT; + parse_state = ParseState::COLLECTING_PAYLOAD; + current_packet_id = PacketID::AHRS; + break; + case PacketID::INS: + target_payload_len = PAYLOAD_SIZE_INS; + parse_state = ParseState::COLLECTING_PAYLOAD; + current_packet_id = PacketID::INS; + break; + default: + // The "New" ID is also bad. Drop header and retry. + reset_parser(); + return; + } + } else { + parse_state = ParseState::WAITING_ID; + } + } else { + // Legacy Mode + target_payload_len = PAYLOAD_SIZE_IMU; + parse_state = ParseState::COLLECTING_PAYLOAD; + } +} + +// Checksum Validator & Deep Debugger +bool AP_ExternalAHRS_SensAItion_Parser::buffer_contains_valid_packet() const +{ + if (packet_buffer_len < 2 || packet_buffer[0] != HEADER_BYTE) { + return false; + } + + const uint8_t calculated = crc_xor_of_bytes(&packet_buffer[1], packet_buffer_len - 2); + const uint8_t received = packet_buffer[packet_buffer_len - 1]; + + if (calculated != received) { + return false; + } + + return true; +} + +// The Core State Machine +bool AP_ExternalAHRS_SensAItion_Parser::parse_single_byte(uint8_t byte) +{ + // Safety: Prevent buffer overflow + if (packet_buffer_len >= MAX_PACKET_SIZE) { + handle_invalid_packet(); + if (packet_buffer_len >= MAX_PACKET_SIZE) { + reset_parser(); + } + } + + switch (parse_state) { + case ParseState::WAITING_HEADER: + if (byte == HEADER_BYTE) { + packet_buffer_len = 0; + packet_buffer[packet_buffer_len++] = byte; + + if (config_mode == ConfigMode::INTERLEAVED_INS) { + parse_state = ParseState::WAITING_ID; + } else { + target_payload_len = PAYLOAD_SIZE_IMU; + parse_state = ParseState::COLLECTING_PAYLOAD; + } + } + break; + + case ParseState::WAITING_ID: + packet_buffer[packet_buffer_len++] = byte; + + switch (static_cast(byte)) { + case PacketID::IMU: + target_payload_len = PAYLOAD_SIZE_IMU; + current_packet_id = PacketID::IMU; + parse_state = ParseState::COLLECTING_PAYLOAD; + break; + + case PacketID::AHRS: + target_payload_len = PAYLOAD_SIZE_QUAT; + current_packet_id = PacketID::AHRS; + parse_state = ParseState::COLLECTING_PAYLOAD; + break; + + case PacketID::INS: + target_payload_len = PAYLOAD_SIZE_INS; + current_packet_id = PacketID::INS; + parse_state = ParseState::COLLECTING_PAYLOAD; + break; + + default: + parse_errors++; + handle_invalid_packet(); + break; + } + break; + + case ParseState::COLLECTING_PAYLOAD: + packet_buffer[packet_buffer_len++] = byte; + + // Calculate Expected Total Length + // Legacy: Header(1) + Payload(N) + CRC(1) + // Interleaved: Header(1) + ID(1) + Payload(N) + CRC(1) + const size_t overhead = (config_mode == ConfigMode::INTERLEAVED_INS) ? 3 : 2; + const size_t expected_total_len = target_payload_len + overhead; + + if (packet_buffer_len >= expected_total_len) { + if (buffer_contains_valid_packet()) { + valid_packets++; + return true; + } + + parse_errors++; + handle_invalid_packet(); + return false; + } + break; + } + + return false; +} + +bool AP_ExternalAHRS_SensAItion_Parser::validate_checksum() const +{ + return buffer_contains_valid_packet(); +} + +// Router +void AP_ExternalAHRS_SensAItion_Parser::decode_packet(Measurement& measurement) +{ + const uint8_t* payload = nullptr; + + if (config_mode == ConfigMode::INTERLEAVED_INS) { + payload = &packet_buffer[2]; // Skip Header + ID + switch (current_packet_id) { + case PacketID::IMU: + decode_imu(payload, measurement); + break; + case PacketID::AHRS: + decode_ahrs(payload, measurement); + break; + case PacketID::INS: + decode_ins(payload, measurement); + break; + case PacketID::UNKNOWN: + assert(false && "decode_packet() should only be called with a valid packet ID!"); + break; + } + } else { + payload = &packet_buffer[1]; // Skip Header + decode_imu(payload, measurement); + } +} + +// IMU Decoder (Packet 0) +void AP_ExternalAHRS_SensAItion_Parser::decode_imu(const uint8_t* payload, Measurement& measurement) +{ + // ... [Code omitted: No changes to decoders, use your existing implementation] ... + // Accel (Bytes 0-11): 3 x Int32 (ug) + const int32_t accel_x_ug = be32toh_ptr(&payload[0]); + const int32_t accel_y_ug = be32toh_ptr(&payload[4]); + const int32_t accel_z_ug = be32toh_ptr(&payload[8]); + + // Gyro (Bytes 12-23): 3 x Int32 (udeg/s) + const int32_t gyro_x_udegs = be32toh_ptr(&payload[12]); + const int32_t gyro_y_udegs = be32toh_ptr(&payload[16]); + const int32_t gyro_z_udegs = be32toh_ptr(&payload[20]); + + // Temp (Bytes 24-25): 1 x Int16 (Scaled) + const int16_t temp_raw = be16toh_ptr(&payload[24]); + + // Mag (Bytes 26-31): 3 x Int16 (mGauss) + const int16_t mag_x_mgauss = be16toh_ptr(&payload[26]); + const int16_t mag_y_mgauss = be16toh_ptr(&payload[28]); + const int16_t mag_z_mgauss = be16toh_ptr(&payload[30]); + + // Baro (Bytes 32-35): 1 x Int32 (0.1 Pa) + const int32_t baro_raw = be32toh_ptr(&payload[32]); + + // Accel: ug -> m/s^2 (Note: 1,000,000 ug = 9.81 m/s^2 roughly) + const float ug_to_mss = 1.0e-6f * GRAVITY_MSS; + measurement.acceleration_mss = Vector3f(accel_x_ug, accel_y_ug, accel_z_ug) * ug_to_mss; + + // Gyro: udeg/s -> rad/s (Note: 1,000,000 udeg/s = 1 deg/s = 0.017 rad/s) + const float udeg_to_rad = 1.0e-6f * DEG_TO_RAD; + measurement.angular_velocity_rads = Vector3f(gyro_x_udegs, gyro_y_udegs, gyro_z_udegs) * udeg_to_rad; + + // Temp: (Raw * 0.008) + 20 + measurement.temperature_degc = ((float)temp_raw * 0.008f) + 20.0f; + + // Mag: mGauss (Pass-through, ArduPilot expects mGauss) + measurement.magnetic_field_mgauss = Vector3f(mag_x_mgauss, mag_y_mgauss, mag_z_mgauss); + + // Baro: 0.1 Pa -> Pascal + measurement.air_pressure_p = (float)baro_raw * 0.1f; + + // Metadata + measurement.type = MeasurementType::IMU; + measurement.timestamp_us = AP_HAL::micros64(); +} + +void AP_ExternalAHRS_SensAItion_Parser::decode_ahrs(const uint8_t* payload, Measurement& measurement) +{ + // Payload layout: W (0-3), X (4-7), Y (8-11), Z (12-15) + const int32_t quat_w_raw = be32toh_ptr(&payload[0]); + const int32_t quat_x_raw = be32toh_ptr(&payload[4]); + const int32_t quat_y_raw = be32toh_ptr(&payload[8]); + const int32_t quat_z_raw = be32toh_ptr(&payload[12]); + + const float scale_factor = 1.0e-6f; + + measurement.orientation = Quaternion( + (float)quat_w_raw * scale_factor, + (float)quat_x_raw * scale_factor, + (float)quat_y_raw * scale_factor, + (float)quat_z_raw * scale_factor + ); + + measurement.type = MeasurementType::AHRS; + measurement.timestamp_us = AP_HAL::micros64(); +} + +void AP_ExternalAHRS_SensAItion_Parser::decode_ins(const uint8_t* payload, Measurement& measurement) +{ + // --- 1. PARSE RAW BYTES (Big-Endian) --- + + // 0-3: Num Sats (G2, G1) + measurement.num_sats_gnss2 = payload[1]; + measurement.num_sats_gnss1 = payload[3]; + + // 4-7: Error Flags + measurement.error_flags = be32toh_ptr(&payload[4]); + + // 8: Sensor Valid + measurement.sensor_valid = payload[8]; + + // 9-16: Lat/Lon (1e-7 deg) + const int32_t lat_raw = be32toh_ptr(&payload[9]); + const int32_t lon_raw = be32toh_ptr(&payload[13]); + + // 17-28: Velocity N, E, D (mm/s) + const int32_t vel_n_mm = be32toh_ptr(&payload[17]); + const int32_t vel_e_mm = be32toh_ptr(&payload[21]); + const int32_t vel_d_mm = be32toh_ptr(&payload[25]); + + // 29-32: Altitude relative to WGS 84 ellipsoid (mm) + const int32_t alt_raw_mm = be32toh_ptr(&payload[29]); + + // 33: Alignment Status + measurement.alignment_status = payload[33]; + + // 34-37: Time of week (ms) + measurement.time_itow_ms = be32toh_ptr(&payload[34]); + + // 38-39: GNSS Fix + measurement.gnss2_fix = payload[38]; + measurement.gnss1_fix = payload[39]; + + // 40-44: UTC Date/Time + const uint16_t year = (uint16_t)((payload[40]<<8) | payload[41]); + const uint16_t month = (uint16_t)((payload[42]<<8) | payload[43]); + const uint8_t day = payload[44]; + + // 45-68: Accuracy Metrics (mm or mm/s) + const int32_t acc_lat_mm = be32toh_ptr(&payload[45]); + const int32_t acc_lon_mm = be32toh_ptr(&payload[49]); + const int32_t acc_vn_mm = be32toh_ptr(&payload[53]); + const int32_t acc_ve_mm = be32toh_ptr(&payload[57]); + const int32_t acc_vd_mm = be32toh_ptr(&payload[61]); + const int32_t acc_vd_pos_mm = be32toh_ptr(&payload[65]); + + // --- 2. POPULATE & CONVERT --- + const float mm_to_cm = 0.1f; + measurement.location = Location(lat_raw, lon_raw, alt_raw_mm * mm_to_cm, Location::AltFrame::ABSOLUTE); + + const float mms_to_ms = 0.001f; + const float mm_to_m = 0.001f; + + measurement.velocity_ned.x = (float)vel_n_mm * mms_to_ms; + measurement.velocity_ned.y = (float)vel_e_mm * mms_to_ms; + measurement.velocity_ned.z = (float)vel_d_mm * mms_to_ms; + + // Accuracy + measurement.pos_accuracy.x = (float)acc_lat_mm * mm_to_m; // Horizontal Latitude + measurement.pos_accuracy.y = (float)acc_lon_mm * mm_to_m; // Horizontal Longitude + measurement.pos_accuracy.z = (float)acc_vd_pos_mm * mm_to_m; // Vertical + + measurement.vel_accuracy.x = (float)acc_vn_mm * mms_to_ms; + measurement.vel_accuracy.y = (float)acc_ve_mm * mms_to_ms; + measurement.vel_accuracy.z = (float)acc_vd_mm * mms_to_ms; + + // GPS Week Calculation (Gate check) + if (measurement.gnss1_fix >= 2) { + measurement.gps_week = calculate_gps_week(year, month, day); + } else { + measurement.gps_week = 0; + } + + measurement.type = MeasurementType::INS; + measurement.timestamp_us = AP_HAL::micros64(); + +} + +uint16_t AP_ExternalAHRS_SensAItion_Parser::calculate_gps_week(uint16_t year, uint8_t month, uint8_t day) +{ + // Convert to BCD form (DDMMYY) - the GPS_Backend function assumes year is 20YY + const uint32_t bcd_date = year % 100U + month * 100U + day * 10000U; + + const uint32_t bcd_time_ms = 0U; + uint16_t gps_week; + uint32_t gps_time_ms; + AP_GPS_Backend::BCD_to_gps_time(bcd_date, bcd_time_ms, gps_week, gps_time_ms); + + return gps_week; +} diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_SensAItion_Parser.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_SensAItion_Parser.h new file mode 100644 index 0000000000000..c17a87abea24f --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_SensAItion_Parser.h @@ -0,0 +1,159 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/* + SensAItion Protocol Parser + Handles binary protocol parsing (state machine, packet validation, checksums) + Separated from ArduPilot state management for testability +*/ + +#pragma once + +#include +#include +#include + +class AP_ExternalAHRS_SensAItion_Parser +{ +public: + // Configuration Mode defined in EAHRS_OPTIONS (Bit 1) + enum class ConfigMode { + IMU = 0, // IMU Only Mode (No ID byte, fixed 38 byte packet) + INTERLEAVED_INS = 1 // Interleaved Mode (Header -> ID -> Payload) + }; + + // Type of data contained in a decoded Measurement + enum class MeasurementType { + UNINITIALIZED = 0, + IMU, + AHRS, // Maps to Packet 1 (Orientation) + INS // Maps to Packet 2 (Navigation) + }; + + // Public Constants (Available to Driver) + static const uint16_t MAX_PACKET_SIZE = 1024; + static const uint8_t HEADER_BYTE = 0xFA; + + // Container for decoded data passed to Backend + struct Measurement { + MeasurementType type = MeasurementType::UNINITIALIZED; + uint64_t timestamp_us; + + // Packet 0 (IMU) Data + Vector3f acceleration_mss; + Vector3f angular_velocity_rads; + Vector3f magnetic_field_mgauss; + float temperature_degc; + float air_pressure_p; + + // Packet 1 (Orientation) Data + Quaternion orientation; + + // Packet 2 (INS) Data + Location location; // Lat/Lon/Alt + Vector3f velocity_ned; // North/East/Down (m/s) + + // Accuracy Metrics (Vectors as requested) + // ArduPilot often uses float for horiz/vert, but Vector3f is more flexible + // if the sensor provides 3-axis accuracy. + // Based on your config (AccLat, AccLon, AccPosD), we have 3 components. + Vector3f pos_accuracy; // North/East/Down (m) + Vector3f vel_accuracy; // North/East/Down (m/s) + + // Status & Health Flags + uint8_t alignment_status; // 1 = Align OK + uint8_t gnss1_fix; + uint8_t gnss2_fix; + + uint8_t num_sats_gnss1; + uint8_t num_sats_gnss2; + + // Time + uint32_t time_itow_ms; // GNSS time of week (ms) + uint16_t gps_week; // Calculated Week Number + + uint32_t error_flags; // Bitmask from sensor + uint8_t sensor_valid; // Validity bitmask + }; + + // Constructor + AP_ExternalAHRS_SensAItion_Parser(ConfigMode mode); + + // Parse at most 'data_size' bytes from 'data', return # of bytes parsed. + // If we parsed less than 'data_size' bytes, 'meas' contains a measurement, + // otherwise it contains a valid measurement OR is uninitialized. + size_t parse_stream(const uint8_t* data, size_t data_size, Measurement& meas); + + // Number of parsed full length buffers that did not contain a valid packet + uint32_t get_parse_errors() const + { + return parse_errors; + } + + // Number of valid packets received during object lifetime + uint32_t get_valid_packets() const + { + return valid_packets; + } + +private: + // Payload Sizes (Excluding Header, ID, CRC) + static const uint8_t PAYLOAD_SIZE_IMU = 36; // Packet 0 + static const uint8_t PAYLOAD_SIZE_QUAT = 16; // Packet 1 + static const uint8_t PAYLOAD_SIZE_INS = 69; // Packet 2 + + // Packet IDs (Interleaved Mode) + enum class PacketID : uint8_t { + IMU = 0x00, + AHRS = 0x01, + INS = 0x02, + UNKNOWN = 0xFF + }; + + // Internal State Machine + enum class ParseState { + WAITING_HEADER, + WAITING_ID, + COLLECTING_PAYLOAD + }; + + // Core Logic + void reset_parser(); // Does not reset packet and parse error counters + bool parse_single_byte(uint8_t byte); // Return true if it was the last byte of a valid packet + void handle_invalid_packet(); // Robust error recovery (memmove) + bool buffer_contains_valid_packet() const; + bool validate_checksum() const; + + // Decoders + void decode_packet(Measurement& measurement); + void decode_imu(const uint8_t* payload, Measurement& measurement); + void decode_ahrs(const uint8_t* payload, Measurement& measurement); + void decode_ins(const uint8_t* payload, Measurement& measurement); + + //Helpers + uint16_t calculate_gps_week(uint16_t year, uint8_t month, uint8_t day); + + // Members + ConfigMode config_mode; + ParseState parse_state; + PacketID current_packet_id; + + uint8_t packet_buffer[MAX_PACKET_SIZE]; + size_t packet_buffer_len; + size_t target_payload_len; + + uint32_t valid_packets; + uint32_t parse_errors; +}; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h index 378d91daf86bb..6945f178bd66b 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h @@ -32,7 +32,7 @@ #ifndef AP_EXTERNAL_AHRS_SBG_ENABLED #define AP_EXTERNAL_AHRS_SBG_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED -#endif +#endif #ifndef AP_EXTERNAL_AHRS_GSOF_ENABLED #define AP_EXTERNAL_AHRS_GSOF_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED @@ -41,3 +41,7 @@ #ifndef AP_EXTERNALAHRS_GSOF_DEBUG_ENABLED #define AP_EXTERNALAHRS_GSOF_DEBUG_ENABLED 0 #endif + +#ifndef AP_EXTERNAL_AHRS_SENSAITION_ENABLED +#define AP_EXTERNAL_AHRS_SENSAITION_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED +#endif diff --git a/libraries/AP_ExternalAHRS/tests/test_sensaition_parser.cpp b/libraries/AP_ExternalAHRS/tests/test_sensaition_parser.cpp new file mode 100644 index 0000000000000..6053bb877b3fe --- /dev/null +++ b/libraries/AP_ExternalAHRS/tests/test_sensaition_parser.cpp @@ -0,0 +1,706 @@ +#include +#include + +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + +namespace +{ +// Units definitions Kebni +constexpr float UG_PER_MSS = 1e6f / 9.80665f; +constexpr float UDEGS_PER_RADS = 1e6f * 180.0f / 3.1415926f; +constexpr float MHPA_PER_PA = 1e3f / 100.0f; + +using Parser = AP_ExternalAHRS_SensAItion_Parser; +} + +// --- HELPER FUNCTIONS --- +static bool is_equal(const float f1, const float f2, const float eps) +{ + return (fabs(f1 - f2) < eps); +} + +static void fill_be32(uint8_t* data, size_t& loc, int32_t val) +{ + data[loc++] = (val >> 24) & 0xFF; + data[loc++] = (val >> 16) & 0xFF; + data[loc++] = (val >> 8) & 0xFF; + data[loc++] = val & 0xFF; +} + +static void fill_be16(uint8_t* data, size_t& loc, int16_t val) +{ + data[loc++] = (val >> 8) & 0xFF; + data[loc++] = val & 0xFF; +} + +static void fill_u8(uint8_t* data, size_t& loc, uint8_t val) +{ + data[loc++] = val; +} + +// --- UPDATED STRUCT (Matches Parser + Date/Week Support) --- +struct Measurement { + Parser::MeasurementType type; + + // Packet 0 (IMU) Data + Vector3f acceleration_mss; + Vector3f angular_velocity_rads; + Vector3f magnetic_field_mgauss; + float temperature_degc; + float air_pressure_p; + + // Packet 1 (Orientation) Data + Quaternion orientation; + + // Packet 2 (INS) Data + Location location; // Lat/Lon/Alt + Vector3f velocity_ned; // North/East/Down m/s + + // Accuracy Metrics (Vectors) + Vector3f pos_accuracy; // X=Lat, Y=Lon, Z=Alt (meters) + Vector3f vel_accuracy; // X=VelN, Y=VelE, Z=VelD (m/s) + + // Status & Health Flags + uint8_t alignment_status; // 1 = Align OK + uint8_t gnss1_fix; + uint8_t gnss2_fix; + + uint8_t num_sats_gnss1; + uint8_t num_sats_gnss2; + + // Time & Date (Input for Generator) + uint32_t time_itow_ms; + uint16_t year; + uint8_t month; + uint8_t day; + + // The Calculated Result (Output from Parser) + uint16_t gps_week; + + uint32_t error_flags; // Bitmask from sensor + uint8_t sensor_valid; // Byte 49 (New in v5) +}; + +// --- UPDATED GENERATOR (Matches 69-Byte Parser Layout) --- +static void fill_simulated_packet(uint8_t* data, size_t& data_length, + const Measurement& m, + Parser::ConfigMode mode) +{ + size_t idx = 0; + + // 1. Header & ID + data[idx++] = 0xFA; + if (mode == Parser::ConfigMode::INTERLEAVED_INS) { + switch (m.type) { + case Parser::MeasurementType::IMU: + data[idx++] = 0x00; + break; + case Parser::MeasurementType::AHRS: + data[idx++] = 0x01; + break; + case Parser::MeasurementType::INS: + data[idx++] = 0x02; + break; + default: + break; + } + } + + // 2. Payload + if (m.type == Parser::MeasurementType::IMU) { + fill_be32(data, idx, m.acceleration_mss.x * UG_PER_MSS); + fill_be32(data, idx, m.acceleration_mss.y * UG_PER_MSS); + fill_be32(data, idx, m.acceleration_mss.z * UG_PER_MSS); + fill_be32(data, idx, m.angular_velocity_rads.x * UDEGS_PER_RADS); + fill_be32(data, idx, m.angular_velocity_rads.y * UDEGS_PER_RADS); + fill_be32(data, idx, m.angular_velocity_rads.z * UDEGS_PER_RADS); + fill_be16(data, idx, (int16_t)((m.temperature_degc - 20.0f) / 0.008f)); + fill_be16(data, idx, m.magnetic_field_mgauss.x); + fill_be16(data, idx, m.magnetic_field_mgauss.y); + fill_be16(data, idx, m.magnetic_field_mgauss.z); + fill_be32(data, idx, m.air_pressure_p * MHPA_PER_PA); + + } else if (m.type == Parser::MeasurementType::AHRS) { + fill_be32(data, idx, m.orientation.q1 * 1e6); + fill_be32(data, idx, m.orientation.q2 * 1e6); + fill_be32(data, idx, m.orientation.q3 * 1e6); + fill_be32(data, idx, m.orientation.q4 * 1e6); + + } else if (m.type == Parser::MeasurementType::INS) { + // --- 69-BYTE LAYOUT --- + + // 0-3: Sats (Big Endian of 2 shorts) + // GNSS2 is first Short, GNSS1 is second Short + // We put values in the LSB of each Short: [00][Count] + fill_u8(data, idx, 0); // GNSS2 Hi + fill_u8(data, idx, m.num_sats_gnss2); // GNSS2 Lo + fill_u8(data, idx, 0); // GNSS1 Hi + fill_u8(data, idx, m.num_sats_gnss1); // GNSS1 Lo + + // 4-7: Flags + fill_be32(data, idx, m.error_flags); + + // 8: Valid + fill_u8(data, idx, m.sensor_valid); + + // 9-32: Nav Data + fill_be32(data, idx, m.location.lat); + fill_be32(data, idx, m.location.lng); + + // Velocity N, E, D + fill_be32(data, idx, m.velocity_ned.x * 1000); + fill_be32(data, idx, m.velocity_ned.y * 1000); + fill_be32(data, idx, m.velocity_ned.z * 1000); + + // Altitude + fill_be32(data, idx, m.location.alt * 10); // cm -> mm + + // 33: Alignment + fill_u8(data, idx, m.alignment_status); + + // 34-37: iTOW + fill_be32(data, idx, m.time_itow_ms); + + // 38-39: GNSS Fix (Mask 5 -> 2 Bytes) + // Wire: [GNSS2][GNSS1] + fill_u8(data, idx, m.gnss2_fix); + fill_u8(data, idx, m.gnss1_fix); + + // 40-43: UTC Date (Mask F -> 4 Bytes) + // Wire: [Y_MSB][Y_LSB][Pad][Month] + fill_u8(data, idx, (m.year >> 8) & 0xFF); + fill_u8(data, idx, m.year & 0xFF); + fill_u8(data, idx, 0); // Pad + fill_u8(data, idx, m.month); + + // 44: UTC Time (Mask 8 -> 1 Byte) + // Wire: [Day] + fill_u8(data, idx, m.day); + + // 45-68: Accuracy (6 x 4 Bytes) + // Order: Lat, Lon, VelN, VelE, VelD, PosD + fill_be32(data, idx, m.pos_accuracy.x * 1000); // Lat Acc + fill_be32(data, idx, m.pos_accuracy.y * 1000); // Lon Acc + fill_be32(data, idx, m.vel_accuracy.x * 1000); // Vel N + fill_be32(data, idx, m.vel_accuracy.y * 1000); // Vel E + fill_be32(data, idx, m.vel_accuracy.z * 1000); // Vel D + fill_be32(data, idx, m.pos_accuracy.z * 1000); // Pos D + } + + // 4. CRC + uint8_t checksum = 0; + for (size_t i = 1; i < idx; ++i) { + checksum ^= data[i]; + } + data[idx++] = checksum; + + data_length = idx; +} + +// --- DEFAULT FACTORY --- +static Measurement default_measurement(Parser::MeasurementType type) +{ + Measurement m = {}; + m.type = type; + if (type == Parser::MeasurementType::IMU) { + m.acceleration_mss.x = 4.0f; + m.acceleration_mss.y = 5.0f; + m.acceleration_mss.z = 6.0f; + m.angular_velocity_rads.x = 0.1f; + m.angular_velocity_rads.y = 0.2f; + m.angular_velocity_rads.z = 0.3f; + m.temperature_degc = 56.0f; + m.magnetic_field_mgauss.x = 31.0f; + m.magnetic_field_mgauss.y = 41.0f; + m.magnetic_field_mgauss.z = 51.0f; + m.air_pressure_p = 1235.0f; + } + if (type == Parser::MeasurementType::INS) { + m.location.lat = 590000000; + m.location.lng = 180000000; + m.location.alt = 5000; + m.velocity_ned = Vector3f(1, 0, 0); + + m.pos_accuracy = Vector3f(0.5f, 0.5f, 0.8f); + m.vel_accuracy = Vector3f(0.1f, 0.1f, 0.1f); + + m.alignment_status = 1; + m.gnss1_fix = 3; + m.gnss2_fix = 0; + m.num_sats_gnss1 = 12; + m.time_itow_ms = 1000; + m.year = 2025; + m.month = 12; + m.day = 10; + m.sensor_valid = true; + } + return m; +} +static bool cmp_packages(Measurement& in, Parser::Measurement& out) +{ + if (in.type != out.type) { + return false; + } + // + + // + switch (in.type) { + case Parser::MeasurementType::IMU: + if (!is_equal(in.acceleration_mss[0], out.acceleration_mss[0], 0.00001f) || + !is_equal(in.acceleration_mss[1], out.acceleration_mss[1], 0.00001f) || + !is_equal(in.acceleration_mss[2], out.acceleration_mss[2], 0.00001f) || + !is_equal(in.angular_velocity_rads[0], out.angular_velocity_rads[0], 0.000001f) || + !is_equal(in.angular_velocity_rads[1], out.angular_velocity_rads[1], 0.000001f) || + !is_equal(in.angular_velocity_rads[2], out.angular_velocity_rads[2], 0.000001f) || + !is_equal(in.magnetic_field_mgauss[0], out.magnetic_field_mgauss[0]) || + !is_equal(in.magnetic_field_mgauss[1], out.magnetic_field_mgauss[1]) || + !is_equal(in.magnetic_field_mgauss[2], out.magnetic_field_mgauss[2]) || + !is_equal(in.temperature_degc, out.temperature_degc) || + !is_equal(in.air_pressure_p, out.air_pressure_p)) { + return false; + } + break; + case Parser::MeasurementType::AHRS: + if (!is_equal(in.orientation.q1, out.orientation.q1) || + !is_equal(in.orientation.q2, out.orientation.q2) || + !is_equal(in.orientation.q3, out.orientation.q3) || + !is_equal(in.orientation.q4, out.orientation.q4)) { + return false; + } + break; + case Parser::MeasurementType::INS: + if (in.location.lat != out.location.lat || + in.location.lng != out.location.lng || + in.location.alt != out.location.alt || + !is_equal(in.velocity_ned[0], out.velocity_ned[0]) || + !is_equal(in.velocity_ned[1], out.velocity_ned[1]) || + !is_equal(in.velocity_ned[2], out.velocity_ned[2]) || + !is_equal(in.pos_accuracy[0], out.pos_accuracy[0]) || + !is_equal(in.pos_accuracy[1], out.pos_accuracy[1]) || + !is_equal(in.pos_accuracy[2], out.pos_accuracy[2]) || + !is_equal(in.vel_accuracy[0], out.vel_accuracy[0]) || + !is_equal(in.vel_accuracy[1], out.vel_accuracy[1]) || + !is_equal(in.vel_accuracy[2], out.vel_accuracy[2]) || + in.alignment_status != out.alignment_status || + in.gnss1_fix != out.gnss1_fix || + in.gnss2_fix != out.gnss2_fix || + in.num_sats_gnss1 != out.num_sats_gnss1 || + in.num_sats_gnss2 != out.num_sats_gnss2 || + in.time_itow_ms != out.time_itow_ms || + out.gps_week != 2396 || + in.error_flags != out.error_flags || + in.sensor_valid != out.sensor_valid) { + return false; + } + break; + default: + return false; + } + return true; +} + +// --------------------------------------------------------------------------- +// LEGACY MODE TESTS +// --------------------------------------------------------------------------- + +TEST(SensAItionParser, Legacy_IMU_HappyPath) +{ + Parser parser(Parser::ConfigMode::IMU); + auto in = default_measurement(Parser::MeasurementType::IMU); + uint8_t buffer[100]; + size_t len = 100; + fill_simulated_packet(buffer, len, in, Parser::ConfigMode::IMU); + EXPECT_EQ(len, 38u); + + Parser::Measurement meas; + const auto parsed_bytes = parser.parse_stream(buffer, len, meas); + + EXPECT_EQ(parsed_bytes, 38u); + EXPECT_EQ(meas.type, Parser::MeasurementType::IMU); + EXPECT_TRUE(cmp_packages(in, meas)); +} + +TEST(SensAItionParser, Legacy_RejectsInvalidChecksum) +{ + Parser parser(Parser::ConfigMode::IMU); + auto in = default_measurement(Parser::MeasurementType::IMU); + uint8_t buffer[100]; + size_t len = 100; + fill_simulated_packet(buffer, len, in, Parser::ConfigMode::IMU); + buffer[len - 1] += 1; + uint32_t err_start = parser.get_parse_errors(); + + Parser::Measurement meas; + parser.parse_stream(buffer, len, meas); + + EXPECT_GT(parser.get_parse_errors(), err_start); +} + +TEST(SensAItionParser, Legacy_RejectsTooSmallBuffer) +{ + Parser parser(Parser::ConfigMode::IMU); + auto in = default_measurement(Parser::MeasurementType::IMU); + uint8_t buffer[100]; + size_t len = 100; + fill_simulated_packet(buffer, len, in, Parser::ConfigMode::IMU); + uint32_t valid_start = parser.get_valid_packets(); + + Parser::Measurement meas; + parser.parse_stream(buffer, len - 5, meas); + + EXPECT_EQ(parser.get_valid_packets(), valid_start); +} + +TEST(SensAItionParser, Legacy_ValidPacketsCount) +{ + Parser parser(Parser::ConfigMode::IMU); + auto in = default_measurement(Parser::MeasurementType::IMU); + uint8_t buffer[100]; + size_t len = 100; + fill_simulated_packet(buffer, len, in, Parser::ConfigMode::IMU); + uint32_t valid_start = parser.get_valid_packets(); + + Parser::Measurement meas; + for (int i = 0; i < 5; i++) { + parser.parse_stream(buffer, len, meas); + } + + EXPECT_EQ(parser.get_valid_packets(), valid_start + 5); +} + +TEST(SensAItionParser, Legacy_FalseHeaderInPayload) +{ + Parser parser(Parser::ConfigMode::IMU); + auto in = default_measurement(Parser::MeasurementType::IMU); + uint8_t packet[38]; + size_t len = 38; + fill_simulated_packet(packet, len, in, Parser::ConfigMode::IMU); + packet[5] = 0xFA; + uint8_t checksum = 0; + for (size_t i = 1; i < len - 1; ++i) { + checksum ^= packet[i]; + } + packet[len - 1] = checksum; + uint32_t start_valid = parser.get_valid_packets(); + + Parser::Measurement meas; + for (size_t i = 0; i < len; i++) { + parser.parse_stream(&packet[i], 1, meas); + } + + EXPECT_EQ(parser.get_valid_packets(), start_valid + 1); +} + +TEST(SensAItionParser, Legacy_FragmentedHeaderRecovery) +{ + Parser parser(Parser::ConfigMode::IMU); + auto in = default_measurement(Parser::MeasurementType::IMU); + uint8_t valid[38]; + size_t len = 38; + fill_simulated_packet(valid, len, in, Parser::ConfigMode::IMU); + uint8_t stream[120]; + size_t slen = 0; + stream[slen++] = 0xFA; stream[slen++] = 0x00; + memcpy(&stream[slen], valid, 38); + slen += 38; + memcpy(&stream[slen], valid, 38); + slen += 38; + uint32_t start_valid = parser.get_valid_packets(); + + Parser::Measurement meas; + for (size_t i = 0; i < slen; i++) { + parser.parse_stream(&stream[i], 1, meas); + } + + EXPECT_GE(parser.get_valid_packets() - start_valid, 1u); +} + +// --------------------------------------------------------------------------- +// INTERLEAVED MODE TESTS +// --------------------------------------------------------------------------- + +TEST(SensAItionParser, Interleaved_IMU_HappyPath) +{ + Parser parser(Parser::ConfigMode::INTERLEAVED_INS); + auto in = default_measurement(Parser::MeasurementType::IMU); + in.acceleration_mss.x = 2.5f; + uint8_t buffer[64]; + size_t len = 64; + fill_simulated_packet(buffer, len, in, Parser::ConfigMode::INTERLEAVED_INS); + EXPECT_EQ(len, 39u); + + Parser::Measurement meas; + const auto parsed_bytes = parser.parse_stream(buffer, len, meas); + + EXPECT_EQ(parsed_bytes, 39u); + EXPECT_EQ(meas.type, Parser::MeasurementType::IMU); + EXPECT_NEAR(meas.acceleration_mss.x, 2.5f, 0.01f); + EXPECT_TRUE(cmp_packages(in, meas)); +} + +TEST(SensAItionParser, Interleaved_INS_HappyPath) +{ + Parser parser(Parser::ConfigMode::INTERLEAVED_INS); + auto in = default_measurement(Parser::MeasurementType::INS); + uint8_t buffer[100]; + size_t len = 100; + fill_simulated_packet(buffer, len, in, Parser::ConfigMode::INTERLEAVED_INS); + EXPECT_EQ(len, 72u); // Verified + + Parser::Measurement meas; + parser.parse_stream(buffer, len, meas); + + EXPECT_EQ(meas.type, Parser::MeasurementType::INS); + EXPECT_TRUE(cmp_packages(in, meas)); +} + +TEST(SensAItionParser, Interleaved_InvalidID) +{ + Parser parser(Parser::ConfigMode::INTERLEAVED_INS); + uint8_t bad[] = { 0xFA, 0x99, 0x00, 0x00 }; + uint32_t start_err = parser.get_parse_errors(); + + Parser::Measurement meas; + parser.parse_stream(bad, 4, meas); + + EXPECT_GT(parser.get_parse_errors(), start_err); +} + +TEST(SensAItionParser, Handle_Interleaved_Packets) +{ + Parser parser(Parser::ConfigMode::INTERLEAVED_INS); + + // Create a buffer with one INS and IMU packet back-to-back + uint8_t stream[200]; + size_t len = 0; + + // Create INS Packet + auto m_ins = default_measurement(Parser::MeasurementType::INS); + m_ins.location.lat = 123456789; // Unique marker + size_t ins_len = 0; + fill_simulated_packet(&stream[0], ins_len, m_ins, Parser::ConfigMode::INTERLEAVED_INS); + len += ins_len; + + // Create IMU Packet immediately after + auto m_imu = default_measurement(Parser::MeasurementType::IMU); + m_imu.acceleration_mss.z = -15.0f; // Unique marker + size_t imu_len = 0; + fill_simulated_packet(&stream[len], imu_len, m_imu, Parser::ConfigMode::INTERLEAVED_INS); + len += imu_len; + + // Parse first packet + Parser::Measurement meas; + auto parsed_bytes = parser.parse_stream(stream, len, meas); + + EXPECT_EQ(parsed_bytes, ins_len); + EXPECT_EQ(meas.type, Parser::MeasurementType::INS); + EXPECT_TRUE(cmp_packages(m_ins, meas)); + + // Parse second packet + parsed_bytes = parser.parse_stream(&stream[parsed_bytes], len - parsed_bytes, meas); + EXPECT_EQ(parsed_bytes, imu_len); + EXPECT_EQ(meas.type, Parser::MeasurementType::IMU); + EXPECT_TRUE(cmp_packages(m_imu, meas)); +} + +TEST(SensAItionParser, Interleaved_MixedStream_Transitions) +{ + Parser parser(Parser::ConfigMode::INTERLEAVED_INS); + auto m_imu = default_measurement(Parser::MeasurementType::IMU); + auto m_ahrs = default_measurement(Parser::MeasurementType::AHRS); + auto m_ins = default_measurement(Parser::MeasurementType::INS); + uint8_t stream[200]; + size_t len = 0; + size_t part_len; + + part_len = 200 - len; + fill_simulated_packet(&stream[len], part_len, m_imu, Parser::ConfigMode::INTERLEAVED_INS); + len += part_len; + part_len = 200 - len; + fill_simulated_packet(&stream[len], part_len, m_ahrs, Parser::ConfigMode::INTERLEAVED_INS); + len += part_len; + part_len = 200 - len; + fill_simulated_packet(&stream[len], part_len, m_ins, Parser::ConfigMode::INTERLEAVED_INS); + len += part_len; + + Parser::Measurement meas; + auto parsed_bytes = parser.parse_stream(stream, len, meas); + EXPECT_TRUE(cmp_packages(m_imu, meas)); + + parsed_bytes += parser.parse_stream(&stream[parsed_bytes], len - parsed_bytes, meas); + EXPECT_TRUE(cmp_packages(m_ahrs, meas)); + + parsed_bytes += parser.parse_stream(&stream[parsed_bytes], len - parsed_bytes, meas); + EXPECT_EQ(parsed_bytes, len); + EXPECT_TRUE(cmp_packages(m_ins, meas)); +} + +TEST(SensAItionParser, Interleaved_INS_Fragmentation) +{ + Parser parser(Parser::ConfigMode::INTERLEAVED_INS); + auto in = default_measurement(Parser::MeasurementType::INS); + uint8_t packet[100]; + size_t len = 100; + fill_simulated_packet(packet, len, in, Parser::ConfigMode::INTERLEAVED_INS); + + Parser::Measurement meas; + // Parse all but one byte + auto parsed_bytes = parser.parse_stream(packet, len - 1, meas); + EXPECT_EQ(meas.type, Parser::MeasurementType::UNINITIALIZED); + + // Then parse the finishing byte of the message + parsed_bytes += parser.parse_stream(&packet[parsed_bytes], len - parsed_bytes, meas); + EXPECT_EQ(meas.type, Parser::MeasurementType::INS); +} + +TEST(SensAItionParser, Interleaved_Data_StressTest) +{ + Parser parser(Parser::ConfigMode::INTERLEAVED_INS); + auto in = default_measurement(Parser::MeasurementType::INS); + in.location.lat = -593293230; + in.velocity_ned.x = -15.5f; + uint8_t buffer[100]; + size_t len = 100; + fill_simulated_packet(buffer, len, in, Parser::ConfigMode::INTERLEAVED_INS); + + Parser::Measurement meas; + parser.parse_stream(buffer, len, meas); + EXPECT_EQ(meas.location.lat, -593293230); + EXPECT_NEAR(meas.velocity_ned.x, -15.5f, 0.01f); +} + +TEST(SensAItionParser, Interleaved_NoiseRecovery) +{ + Parser parser(Parser::ConfigMode::INTERLEAVED_INS); + auto in = default_measurement(Parser::MeasurementType::INS); + uint8_t valid[100]; + size_t len = 100; + fill_simulated_packet(valid, len, in, Parser::ConfigMode::INTERLEAVED_INS); + uint8_t stream[200]; + memset(stream, 0xEE, 20); + memcpy(&stream[20], valid, len); + + Parser::Measurement meas; + parser.parse_stream(stream, len + 20, meas); + + EXPECT_EQ(meas.type, Parser::MeasurementType::INS); +} + +TEST(SensAItionParser, Legacy_IMU_PartialStream) +{ + Parser parser(Parser::ConfigMode::IMU); + auto in = default_measurement(Parser::MeasurementType::IMU); + uint8_t packet[100]; + memset(packet, 0x00, 100); // Ensure known content after the packet + size_t chunk_size = 5; + size_t len = 20 * chunk_size; + fill_simulated_packet(packet, len, in, Parser::ConfigMode::IMU); + + Parser::Measurement meas; + for (size_t i = 0; i < len; i += chunk_size) { + parser.parse_stream(&packet[i], chunk_size, meas); + } + + EXPECT_EQ(meas.type, Parser::MeasurementType::IMU); +} + +TEST(SensAItionParser, Interleaved_INS_FullFieldVerification) +{ + Parser parser(Parser::ConfigMode::INTERLEAVED_INS); + auto in = default_measurement(Parser::MeasurementType::INS); + + // Setup Distinct Values + in.num_sats_gnss1 = 22; + in.num_sats_gnss2 = 18; + in.error_flags = 0xCAFEBABE; + in.sensor_valid = true; + in.location.lat = -593293230; + in.location.lng = 180685810; + in.location.alt = 1500; // cm + in.velocity_ned = Vector3f(-5.5f, 2.2f, 0.5f); + in.alignment_status = 1; + in.time_itow_ms = 987654321; + in.gnss1_fix = 3; + in.gnss2_fix = 2; + + // Time -> Week Calculation Check + // 2025-12-10 is GPS Week 2396 + in.year = 2025; + in.month = 12; + in.day = 10; + uint16_t expected_week = 2396; + + // Vector Accuracy + in.pos_accuracy = Vector3f(0.1f, 0.2f, 0.3f); // Lat, Lon, Alt + in.vel_accuracy = Vector3f(0.4f, 0.5f, 0.6f); // N, E, D + + // Generate & Parse + uint8_t buffer[100]; + size_t len = 100; + fill_simulated_packet(buffer, len, in, Parser::ConfigMode::INTERLEAVED_INS); + + Parser::Measurement meas; + parser.parse_stream(buffer, len, meas); + + // VERIFICATION + auto& out = meas; + EXPECT_EQ(out.type, Parser::MeasurementType::INS); + EXPECT_EQ(out.num_sats_gnss1, in.num_sats_gnss1); + EXPECT_EQ(out.num_sats_gnss2, in.num_sats_gnss2); + EXPECT_EQ(out.error_flags, in.error_flags); + EXPECT_EQ(out.sensor_valid, in.sensor_valid); + + EXPECT_EQ(out.location.lat, in.location.lat); + EXPECT_EQ(out.location.lng, in.location.lng); + EXPECT_EQ(out.location.alt, in.location.alt); + EXPECT_NEAR(out.velocity_ned.x, in.velocity_ned.x, 0.001f); + + EXPECT_EQ(out.alignment_status, in.alignment_status); + EXPECT_EQ(out.time_itow_ms, in.time_itow_ms); + EXPECT_EQ(out.gnss1_fix, in.gnss1_fix); + EXPECT_EQ(out.gnss2_fix, in.gnss2_fix); + + // Week Verify + EXPECT_EQ(out.gps_week, expected_week) << "GPS Week Calc Failed"; + + // Vector Verify + EXPECT_NEAR(out.pos_accuracy.x, in.pos_accuracy.x, 0.001f); + EXPECT_NEAR(out.pos_accuracy.y, in.pos_accuracy.y, 0.001f); + EXPECT_NEAR(out.pos_accuracy.z, in.pos_accuracy.z, 0.001f); + + EXPECT_NEAR(out.vel_accuracy.x, in.vel_accuracy.x, 0.001f); + EXPECT_NEAR(out.vel_accuracy.y, in.vel_accuracy.y, 0.001f); + EXPECT_NEAR(out.vel_accuracy.z, in.vel_accuracy.z, 0.001f); +} + +TEST(SensAItionParser, GPS_Week_Calculation_EdgeCases) +{ + Parser parser(Parser::ConfigMode::INTERLEAVED_INS); + auto in = default_measurement(Parser::MeasurementType::INS); + uint8_t buffer[100]; + size_t len = 100; + Parser::Measurement meas; + + // CASE 1: Leap Year (Feb 29 2024) + // 2024-02-29 -> Week 2303 + in.year = 2024; in.month = 2; in.day = 29; + fill_simulated_packet(buffer, len, in, Parser::ConfigMode::INTERLEAVED_INS); + parser.parse_stream(buffer, len, meas); + EXPECT_EQ(meas.gps_week, 2303) << "Failed Leap Year Calc"; + + // CASE 2: No Fix -> Week Should be 0 + in.gnss1_fix = 0; // Lost fix + in.year = 2025; in.month = 1; in.day = 1; + fill_simulated_packet(buffer, len, in, Parser::ConfigMode::INTERLEAVED_INS); + parser.parse_stream(buffer, len, meas); + EXPECT_EQ(meas.gps_week, 0) << "Week should be 0 when fix is lost"; + + // CASE 3: Year 2000 (translates to 00 in BCD format) should be correctly handled + in = default_measurement(Parser::MeasurementType::INS); + in.year = 2000; in.month = 1; in.day = 31; + fill_simulated_packet(buffer, len, in, Parser::ConfigMode::INTERLEAVED_INS); + parser.parse_stream(buffer, len, meas); + EXPECT_EQ(meas.gps_week, 1047) << "Failed converting the year 2000 to GPS week"; +} + +AP_GTEST_MAIN() diff --git a/libraries/AP_ExternalAHRS/tests/wscript b/libraries/AP_ExternalAHRS/tests/wscript new file mode 100755 index 0000000000000..5f4134d0ccb37 --- /dev/null +++ b/libraries/AP_ExternalAHRS/tests/wscript @@ -0,0 +1,6 @@ +#!/usr/bin/env python3 + +def build(bld): + bld.ap_find_tests( + use='ap', + ) diff --git a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp index ab5d5675e19ca..a17f73a224b5c 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_FlashMemory_LittleFS.cpp @@ -618,6 +618,7 @@ void AP_Filesystem_FlashMemory_LittleFS::mark_dead() #define JEDEC_ID_CYPRESS_S25FL064L 0x016017 #define JEDEC_ID_CYPRESS_S25FL128L 0x016018 #define JEDEC_ID_GIGA_GD25Q16E 0xC84015 +#define JEDEC_ID_ZBIT_ZB25VQ128 0x5E4018 /* Hardware-specific constants */ @@ -788,6 +789,7 @@ uint32_t AP_Filesystem_FlashMemory_LittleFS::find_block_size_and_count() { case JEDEC_ID_WINBOND_W25Q128: case JEDEC_ID_WINBOND_W25Q128_2: case JEDEC_ID_CYPRESS_S25FL128L: + case JEDEC_ID_ZBIT_ZB25VQ128: block_count = 256; /* 16MiB */ break; diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 380730a9af679..546513714d4c0 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -52,11 +52,18 @@ AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GP state.have_vertical_accuracy = false; } -/** - fill in time_week_ms and time_week from BCD date and time components - assumes MTK19 millisecond form of bcd_time +/* + get the last time of week in ms */ -void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds) +uint32_t AP_GPS_Backend::get_last_itow_ms(void) const +{ + if (!_have_itow) { + return state.time_week_ms; + } + return (_pseudo_itow_delta_ms == 0)?(_last_itow_ms):((_pseudo_itow/1000ULL) + _pseudo_itow_delta_ms); +} + +void AP_GPS_Backend::BCD_to_gps_time(uint32_t bcd_date, uint32_t bcd_time_ms, uint16_t& gps_week, uint32_t& gps_time_ms) { struct tm tm {}; @@ -64,7 +71,7 @@ void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds) tm.tm_mon = ((bcd_date / 100U) % 100U)-1; tm.tm_mday = bcd_date / 10000U; - uint32_t v = bcd_milliseconds; + uint32_t v = bcd_time_ms; uint16_t msec = v % 1000U; v /= 1000U; tm.tm_sec = v % 100U; v /= 100U; tm.tm_min = v % 100U; v /= 100U; @@ -79,20 +86,9 @@ void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds) uint32_t ret = unix_time + leap_seconds_unix - unix_to_GPS_secs; // get GPS week and time - state.time_week = ret / AP_SEC_PER_WEEK; - state.time_week_ms = (ret % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC; - state.time_week_ms += msec; -} - -/* - get the last time of week in ms - */ -uint32_t AP_GPS_Backend::get_last_itow_ms(void) const -{ - if (!_have_itow) { - return state.time_week_ms; - } - return (_pseudo_itow_delta_ms == 0)?(_last_itow_ms):((_pseudo_itow/1000ULL) + _pseudo_itow_delta_ms); + gps_week = ret / AP_SEC_PER_WEEK; + gps_time_ms = (ret % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC; + gps_time_ms += msec; } /* diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 0266431fd4ced..f779b36857d52 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -117,6 +117,9 @@ class AP_GPS_Backend return gps.option_set(option); } + // Convert BCD date (DDMMYY) and time (MTK19 millisecond form) to GPS week and time + static void BCD_to_gps_time(uint32_t bcd_date, uint32_t bcd_time_ms, uint16_t& gps_week, uint32_t& gps_time_ms); + protected: AP_HAL::UARTDriver *port; ///< UART we are attached to AP_GPS &gps; ///< access to frontend (for parameters) @@ -142,7 +145,10 @@ class AP_GPS_Backend fill in time_week_ms and time_week from BCD date and time components assumes MTK19 millisecond form of bcd_time */ - void make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds); + void make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds) + { + BCD_to_gps_time(bcd_date, bcd_milliseconds, state.time_week, state.time_week_ms); + } void _detection_message(char *buffer, uint8_t buflen) const; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/hwdef.dat index cc7b34d33762a..da5ec7fba492b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Atlas-Control/hwdef.dat @@ -19,7 +19,7 @@ STM32_ST_USE_TIMER 2 FLASH_RESERVE_START_KB 128 # order of UARTs (and USB) -SERIAL_ORDER OTG1 USART2 USART6 USART1 UART4 EMPTY UART7 OTG2 +SERIAL_ORDER OTG1 USART2 USART6 USART1 UART4 UART8 UART7 OTG2 # now we define the pins that USB is connected on PA11 OTG_FS_DM OTG1 @@ -148,14 +148,18 @@ PG8 USART6_RTS USART6 PF6 UART7_RX UART7 NODMA PE8 UART7_TX UART7 NODMA +# SBUS, DSM port +PE0 UART8_RX UART8 +PE1 UART8_TX UART8 + # PWM AUX channels -PH10 TIM5_CH1 TIM5 PWM(1) GPIO(50) BIDIR +PH10 TIM5_CH1 TIM5 PWM(1) GPIO(50) PH11 TIM5_CH2 TIM5 PWM(2) GPIO(51) -PH12 TIM5_CH3 TIM5 PWM(3) GPIO(52) BIDIR +PH12 TIM5_CH3 TIM5 PWM(3) GPIO(52) PI0 TIM5_CH4 TIM5 PWM(4) GPIO(53) -PD12 TIM4_CH1 TIM4 PWM(5) GPIO(54) BIDIR +PD12 TIM4_CH1 TIM4 PWM(5) GPIO(54) PD13 TIM4_CH2 TIM4 PWM(6) GPIO(55) -PD14 TIM4_CH3 TIM4 PWM(7) GPIO(56) BIDIR +PD14 TIM4_CH3 TIM4 PWM(7) GPIO(56) PD15 TIM4_CH4 TIM4 PWM(8) GPIO(57) PE9 TIM1_CH1 TIM1 PWM(9) GPIO(58) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HWH7/HWH7.png b/libraries/AP_HAL_ChibiOS/hwdef/HWH7/HWH7.png new file mode 100644 index 0000000000000..2f8dd1cd00730 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/HWH7/HWH7.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HWH7/README.md b/libraries/AP_HAL_ChibiOS/hwdef/HWH7/README.md new file mode 100644 index 0000000000000..26cd745c4f546 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/HWH7/README.md @@ -0,0 +1,125 @@ +# HWH7 Flight Controller (RTF-H7-FC-8S) + +The **HWH7** is a flight controller designed and produced by HW. + +## Features + +- MCU: STM32H743, 480 MHz, 2 MB Flash. +- IMUs: Dual SPI IMU support (two sensor positions). Default/probed: ICM-42688P .Alternative/probed: BMI270. +- Barometer: I2C barometer (SPL06 or DPS310 at 0x76). +- OSD: AT7456E (SPI) + optional MSP DisplayPort (for DJI/HD systems). +- Logging: microSD (SDMMC1 4-bit). +- PWM Outputs: 12 total. Outputs 1–8 support bidirectional DShot. +- ESC connectivity: up to two ESCs via 8‑pin ribbon connectors (board wiring). +- Power (board hardware): 4S–8S LiPo input (12–35.6 V), on-board 5V/3A and 12V/3A BECs. +- Other hardware (board option): 512Mb blackbox flash. + +## Pinout / Connectors + +![image](HWH7.png) + +## Serial Ports (UARTs) + +Serial ordering (ArduPilot): `SERIAL0..SERIAL8` = `OTG1, USART1, USART2, USART3, UART4, UART5, USART6, UART7, UART8` + +| Typical Label | ArduPilot Port | HW Port | TX Pin | RX Pin | Default Use | +| --- | --- | --- | --- | --- | --- | +| USB Type‑C | SERIAL0 | OTG1 | PA11(DM) | PA12(DP) | MAVLink over USB | +| UART1 (TX1/RX1) | SERIAL1 | USART1 | PA9 | PA10 | RC Input (SBUS/DSM/CRSF via serial RCIN) | +| UART2 (TX2/RX2) | SERIAL2 | USART2 | PA2 | PA3 | DisplayPort | +| UART3 | SERIAL3 | USART3 | PD8 | PD9 | (connected to internal BT module, not currently usable by ArduPilot) | +| UART4 (TX4/RX4) | SERIAL4 | UART4 | PD1 | PD0 | GPS | +| UART5 | SERIAL5 | UART5 | PB6 | PB5 | User | +| UART6 | SERIAL6 | USART6 | PC6 | PC7 | User | +| UART7 | SERIAL7 | UART7 | PE8 | PE7 | User | +| UART8 | SERIAL8 | UART8 | PE1 | PE0 | ESC Telemetry | + +## PWM Outputs + +The HWH7 supports up to 12 PWM or DShot outputs. These outputs are organized into 5 groups based on the MCU timers: + +| Output | MCU Pin | Timer | Notes | +|---:|---|---|---| +| 1 | PE14 | TIM1_CH4 | bidirectional DShot capable | +| 2 | PE13 | TIM1_CH3 | bidirectional DShot capable | +| 3 | PE11 | TIM1_CH2 | bidirectional DShot capable | +| 4 | PE9  | TIM1_CH1 | bidirectional DShot capable | +| 5 | PD12 | TIM4_CH1 | bidirectional DShot capable | +| 6 | PD13 | TIM4_CH2 | bidirectional DShot capable | +| 7 | PD14 | TIM4_CH3 | bidirectional DShot capable | +| 8 | PD15 | TIM4_CH4 | bidirectional DShot capable | +| 9 | PE5  | TIM15_CH1 | standard PWM | +| 10 | PE6 | TIM15_CH2 | standard PWM | +| 11 | PA0 | TIM2_CH1 | “LED pad” (can be used as an output) | +| 12 | PA1 | TIM5_CH2 | buzzer output (HAL_BUZZER_PIN) | + +- **Group 1:** PWM 1, 2, 3, 4 (TIM1) +- **Group 2:** PWM 5, 6, 7, 8 (TIM4) +- **Group 3:** PWM 9, 10 (TIM15) +- **Group 4:** PWM 11 (TIM2) +- **Group 5:** PWM 12 (TIM5) + +- **Rate and Protocol Consistency:** All channels within the same group **must** use the same output rate and protocol. If any channel in a group is configured for DShot, all other channels in that group must also be configured for DShot. +- **Bi-directional DShot:** Support is available for **PWM 1 through 8** (Groups 1 and 2). +- **Timer Grouping Warning:** Be cautious when mixing different types of servos, ESCs, or other timer-based outputs. For example, if PWM 9 is used for a standard servo and PWM 10 is used for a NeoPixel LED, they will conflict because they share **Group 3 (TIM15)**. In typical setups, PE5 and PE6 are used together as servo outputs by default. + +## Battery Monitor (ADC) + +The board has a built-in voltage sensor and external current sensor input. + +- **BATT_MONITOR** = 4 (Analog Voltage and Current) +- **BATT_VOLT_PIN** = 10 (PC0) +- **BATT_CURR_PIN** = 11 (PC1) +- **BATT_VOLT_MULT** = 11.0 +- **BATT_AMP_PERVLT** = 5.882 (or specify your sensor sensitivity like 170mV/A) + +## User GPIOs + +- GPIO 62 (PD10): Status LED. +- GPIO 80 (PE2) controls the camera output to the connectors marked "CAM1" and "CAM2". Setting this GPIO high switches the video output from CAM1 to CAM2. By default, RELAY1 is configured to control this pin and sets the GPIO high (CAM1). +- GPIO 81 (PE4): controls the VTX power output to the connectors marked "12V".Setting this GPIO low switches the video power off.By default, RELAY2 is configured to control this pin and sets the GPIO high (Power ON). +- GPIO 82 (PD10): This pin is active-low. The Bluetooth function is disabled by default when the pin is low. This feature is currently not supported in ArduPilot. + +## Compass + +The HWH7 does not have a builtin compass, but you can attach an external compass using I2C on the SDA and SCL pads. + +## ESC Telemetry + +The HWH7 board features two 8-pin ESC connectors. Telemetry from these connectors is routed to **UART8**. + +Note that due to hardware routing: + +- **ESC Connector 1** telemetry is connected to **UART8_RX (PE0)**. +- **ESC Connector 2** telemetry is connected to **UART8_TX (PE1)**. + +Because both signals share the same UART port on different pins, **simultaneous telemetry from both ESCs is not supported.** You must choose which ESC to receive telemetry from by configuring the `SERIAL8_OPTIONS` parameter: + +- **To use telemetry from ESC 1:** Set `SERIAL8_OPTIONS` to `0` (default). +- **To use telemetry from ESC 2:** Set `SERIAL8_OPTIONS` to `8` (SwapTXRX), which allows the flight controller to listen for telemetry on the TX8 pin. + +## OSD + +The HWH7 includes an integrated AT7456E analog OSD chip, accessible via the CAM1/2 and VTX pins. MSP DisplayPort OSD is also supported and enabled by default for use with digital video systems. + +## microSD Logging + +The board uses **SDMMC1** (4-bit) for microSD logging. + +The bootloader hwdef enables flashing firmware from microSD (`AP_BOOTLOADER_FLASH_FROM_SD_ENABLED`). + +## Board ID + +`AP_HW_HWH7` + +## Loading Firmware + +Initial firmware load can be done with DFU by plugging in USB with the bootloader button pressed. + +Firmware can be found on the `firmware server `__ in folders marked "HWH7" + +Then load the `*_with_bl.hex` firmware using a DFU tool. + +Once the initial firmware is loaded you can update the firmware using any ArduPilot ground station software. + +Updates should be done with the `*.apj` firmware files. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HWH7/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/HWH7/defaults.parm new file mode 100644 index 0000000000000..fc01c72698186 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/HWH7/defaults.parm @@ -0,0 +1,5 @@ +# setup for LEDs on TIM2chan1 +SERVO11_FUNCTION 120 + +#enable second OSD type +OSD_TYPE2 = 5 \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HWH7/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/HWH7/hwdef-bl.dat new file mode 100644 index 0000000000000..834c0f977ea48 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/HWH7/hwdef-bl.dat @@ -0,0 +1,67 @@ +# hw definition file for processing by chibios_pins.py +# for HWH7 hardware + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# board ID. See Tools/AP_Bootloader/board_types.txt +APJ_BOARD_ID AP_HW_HWH7 + +# crystal frequency +OSCILLATOR_HZ 8000000 + +# flash size +FLASH_SIZE_KB 2048 + +# 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 + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 USART6 + +PC6 USART6_TX USART6 +PC7 USART6_RX USART6 + +# PA10 IO-debug-console +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +# pins for SWD debugging +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# LEDs +PB2 LED_BOOTLOADER OUTPUT LOW +define HAL_LED_ON 0 + +# CS pins +PA4 IMU1_CS CS +PB12 IMU2_CS CS +PA15 MAX7456_CS CS + + + +# 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 +PC8 SDMMC1_D0 SDMMC1 +PC9 SDMMC1_D1 SDMMC1 +PC10 SDMMC1_D2 SDMMC1 +PC11 SDMMC1_D3 SDMMC1 +PC12 SDMMC1_CK SDMMC1 +PD2 SDMMC1_CMD SDMMC1 + +# enable FAT filesystem support (needs a microSD defined via SDMMC) +define HAL_OS_FATFS_IO 1 + +define AP_BOOTLOADER_FLASH_FROM_SD_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HWH7/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/HWH7/hwdef.dat new file mode 100644 index 0000000000000..c384391d160fc --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/HWH7/hwdef.dat @@ -0,0 +1,210 @@ +# hw definition file for processing by chibios_pins.py +# for HWH7 hardware + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# board ID. See Tools/AP_Bootloader/board_types.txt +APJ_BOARD_ID AP_HW_HWH7 + +# crystal frequency +OSCILLATOR_HZ 8000000 + +# system timer +STM32_ST_USE_TIMER 12 +define CH_CFG_ST_RESOLUTION 16 + +#disable safety button by default +define BOARD_SAFETY_ENABLE_DEFAULT 0 + +# flash size +FLASH_SIZE_KB 2048 +FLASH_RESERVE_START_KB 128 + +# MCU Clock +MCU_CLOCKRATE_MHZ 480 + +define HAL_STORAGE_SIZE 32768 +STORAGE_FLASH_PAGE 14 + +# default to all pins low to avoid ESD issues +DEFAULTGPIO OUTPUT LOW PULLDOWN + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 USART1 USART2 USART3 UART4 UART5 USART6 UART7 UART8 + +# USB +USB_STRING_MANUFACTURER "ArduPilot" +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +# SWD +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# SPI1 (ICM-42688 #1) +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 +PA4 IMU1_CS CS + +# SPI2 (ICM-42688 #2) +PD3 SPI2_SCK SPI2 +PC2 SPI2_MISO SPI2 +PC3 SPI2_MOSI SPI2 +PB12 IMU2_CS CS + +# SPI3 - AT7456E +PB3 SPI3_SCK SPI3 +PB4 SPI3_MISO SPI3 +PD6 SPI3_MOSI SPI3 +PA15 MAX7456_CS CS + +# only one I2C bus +I2C_ORDER I2C2 + +# I2C2 +PB10 I2C2_SCL I2C2 +PB11 I2C2_SDA I2C2 + +# barometers +BARO SPL06 I2C:0:0x76 +BARO DPS310 I2C:0:0x76 + +# Battery monitor (defaults from user) +define HAL_BATT_MONITOR_DEFAULT 4 +# VBAT on PC0 (ADC INP10), current on PC1 (ADC INP11) +PC0 BATT_VOLTAGE_SENS ADC1 SCALE(1) +PC1 BATT_CURRENT_SENS ADC1 SCALE(1) +define HAL_BATT_VOLT_PIN 10 +define HAL_BATT_CURR_PIN 11 +define HAL_BATT_VOLT_SCALE 11.0 # 11:1 divider +define HAL_BATT_CURR_SCALE 5.882 # A/V (170 mV/A -> 1 / 0.17) + +# LED +PB2 LED_RED OUTPUT LOW GPIO(62) +define AP_NOTIFY_GPIO_LED_1_ENABLED 1 +define AP_NOTIFY_GPIO_LED_1_PIN 62 + +# USART1 +PA9 USART1_TX USART1 +PA10 USART1_RX USART1 + +# USART2 +PA2 USART2_TX USART2 +PA3 USART2_RX USART2 + +# USART3 +PD8 USART3_TX USART3 +PD9 USART3_RX USART3 + +# UART4 +PD1 UART4_TX UART4 +PD0 UART4_RX UART4 + +# UART5 +PB6 UART5_TX UART5 +PB5 UART5_RX UART5 + +# USART6 +PC6 USART6_TX USART6 +PC7 USART6_RX USART6 + +# UART7 +PE8 UART7_TX UART7 +PE7 UART7_RX UART7 + +# UART8 +PE1 UART8_TX UART8 +PE0 UART8_RX UART8 + +# PWM output pins +PE14 TIM1_CH4 TIM1 PWM(1) GPIO(50) +PE13 TIM1_CH3 TIM1 PWM(2) GPIO(51) BIDIR +PE11 TIM1_CH2 TIM1 PWM(3) GPIO(52) +PE9 TIM1_CH1 TIM1 PWM(4) GPIO(53) BIDIR +PD12 TIM4_CH1 TIM4 PWM(5) GPIO(54) BIDIR +PD13 TIM4_CH2 TIM4 PWM(6) GPIO(55) +PD14 TIM4_CH3 TIM4 PWM(7) GPIO(56) BIDIR +PD15 TIM4_CH4 TIM4 PWM(8) GPIO(57) + +PE5 TIM15_CH1 TIM15 PWM(9) GPIO(58) +PE6 TIM15_CH2 TIM15 PWM(10) GPIO(59) + +# LED-pad +PA0 TIM2_CH1 TIM2 PWM(11) GPIO(60) +define DEFAULT_NTF_LED_TYPES 455 + +# BUZZER +PA1 TIM5_CH2 TIM5 PWM(12) GPIO(61) +define HAL_BUZZER_PIN 61 +define AP_NOTIFY_BUZZER_ENABLED 1 + +# microSD support +PC12 SDMMC1_CK SDMMC1 +PD2 SDMMC1_CMD SDMMC1 +PC8 SDMMC1_D0 SDMMC1 +PC9 SDMMC1_D1 SDMMC1 +PC10 SDMMC1_D2 SDMMC1 +PC11 SDMMC1_D3 SDMMC1 +define FATFS_HAL_DEVICE SDCD1 + +# SPI devices +SPIDEV bmi270_0 SPI1 DEVID1 IMU1_CS MODE3 1*MHZ 10*MHZ +SPIDEV icm42688_0 SPI1 DEVID2 IMU1_CS MODE3 2*MHZ 16*MHZ + +SPIDEV bmi270_1 SPI2 DEVID1 IMU2_CS MODE3 1*MHZ 10*MHZ +SPIDEV icm42688_1 SPI2 DEVID2 IMU2_CS MODE3 2*MHZ 16*MHZ + +SPIDEV osd SPI3 DEVID4 MAX7456_CS MODE0 10*MHZ 10*MHZ + +# DAM +DMA_NOSHARE SPI1* SPI2* + +# two IMUs +# ICM42688(BMI270) + ICM42688(BMI270) +IMU BMI270 SPI:bmi270_0 ROTATION_YAW_270 +IMU Invensensev3 SPI:icm42688_0 ROTATION_NONE + +IMU BMI270 SPI:bmi270_1 ROTATION_YAW_180 +IMU Invensensev3 SPI:icm42688_1 ROTATION_YAW_270 + +define HAL_DEFAULT_INS_FAST_SAMPLE 3 + +# compass +define ALLOW_ARM_NO_COMPASS +define AP_COMPASS_PROBING_ENABLED 1 +define HAL_I2C_INTERNAL_MASK 0 +define HAL_COMPASS_AUTO_ROT_DEFAULT 2 + +# enable FAT filesystem support (needs a microSD defined via SDMMC) +define HAL_OS_FATFS_IO 1 + +# setup for OSD +define OSD_ENABLED 1 +define HAL_OSD_TYPE_DEFAULT 1 +ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin + +# setup for BF migration +define HAL_FRAME_TYPE_DEFAULT 12 + +# User-defined buttons +# camera switching +PE2 CAM_SWITCH OUTPUT LOW GPIO(80) +define RELAY1_PIN_DEFAULT 80 + +# Image transmission power switch +PE4 VTX_POWER OUTPUT HIGH GPIO(81) +define RELAY2_PIN_DEFAULT 81 + +# Bluetooth control pins +PD10 USER_GPIO3 OUTPUT LOW GPIO(82) + +# Serial functions +define DEFAULT_SERIAL1_PROTOCOL SerialProtocol_RCIN +define DEFAULT_SERIAL2_PROTOCOL SerialProtocol_MSP_DisplayPort +define DEFAULT_SERIAL3_PROTOCOL SerialProtocol_None +define DEFAULT_SERIAL8_PROTOCOL SerialProtocol_ESCTelemetry + +# build ABIN for flash-from-bootloader support: +env BUILD_ABIN True diff --git a/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/README.md b/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/README.md index e8a1c08fb6770..1d54699a87556 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/README.md +++ b/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/README.md @@ -5,7 +5,7 @@ The TBS LUCID H7 is a flight controller produced by [TBS](https://www.team-black ## Features - MCU - STM32H743 32-bit processor running at 480 MHz -- IMU - Dual ICM42688 +- IMU - Dual ICM42688 (Dual MPU6000 on some v2 boards) - Barometer - DPS310 - OSD - AT7456E - microSD card slot diff --git a/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/hwdef.dat index 1bc535c8431fc..7369a9f0ce76e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/hwdef.dat @@ -193,6 +193,8 @@ STORAGE_FLASH_PAGE 14 # spi devices SPIDEV imu1 SPI1 DEVID1 IMU1_CS MODE3 2*MHZ 16*MHZ # Clock is 100Mhz so highest clock <= 24Mhz is 100Mhz/8 SPIDEV imu2 SPI4 DEVID1 IMU2_CS MODE3 2*MHZ 16*MHZ # Clock is 100Mhz so highest clock <= 24Mhz is 100Mhz/8 +SPIDEV mpu6000_1 SPI1 DEVID1 IMU1_CS MODE3 1*MHZ 4*MHZ +SPIDEV mpu6000_2 SPI4 DEVID1 IMU2_CS MODE3 1*MHZ 4*MHZ SPIDEV osd SPI2 DEVID4 MAX7456_CS MODE0 10*MHZ 10*MHZ # no built-in compass, but probe the i2c bus for all possible @@ -204,6 +206,8 @@ define HAL_COMPASS_AUTO_ROT_DEFAULT 2 IMU Invensensev3 SPI:imu1 ROTATION_YAW_270 IMU Invensensev3 SPI:imu2 ROTATION_YAW_180 +IMU Invensense SPI:mpu6000_1 ROTATION_ROLL_180 +IMU Invensense SPI:mpu6000_2 ROTATION_ROLL_180_YAW_270 define HAL_DEFAULT_INS_FAST_SAMPLE 3 # DPS310 integrated on I2C2 bus diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat index f3a7e34951b65..808a3655ffdf8 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat @@ -465,3 +465,6 @@ define AP_BATTERY_SMBUS_SOLO_ENABLED (AP_BATTERY_SMBUS_ENABLED && BOARD_FLASH_SI # produce this error if we are on a 1M board define BOARD_CHECK_F427_USE_1M "ERROR: 1M flash use fmuv2" + +# probe for external IST8310 compasses: +define AP_COMPASS_IST8310_EXTERNAL_BUS_PROBING_ENABLED 1 diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp index acb74cbe3e74c..3bc7d4936d1f3 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.cpp +++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp @@ -223,6 +223,32 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const sitl_model->set_volz(&_sitl->volz_sim); return &_sitl->volz_sim; #endif // AP_SIM_VOLZ_ENABLED +#if AP_SIM_SIYI_ZT30_ENABLED + } else if (streq(name, "siyi_zt30")) { + const auto siyi = NEW_NOTHROW SITL::Siyi_ZT30(); + sitl_model->add_gimbal_sim(*siyi); + return siyi; +#endif // AP_SIM_SIYI_ZT30_ENABLED +#if AP_SIM_TOPOTEK_ENABLED + } else if (streq(name, "topotek")) { + const auto topotek = NEW_NOTHROW SITL::Topotek(); + sitl_model->add_gimbal_sim(*topotek); + return topotek; +#endif // AP_SIM_TOPOTEK_ENABLED +#if AP_SIM_VIEWPRO_ENABLED + } else if (streq(name, "viewpro")) { + const auto viewpro = NEW_NOTHROW SITL::Viewpro(); + sitl_model->add_gimbal_sim(*viewpro); + return viewpro; +#endif // AP_SIM_VIEWPRO_ENABLED +#if AP_SIM_AVT_CM62_ENABLED + } else if (streq(name, "avt_cm62_gimbal")) { + static uint8_t mavlink_gimbal_count; + const auto avt_cm62 = NEW_NOTHROW SITL::AVT_CM62(); + avt_cm62->set_instance(mavlink_gimbal_count++); + sitl_model->add_gimbal_sim(*avt_cm62); + return avt_cm62; +#endif // AP_SIM_AVT_CM62_ENABLED } else if (streq(name, "megasquirt")) { if (efi_ms != nullptr) { AP_HAL::panic("Only one megasquirt at a time"); @@ -268,6 +294,19 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const inertiallabs = NEW_NOTHROW SITL::InertialLabs(); return inertiallabs; + } else if (streq(name, "SensAItion")) { + if (sensaition != nullptr) { + AP_HAL::panic("Only one SensAItion at a time"); + } + sensaition = NEW_NOTHROW SITL::SensAItion(false); + return sensaition; + } else if (streq(name, "SensAItionINS")) { + if (sensaition != nullptr) { + AP_HAL::panic("Only one SensAItion at a time"); + } + sensaition = NEW_NOTHROW SITL::SensAItion(true); + return sensaition; + #if AP_SIM_AIS_ENABLED } else if (streq(name, "AIS")) { if ((ais != nullptr) || (ais_replay != nullptr)) { @@ -399,6 +438,10 @@ void SITL_State_Common::sim_update(void) vectornav->update(); } + if (sensaition != nullptr) { + sensaition->update(); + } + if (microstrain5 != nullptr) { microstrain5->update(); } diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h index 82f9488f95e62..c54c315a60133 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.h +++ b/libraries/AP_HAL_SITL/SITL_State_common.h @@ -17,11 +17,17 @@ #include #include #include +#include #include #include #include +#include +#include +#include +#include + #include #include // #include @@ -166,6 +172,9 @@ class HALSITL::SITL_State_Common { // simulated InertialLabs INS SITL::InertialLabs *inertiallabs; + // simulated SensAItion system: + SITL::SensAItion *sensaition; + #if AP_SIM_JSON_MASTER_ENABLED // Ride along instances via JSON SITL backend SITL::JSON_Master ride_along; diff --git a/libraries/AP_Logger/AP_Logger_Flash_JEDEC.cpp b/libraries/AP_Logger/AP_Logger_Flash_JEDEC.cpp index 5f833f6f13d83..025bbdee9063a 100644 --- a/libraries/AP_Logger/AP_Logger_Flash_JEDEC.cpp +++ b/libraries/AP_Logger/AP_Logger_Flash_JEDEC.cpp @@ -58,6 +58,7 @@ extern const AP_HAL::HAL& hal; #define JEDEC_ID_CYPRESS_S25FL064L 0x016017 #define JEDEC_ID_CYPRESS_S25FL128L 0x016018 #define JEDEC_ID_GIGA_GD25Q16E 0xC84015 +#define JEDEC_ID_ZBIT_ZB25VQ128 0x5E4018 void AP_Logger_Flash_JEDEC::Init() { @@ -147,6 +148,7 @@ bool AP_Logger_Flash_JEDEC::getSectorCount(void) case JEDEC_ID_WINBOND_W25Q128: case JEDEC_ID_WINBOND_W25Q128_2: case JEDEC_ID_CYPRESS_S25FL128L: + case JEDEC_ID_ZBIT_ZB25VQ128: blocks = 256; df_PagePerBlock = 256; df_PagePerSector = 16; diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index eef769820c486..94069f59b5d2d 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -648,6 +648,13 @@ struct PACKED log_MotBatt { uint8_t mot_fail_flags; }; +struct PACKED log_SPOL { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t spool_state; + uint8_t des_spool_state; +}; + struct PACKED log_VER { LOG_PACKET_HEADER; uint64_t time_us; @@ -1166,6 +1173,12 @@ struct PACKED log_VER { // @Field: ThrOut: Throttle output // @Field: FailFlags: bit 0 motor failed, bit 1 motors balanced, should be 2 in normal flight +// @LoggerMessage: SPOL +// @Description: Spool state logging +// @Field: TimeUS: Time since system startup +// @Field: Spl: Motors spool state +// @Field: SplDes: Desired motors spool state + // messages for all boards #define LOG_COMMON_STRUCTURES \ { LOG_FORMAT_MSG, sizeof(log_Format), \ @@ -1288,7 +1301,9 @@ LOG_STRUCTURE_FROM_AIS \ { LOG_VER_MSG, sizeof(log_VER), \ "VER", "QBHBBBBIZHBBII", "TimeUS,BT,BST,Maj,Min,Pat,FWT,GH,FWS,APJ,BU,FV,IMI,ICI", "s-------------", "F-------------", false }, \ { LOG_MOTBATT_MSG, sizeof(log_MotBatt), \ - "MOTB", "QfffffB", "TimeUS,LiftMax,BatVolt,ThLimit,ThrAvMx,ThrOut,FailFlags", "s------", "F------" , true } + "MOTB", "QfffffB", "TimeUS,LiftMax,BatVolt,ThLimit,ThrAvMx,ThrOut,FailFlags", "s------", "F------" , true }, \ + { LOG_SPOL_MSG, sizeof(log_SPOL), \ + "SPOL", "QBB", "TimeUS,Spl,SplDes", "s--", "F--" , true } // message types 0 to 31 reserved for vehicle-specific use @@ -1374,6 +1389,7 @@ enum LogMessages : uint8_t { LOG_SCRIPTING_MSG, LOG_VIDEO_STABILISATION_MSG, LOG_MOTBATT_MSG, + LOG_SPOL_MSG, LOG_VER_MSG, LOG_RCOUT2_MSG, LOG_RCOUT3_MSG, diff --git a/libraries/AP_Math/crc.cpp b/libraries/AP_Math/crc.cpp index 8c784d58ad0a6..467cb6cb5741e 100644 --- a/libraries/AP_Math/crc.cpp +++ b/libraries/AP_Math/crc.cpp @@ -272,6 +272,15 @@ uint8_t crc8_rds02uf(const uint8_t *data, uint16_t length) return crc; } +uint8_t crc_xor_of_bytes(const uint8_t* data, uint16_t length) +{ + uint8_t checksum = 0; + for (uint16_t i = 0; i < length; i++) { + checksum ^= data[i]; + } + return checksum; +} + /* xmodem CRC thanks to avr-liberty https://github.com/dreamiurg/avr-liberty diff --git a/libraries/AP_Math/crc.h b/libraries/AP_Math/crc.h index 68243bd16964b..517a895e7a2bb 100644 --- a/libraries/AP_Math/crc.h +++ b/libraries/AP_Math/crc.h @@ -29,6 +29,7 @@ uint8_t crc8_dvb_update(uint8_t crc, const uint8_t* buf, const uint16_t buf_len) uint8_t crc8_maxim(const uint8_t *data, uint16_t length); uint8_t crc8_sae(const uint8_t *data, uint16_t length); uint8_t crc8_rds02uf(const uint8_t *data, uint16_t length); +uint8_t crc_xor_of_bytes(const uint8_t* data, uint16_t length); uint16_t crc_xmodem_update(uint16_t crc, uint8_t data); uint16_t crc_xmodem(const uint8_t *data, uint16_t len); uint32_t crc_crc32(uint32_t crc, const uint8_t *buf, uint32_t size); diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 8ba6499c357d5..000e369b36389 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -18,6 +18,8 @@ #include #include #include +#include +#include #define AP_MOTORS_SLEW_FILTER_CUTOFF 50.0f @@ -274,6 +276,31 @@ void AP_Motors::set_frame_string(const char * str) { } #endif +#if HAL_LOGGING_ENABLED +void AP_Motors::Log_Write_SPOL() { + + const bool spool_state_changed{_spool_state != _logged_spool_state}; + const bool des_spool_state_changed{_spool_desired != _logged_spool_desired}; + + // Log only changes. + if (!spool_state_changed && !des_spool_state_changed) { + return; + } + + _logged_spool_state = _spool_state; + _logged_spool_desired = _spool_desired; + + const struct log_SPOL pkt { + LOG_PACKET_HEADER_INIT(LOG_SPOL_MSG), + time_us : AP_HAL::micros64(), + spool_state : (uint8_t)_spool_state, + des_spool_state : (uint8_t)_spool_desired + }; + AP::logger().WriteBlock(&pkt, sizeof(pkt)); + +} +#endif + // output_test_seq - spin a motor at the pwm value specified // motor_seq is the motor's sequence number from 1 to the number of motors on the frame // pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000 diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index d6c2875ee9477..97150a16dedf8 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -282,6 +282,9 @@ class AP_Motors { #if HAL_LOGGING_ENABLED // write log, to be called at 10hz virtual void Log_Write() {}; + + // log the spool rate, writes upon change + void Log_Write_SPOL(); #endif enum MotorOptions : uint8_t { @@ -332,6 +335,8 @@ class AP_Motors { LowPassFilterFloat _throttle_slew_filter; // filter for the output of the throttle slew DesiredSpoolState _spool_desired; // desired spool state SpoolState _spool_state; // current spool mode + DesiredSpoolState _logged_spool_desired; // last logged spool state + SpoolState _logged_spool_state; // last logged spool mode // mask of what channels need fast output uint32_t _motor_fast_mask; diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index a8dcb1865fcb6..cf411cdd156ce 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -728,6 +728,7 @@ bool AP_Mount::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len) return true; } +#if AP_SCRIPTING_ENABLED // get target rate in deg/sec. returns true on success bool AP_Mount::get_rate_target(uint8_t instance, float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) { @@ -748,7 +749,6 @@ bool AP_Mount::get_angle_target(uint8_t instance, float& roll_deg, float& pitch_ return backend->get_angle_target(roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame); } -#if AP_SCRIPTING_ENABLED // get mount target location. returns true on success bool AP_Mount::get_location_target(uint8_t instance, Location& target_loc) { diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 88c206ed3a18d..ec5a52402ec21 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -246,13 +246,13 @@ class AP_Mount // any failure_msg returned will not include a prefix bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len); +#if AP_SCRIPTING_ENABLED // get target rate in deg/sec. returns true on success bool get_rate_target(uint8_t instance, float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame); // get target angle in deg. returns true on success bool get_angle_target(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame); -#if AP_SCRIPTING_ENABLED // get mount target location. returns true on success bool get_location_target(uint8_t instance, Location& target_loc); #endif diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 96c7690eca92c..4ceafb2d03a0a 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -120,6 +120,9 @@ void AP_Mount_Backend::update_mnt_target_from_rc_target() mnt_target.angle_rad.yaw_is_ef = FPV_option ? false : _yaw_lock; mnt_target.angle_rad.roll_is_ef = FPV_option ? false : _roll_lock; mnt_target.angle_rad.pitch_is_ef = FPV_option ? false : _pitch_lock; + mnt_target.rate_rads.yaw_is_ef = FPV_option ? false : _yaw_lock; + mnt_target.rate_rads.roll_is_ef = FPV_option ? false : _roll_lock; + mnt_target.rate_rads.pitch_is_ef = FPV_option ? false : _pitch_lock; // if RC_RATE is zero, targets are angle if (_params.rc_rate_max <= 0) { @@ -616,7 +619,12 @@ void AP_Mount_Backend::write_log(uint64_t timestamp_us) float target_pitch = nanf; float target_yaw = nanf; bool target_yaw_is_ef = false; - IGNORE_RETURN(get_angle_target(target_roll, target_pitch, target_yaw, target_yaw_is_ef)); + if (mnt_target.target_type == MountTargetType::ANGLE) { + target_roll = degrees(mnt_target.angle_rad.roll); + target_pitch = degrees(mnt_target.angle_rad.pitch); + target_yaw = degrees(mnt_target.angle_rad.yaw); + target_yaw_is_ef = mnt_target.angle_rad.yaw_is_ef; + } // get rangefinder distance float rangefinder_dist = nanf; @@ -1275,6 +1283,7 @@ void AP_Mount_Backend::send_target_to_gimbal() } +#if AP_SCRIPTING_ENABLED // get target rate in deg/sec. returns true on success bool AP_Mount_Backend::get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) { @@ -1301,7 +1310,6 @@ bool AP_Mount_Backend::get_angle_target(float& roll_deg, float& pitch_deg, float return false; } -#if AP_SCRIPTING_ENABLED // return target location if available // returns true if a target location is available and fills in target_loc argument bool AP_Mount_Backend::get_location_target(Location &_target_loc) diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index d0e0beae3d141..baf87101129ae 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -155,13 +155,13 @@ class AP_Mount_Backend // handle GIMBAL_DEVICE_ATTITUDE_STATUS message virtual void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) {} +#if AP_SCRIPTING_ENABLED // get target rate in deg/sec. returns true on success - bool get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame); + virtual bool get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame); // get target angle in deg. returns true on success - bool get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame); + virtual bool get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame); -#if AP_SCRIPTING_ENABLED // get mount target location. returns true on success bool get_location_target(Location &target_loc); #endif @@ -270,6 +270,8 @@ class AP_Mount_Backend float roll; // roll rate in radians/second float pitch; // roll rate in radians/second float yaw; // roll rate in radians/second + bool pitch_is_ef = true; + bool roll_is_ef = true; bool yaw_is_ef; // if set then `yaw` is a rate in earth frame }; diff --git a/libraries/AP_Mount/AP_Mount_Scripting.cpp b/libraries/AP_Mount/AP_Mount_Scripting.cpp index cee5d98b3c7d4..d5521b33dd95a 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.cpp +++ b/libraries/AP_Mount/AP_Mount_Scripting.cpp @@ -20,7 +20,13 @@ void AP_Mount_Scripting::update() { AP_Mount_Backend::update(); + // reset script target type so get_angle_target / get_rate_target return + // false until send_target_to_gimbal() writes a fresh target this cycle + _script_target_type = ScriptTargetType::NONE; + update_mnt_target(); + + send_target_to_gimbal(); } // return true if healthy @@ -39,6 +45,48 @@ void AP_Mount_Scripting::set_attitude_euler(float roll_deg, float pitch_deg, flo current_angle_deg.z = yaw_bf_deg; } +// called by send_target_to_gimbal() with the angle target for this cycle. +// Store it so get_angle_target() can return it to the Lua script. +void AP_Mount_Scripting::send_target_angles(const MountAngleTarget &angle_rad) +{ + _angle_target = angle_rad; + _script_target_type = ScriptTargetType::ANGLE; +} + +// called by send_target_to_gimbal() with the rate target for this cycle. +// Store it so get_rate_target() can return it to the Lua script. +void AP_Mount_Scripting::send_target_rates(const MountRateTarget &rate_rads) +{ + _rate_target = rate_rads; + _script_target_type = ScriptTargetType::RATE; +} + +// get target angle in deg. returns true on success +bool AP_Mount_Scripting::get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame) +{ + if (_script_target_type != ScriptTargetType::ANGLE) { + return false; + } + roll_deg = degrees(_angle_target.roll); + pitch_deg = degrees(_angle_target.pitch); + yaw_deg = degrees(_angle_target.yaw); + yaw_is_earth_frame = _angle_target.yaw_is_ef; + return true; +} + +// get target rate in deg/sec. returns true on success +bool AP_Mount_Scripting::get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) +{ + if (_script_target_type != ScriptTargetType::RATE) { + return false; + } + roll_degs = degrees(_rate_target.roll); + pitch_degs = degrees(_rate_target.pitch); + yaw_degs = degrees(_rate_target.yaw); + yaw_is_earth_frame = _rate_target.yaw_is_ef; + return true; +} + // get attitude as a quaternion. returns true on success bool AP_Mount_Scripting::get_attitude_quaternion(Quaternion& att_quat) { diff --git a/libraries/AP_Mount/AP_Mount_Scripting.h b/libraries/AP_Mount/AP_Mount_Scripting.h index e16bc0e411c69..940f2cc7db0da 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.h +++ b/libraries/AP_Mount/AP_Mount_Scripting.h @@ -36,23 +36,42 @@ class AP_Mount_Scripting : public AP_Mount_Backend // accessors for scripting backends void set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) override; + // get target rate in deg/sec. returns true on success + bool get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) override; + + // get target angle in deg. returns true on success + bool get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame) override; + protected: - // Scripting doesn't actually send anything (the script polls the - // library for the targets) + // Scripting backends poll get_angle_target / get_rate_target rather than + // receiving pushed targets, so native support for both types is declared so + // send_target_to_gimbal() never converts rates to angles on the backend's + // behalf. send_target_angles() / send_target_rates() store the converted + // target for retrieval via get_angle_target() / get_rate_target(). uint8_t natively_supported_mount_target_types() const override { - return NATIVE_ANGLES_ONLY; + return NATIVE_ANGLES_AND_RATES_ONLY; }; - void send_target_angles(const MountAngleTarget &angle_rad) override {}; + void send_target_angles(const MountAngleTarget &angle_rad) override; + void send_target_rates(const MountRateTarget &rate_rads) override; // get attitude as a quaternion. returns true on success bool get_attitude_quaternion(Quaternion& att_quat) override; private: + enum class ScriptTargetType : uint8_t { + NONE = 0, + ANGLE = 1, + RATE = 2, + }; + // internal variables - uint32_t last_update_ms; // system time of last call to one of the get_ methods. Used for health reporting - Vector3f current_angle_deg; // current gimbal angles in degrees (x=roll, y=pitch, z=yaw) + uint32_t last_update_ms; // system time of last call to one of the get_ methods. Used for health reporting + Vector3f current_angle_deg; // current gimbal angles in degrees (x=roll, y=pitch, z=yaw) + ScriptTargetType _script_target_type {ScriptTargetType::NONE}; + MountAngleTarget _angle_target {}; // last angle target pushed by send_target_angles() + MountRateTarget _rate_target {}; // last rate target pushed by send_target_rates() }; diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 74578d272173a..26d1ea4863872 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -993,13 +993,13 @@ void AP_Mount_Siyi::send_camera_information(mavlink_channel_t chan) const return; } - static const uint8_t vendor_name[32] = "Siyi"; - static uint8_t model_name[32] {}; + static const uint8_t vendor_name[MAVLINK_MSG_CAMERA_INFORMATION_FIELD_VENDOR_NAME_LEN] { "Siyi" }; + uint8_t model_name[MAVLINK_MSG_CAMERA_INFORMATION_FIELD_MODEL_NAME_LEN] {}; const uint32_t fw_version = _fw_version.camera.major | (_fw_version.camera.minor << 8) | (_fw_version.camera.patch << 16); - const char cam_definition_uri[140] {}; + const char cam_definition_uri[MAVLINK_MSG_CAMERA_INFORMATION_FIELD_CAM_DEFINITION_URI_LEN] {}; // copy model name - strncpy((char *)model_name, get_model_name(), sizeof(model_name)-1); + strncpy_noterm((char *)model_name, get_model_name(), sizeof(model_name)); // focal length // To-Do: check these values are correct for A2, ZR30, ZT30 diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index 26fe0a9897611..dc45c4b6aa957 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -324,7 +324,7 @@ class AP_Mount_Siyi : public AP_Mount_Backend_Serial // buffer holding bytes from latest packet. This is only used to calculate the crc uint8_t _msg_buff[AP_MOUNT_SIYI_PACKETLEN_MAX]; uint8_t _msg_buff_len; - const uint8_t _msg_buff_data_start = 8; // data starts at this byte of _msg_buff + static constexpr uint8_t _msg_buff_data_start = 8; // data starts at this byte of _msg_buff // parser state and unpacked fields struct PACKED { diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp index 42079c24114ff..7cf00a3c37fa5 100644 --- a/libraries/AP_Mount/AP_Mount_Topotek.cpp +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -453,13 +453,13 @@ void AP_Mount_Topotek::send_camera_information(mavlink_channel_t chan) const return; } - static const uint8_t vendor_name[32] = "Topotek"; - static uint8_t model_name[32] {}; - const char cam_definition_uri[140] {}; + static const uint8_t vendor_name[MAVLINK_MSG_CAMERA_INFORMATION_FIELD_VENDOR_NAME_LEN] { "Topotek" }; + uint8_t model_name[MAVLINK_MSG_CAMERA_INFORMATION_FIELD_MODEL_NAME_LEN] {}; + const char cam_definition_uri[MAVLINK_MSG_CAMERA_INFORMATION_FIELD_CAM_DEFINITION_URI_LEN] {}; // copy model name if available if (_got_gimbal_model_name) { - strncpy((char*)model_name, (const char*)_model_name, ARRAY_SIZE(model_name)); + strncpy_noterm((char*)model_name, _model_name, ARRAY_SIZE(model_name)); } // capability flags @@ -991,7 +991,13 @@ void AP_Mount_Topotek::gimbal_version_analyse() // extract firmware version // the version can be in the format "1.2.3" or "123" + // _msg_buff[5] holds the ASCII hex-encoded data length byte from the packet header; + // char_to_hex returns 255 for an invalid (non-hex) character, which we treat as a + // malformed packet rather than capping, since a valid length field is always a hex digit const uint8_t data_buf_len = char_to_hex(_msg_buff[5]); + if (data_buf_len == 255) { + return; + } // check for "." bool contains_period = false; @@ -1040,7 +1046,13 @@ void AP_Mount_Topotek::gimbal_version_analyse() // gimbal model name message analysis void AP_Mount_Topotek::gimbal_model_name_analyse() { - strncpy((char *)_model_name, (const char *)_msg_buff + 10, char_to_hex(_msg_buff[5])); + const auto len = char_to_hex(_msg_buff[5]); + if (len == 255) { + // flag value indicating invalid character + return; + } + memset(_model_name, 0, sizeof(_model_name)); + memcpy(_model_name, _msg_buff + 10, MIN((uint8_t)(sizeof(_model_name)-1), len)); // display gimbal model name to user GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s", send_message_prefix, _model_name); diff --git a/libraries/AP_Mount/AP_Mount_Topotek.h b/libraries/AP_Mount/AP_Mount_Topotek.h index eebcee96645fe..c7e235e014ece 100644 --- a/libraries/AP_Mount/AP_Mount_Topotek.h +++ b/libraries/AP_Mount/AP_Mount_Topotek.h @@ -254,7 +254,7 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial bool _got_gimbal_model_name; // true if gimbal's model name has been received bool _last_zoom_stop; // true if zoom has been stopped (used to re-send in order to handle lost packets) bool _last_focus_stop; // true if focus has been stopped (used to re-sent in order to handle lost packets) - uint8_t _model_name[16]; // gimbal model name + char _model_name[16]; // gimbal model name, always null-terminated uint8_t _sent_time_count; // count of current time messages sent to gimbal uint32_t _firmware_ver; // firmware version Vector3f _current_angle_rad; // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw) diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index d8f752ffd6574..f1e60e445ebdf 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -236,10 +236,14 @@ void AP_Mount_Viewpro::process_packet() break; } case CommConfigCmd::QUERY_MODEL: + if (_parsed_msg.data_bytes_received == 0) { + break; + } // gimbal model, length is 10 bytes - strncpy((char *)_model_name, (const char *)&_msg_buff[_msg_buff_data_start+1], sizeof(_model_name)-1); + memset(_model_name, '\0', sizeof(_model_name)); + memcpy(_model_name, &_msg_buff[_msg_buff_data_start+1], MIN(sizeof(_model_name)-1, (size_t)(_parsed_msg.data_bytes_received-1))); _got_model_name = true; - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s", send_text_prefix, (const char*)_model_name); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s", send_text_prefix, _model_name); break; default: // unsupported control command @@ -866,12 +870,12 @@ void AP_Mount_Viewpro::send_camera_information(mavlink_channel_t chan) const return; } - static const uint8_t vendor_name[32] = "Viewpro"; - uint8_t model_name[32] {}; + static const uint8_t vendor_name[MAVLINK_MSG_CAMERA_INFORMATION_FIELD_VENDOR_NAME_LEN] { "Viewpro" }; + uint8_t model_name[MAVLINK_MSG_CAMERA_INFORMATION_FIELD_MODEL_NAME_LEN] {}; if (_got_model_name) { - strncpy((char *)model_name, (const char*)_model_name, MIN(sizeof(model_name), sizeof(_model_name))); + strncpy_noterm((char *)model_name, _model_name, sizeof(model_name)); } - const char cam_definition_uri[140] {}; + const char cam_definition_uri[MAVLINK_MSG_CAMERA_INFORMATION_FIELD_CAM_DEFINITION_URI_LEN] {}; // capability flags const uint32_t flags = CAMERA_CAP_FLAGS_CAPTURE_VIDEO | @@ -886,7 +890,7 @@ void AP_Mount_Viewpro::send_camera_information(mavlink_channel_t chan) const chan, AP_HAL::millis(), // time_boot_ms vendor_name, // vendor_name uint8_t[32] - _model_name, // model_name uint8_t[32] + model_name, // model_name uint8_t[32] _firmware_version, // firmware version uint32_t NaNf, // sensor_size_h float (mm) NaNf, // sensor_size_v float (mm) diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.h b/libraries/AP_Mount/AP_Mount_Viewpro.h index 45d55a69712da..7619843aa978f 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.h +++ b/libraries/AP_Mount/AP_Mount_Viewpro.h @@ -384,7 +384,7 @@ class AP_Mount_Viewpro : public AP_Mount_Backend_Serial // internal variables uint8_t _msg_buff[AP_MOUNT_VIEWPRO_PACKETLEN_MAX]; // buffer holding latest bytes from gimbal uint8_t _msg_buff_len; // number of bytes held in msg buff - const uint8_t _msg_buff_data_start = 2; // data starts at this byte of _msg_buff + static constexpr uint8_t _msg_buff_data_start = 2; // data starts at this byte of _msg_buff // parser state and unpacked fields struct { @@ -407,7 +407,7 @@ class AP_Mount_Viewpro : public AP_Mount_Backend_Serial float _zoom_times; // zoom times received from gimbal uint32_t _firmware_version; // firmware version from gimbal bool _got_firmware_version; // true once we have received the firmware version - uint8_t _model_name[11] {}; // model name received from gimbal + char _model_name[11] {}; // model name received from gimbal, always null-terminated bool _got_model_name; // true once we have received model name float _rangefinder_dist_m; // latest rangefinder distance (in meters) }; diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp index fdd2cfcede3bf..8346ff53e42d1 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.cpp +++ b/libraries/AP_Mount/AP_Mount_Xacti.cpp @@ -289,9 +289,9 @@ void AP_Mount_Xacti::send_camera_information(mavlink_channel_t chan) const return; } - static const uint8_t vendor_name[32] = "Xacti"; - static uint8_t model_name[32] = "CX-GB100"; - const char cam_definition_uri[140] {}; + static const uint8_t vendor_name[MAVLINK_MSG_CAMERA_INFORMATION_FIELD_VENDOR_NAME_LEN] { "Xacti" }; + static uint8_t model_name[MAVLINK_MSG_CAMERA_INFORMATION_FIELD_MODEL_NAME_LEN] { "CX-GB100" }; + const char cam_definition_uri[MAVLINK_MSG_CAMERA_INFORMATION_FIELD_CAM_DEFINITION_URI_LEN] {}; // capability flags const uint32_t flags = CAMERA_CAP_FLAGS_CAPTURE_VIDEO | diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 4b0109f56fa26..a3dc700026d3b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1596,6 +1596,15 @@ bool NavEKF3::configuredToUseGPSForPosXY(void) const return false; } +// check if configured to use GPS for horizontal position estimation +bool NavEKF3::configuredToUseGPSForPos(void) const +{ + if (configuredToUseGPSForPosXY() || sources.getPosZSource(primary) == AP_NavEKF_Source::SourceZ::GPS) { + return true; + } + return false; +} + // write the raw optical flow measurements // rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality // rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes. diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index cc100df67adf4..ecf29100805af 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -361,6 +361,9 @@ class NavEKF3 { // check if configured to use GPS for horizontal position estimation bool configuredToUseGPSForPosXY(void) const; + // check if configured to use GPS for position estimation + bool configuredToUseGPSForPos(void) const; + // Writes the default equivalent airspeed and 1-sigma uncertainty in m/s to be used in forward flight if a measured airspeed is required and not available. void writeDefaultAirSpeed(float airspeed, float uncertainty); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 6d2babb16dddd..29fd29f38efdc 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -62,64 +62,40 @@ void NavEKF3_core::FuseAirspeed() } SK_TAS[1] = SH_TAS[1]; + uint32_t kalman_mask = 0; // values to calculate in Kfusion (others are set to zero) + if (tasDataDelayed.allowFusion && !airDataFusionWindOnly) { - Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]); - Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]); - Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]); - Kfusion[3] = SK_TAS[0]*(P[3][4]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][5]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][6]*vd*SH_TAS[0]); - Kfusion[4] = SK_TAS[0]*(P[4][4]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][5]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][6]*vd*SH_TAS[0]); - Kfusion[5] = SK_TAS[0]*(P[5][4]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][5]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][6]*vd*SH_TAS[0]); - Kfusion[6] = SK_TAS[0]*(P[6][4]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][5]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][6]*vd*SH_TAS[0]); - Kfusion[7] = SK_TAS[0]*(P[7][4]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][5]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][6]*vd*SH_TAS[0]); - Kfusion[8] = SK_TAS[0]*(P[8][4]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][5]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][6]*vd*SH_TAS[0]); - Kfusion[9] = SK_TAS[0]*(P[9][4]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][5]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][6]*vd*SH_TAS[0]); - } else { - // zero indexes 0 to 9 - zero_range(&Kfusion[0], 0, 9); + kalman_mask = (1<<10)-1; } if (tasDataDelayed.allowFusion && !inhibitDelAngBiasStates && !airDataFusionWindOnly) { - Kfusion[10] = SK_TAS[0]*(P[10][4]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][5]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][6]*vd*SH_TAS[0]); - Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]); - Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]); - } else { - // zero indexes 10 to 12 - zero_range(&Kfusion[0], 10, 12); + kalman_mask |= (1<<10) | (1<<11) | (1<<12); } if (tasDataDelayed.allowFusion && !inhibitDelVelBiasStates && !airDataFusionWindOnly) { for (uint8_t index = 0; index < 3; index++) { const uint8_t stateIndex = index + 13; if (!dvelBiasAxisInhibit[index]) { - Kfusion[stateIndex] = SK_TAS[0]*(P[stateIndex][4]*SH_TAS[2] - P[stateIndex][22]*SH_TAS[2] + P[stateIndex][5]*SK_TAS[1] - P[stateIndex][23]*SK_TAS[1] + P[stateIndex][6]*vd*SH_TAS[0]); - } else { - Kfusion[stateIndex] = 0.0f; + kalman_mask |= (1<sources.getPosZSource(core_index) == AP_NavEKF_Source::SourceZ::EXTNAV) { + hgtMea = -extNavDataDelayed.pos.z; + posDownObsNoise = sq(constrain_ftype(extNavDataDelayed.posErr, 0.1f, 10.0f)); + ResetHeight(); + } #endif // EK3_FEATURE_EXTERNAL_NAV } @@ -681,6 +683,13 @@ bool NavEKF3_core::assume_zero_sideslip(void) const // returns false if the origin is already set bool NavEKF3_core::setOriginLLH(const Location &loc) { + // reject external origin setting until the filter has finished + // bootstrap initialisation. InitialiseVariables() resets + // validOrigin, so an origin set before that point is lost. + // Callers (e.g. AHRS use_recorded_origin_maybe) will retry. + if (!statesInitialised) { + return false; + } return setOrigin(loc); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 2093e7e73c8f0..ea1a72073cdb5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -610,59 +610,37 @@ void NavEKF3_core::FuseMagnetometer() 2.0f*q0*q3 + 2.0f*q1*q2 }; - Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] - P[0][2]*SH_MAG[1] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[4] - P[0][18]*SK_MX[3]); - Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] - P[1][2]*SH_MAG[1] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[4] - P[1][18]*SK_MX[3]); - Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[4] - P[2][18]*SK_MX[3]); - Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] - P[3][2]*SH_MAG[1] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[4] - P[3][18]*SK_MX[3]); - Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] - P[4][2]*SH_MAG[1] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[4] - P[4][18]*SK_MX[3]); - Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] - P[5][2]*SH_MAG[1] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[4] - P[5][18]*SK_MX[3]); - Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] - P[6][2]*SH_MAG[1] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[4] - P[6][18]*SK_MX[3]); - Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] - P[7][2]*SH_MAG[1] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[4] - P[7][18]*SK_MX[3]); - Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] - P[8][2]*SH_MAG[1] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[4] - P[8][18]*SK_MX[3]); - Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] - P[9][2]*SH_MAG[1] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[4] - P[9][18]*SK_MX[3]); + uint32_t kalman_mask = (1<<10)-1; // values to calculate in Kfusion (others are set to zero) if (!inhibitDelAngBiasStates) { - Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] - P[10][2]*SH_MAG[1] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[4] - P[10][18]*SK_MX[3]); - Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] - P[11][2]*SH_MAG[1] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[4] - P[11][18]*SK_MX[3]); - Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] - P[12][2]*SH_MAG[1] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[4] - P[12][18]*SK_MX[3]); - } else { - // zero indexes 10 to 12 - zero_range(&Kfusion[0], 10, 12); + kalman_mask |= (1<<10) | (1<<11) | (1<<12); } if (!inhibitDelVelBiasStates) { for (uint8_t index = 0; index < 3; index++) { const uint8_t stateIndex = index + 13; if (!dvelBiasAxisInhibit[index]) { - Kfusion[stateIndex] = SK_MX[0]*(P[stateIndex][19] + P[stateIndex][1]*SH_MAG[0] - P[stateIndex][2]*SH_MAG[1] + P[stateIndex][3]*SH_MAG[2] + P[stateIndex][0]*SK_MX[2] - P[stateIndex][16]*SK_MX[1] + P[stateIndex][17]*SK_MX[4] - P[stateIndex][18]*SK_MX[3]); - } else { - Kfusion[stateIndex] = 0.0f; + kalman_mask |= (1<> 4) msg_count[message_type] = (msg_count[message_type] or 0) + 1 - gcs:send_named_float(message_type, msg_count[message_type]) + gcs:send_named_float(tostring(message_type), msg_count[message_type]) end local complete_message = rtcm_buffer:sub(1, total_length) diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua index 09aba10f44879..9da4fa5ec186f 100644 --- a/libraries/AP_Scripting/applets/plane_follow.lua +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -25,7 +25,7 @@ FOLLP_TURN_DEG - if the target is more than this many degrees left or right, assume it's turning --]] -SCRIPT_VERSION = "4.7.0-074" +SCRIPT_VERSION = "4.7.0-075" SCRIPT_NAME = "Plane Follow" SCRIPT_NAME_SHORT = "PFollow" @@ -564,14 +564,13 @@ local follow_mode = { else if reported_target then -- i.e. if we previously reported a target but lost it if (now_ms - lost_target_now_ms) > 5000 then - gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": lost prior target: " .. follow:get_target_sysid()) + gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": lost prior SYSID: " .. tostring(follow:get_target_sysid())) lost_target_now_ms = now_ms end end reported_target = false - gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": no target: " .. follow:get_target_sysid()) + gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": no target SYSID: " .. tostring(follow:get_target_sysid())) end - return reported_target end function follow_mode.enable() @@ -920,8 +919,10 @@ function Update() mechanism = 1 -- position/location - for logging end - -- The desired heading needs a PID controller, especially when it gets close. - desired_heading = xt_pid:compute(desired_heading, cross_track_distance, (now_ms - now_heading_ms):tofloat() * 0.001) + -- The desired heading needs a PID controller for crosstrack, but only when it gets close. + if close or too_close_follow_up > 0 then + desired_heading = xt_pid:compute(desired_heading, cross_track_distance, (now_ms - now_heading_ms):tofloat() * 0.001) + end -- dv = interim delta velocity based on the pid controller using projected_distance per loop as the error (we want distance == 0) local dv_error = along_track_distance * refresh_rate * 2.0 diff --git a/libraries/AP_Scripting/applets/quadplane_terrain_avoid.lua b/libraries/AP_Scripting/applets/quadplane_terrain_avoid.lua index 6a031f8302fc1..176f0a0ef5582 100644 --- a/libraries/AP_Scripting/applets/quadplane_terrain_avoid.lua +++ b/libraries/AP_Scripting/applets/quadplane_terrain_avoid.lua @@ -960,6 +960,11 @@ while (distance > 0) { ------------------------------------------------------------------------------- -- Lookahead functions - replaces the c++ functions in AP_Terrain ------------------------------------------------------------------------------- +---@param start_location Location_ud +---@param search_bearing number +---@param search_distance number +---@param search_ratio number +---@return Location_ud|nil function Terrain_Lookahead(start_location, search_bearing, search_distance, search_ratio) local highest_location = nil local climb = 0.0 diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index e85234b9cb399..a6a8b61d4f3f6 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -4044,12 +4044,14 @@ function fence:get_margin_breach_time() end ---| 8 # Minimum altitude function fence:get_breaches() end --- Returns minimum safe altitude in meters above home alt frame (i.e. alt_min + margin) ----@return number +-- Returns minimum safe altitude in meters and its altitude frame (i.e. alt_min + margin) +---@return number altitude_m +---@return integer frame -- 0:Above sea level, 1:Above Home, 2:Above Origin, 3:Above Terrain function fence:get_safe_alt_min() end --- Returns maximum safe altitude in meters above home alt frame (i.e. alt_max - margin) ----@return number +-- Returns maximum safe altitude in meters and its altitude frame (i.e. alt_max - margin) +---@return number altitude_m +---@return integer frame -- 0:Above sea level, 1:Above Home, 2:Above Origin, 3:Above Terrain function fence:get_safe_alt_max() end -- Returns configured fences diff --git a/libraries/AP_Scripting/examples/mount-driver.lua b/libraries/AP_Scripting/examples/mount-driver.lua new file mode 100644 index 0000000000000..1eb25497bf3aa --- /dev/null +++ b/libraries/AP_Scripting/examples/mount-driver.lua @@ -0,0 +1,153 @@ +-- mount-driver.lua: Example scripting gimbal driver +-- +-- Template for writing a Lua gimbal driver using the scripting mount backend. +-- Populate send_target_angles and send_target_rates with your gimbal's +-- protocol (serial, CAN, etc). This example simulates a gimbal by tracking +-- targets internally and reporting them back as attitude. +-- +-- Setup: +-- Set MNT1_TYPE = 9 (Scripting) and reboot +-- Copy this script to the APM/scripts directory and reboot +-- +-- Advanced usage: +-- The gimbal can be used as the Nth mount by setting MNTn_TYPE = 9 and +-- modifying the MOUNT_INSTANCE below. + +-- user definitions +local MOUNT_INSTANCE = 0 -- default to MNT1 + +-- global definitions +local INIT_INTERVAL_MS = 3000 -- attempt to initialise the gimbal at this interval +local UPDATE_INTERVAL_MS = 100 -- update at 10hz +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +-- local variables +local sim_state = { + roll_ef_deg=0, -- roll/pitch earth frame, yaw body frame + pitch_ef_deg=0, -- (common for pwm-controlled brushless gimbals) + yaw_bf_deg=0 +} +local initialised = false +local last_update_ms = 0 + +-- wrap yaw angle in degrees to value between 0 and 360 +local function wrap_360(angle) + local res = math.fmod(angle, 360.0) + if res < 0 then + res = res + 360.0 + end + return res +end + +-- wrap yaw angle in degrees to value between -180 and +180 +local function wrap_180(angle_deg) + local res = wrap_360(angle_deg) + if res > 180 then + res = res - 360 + end + return res +end + +-- bind mount type parameter +local MNT_TYPE = Parameter("MNT" .. (MOUNT_INSTANCE + 1) .. "_TYPE") + +-- perform any required initialisation +local function init() + if MNT_TYPE:get() ~= 9 then + gcs:send_text(MAV_SEVERITY.CRITICAL, "MountDriver: set MNT" .. (MOUNT_INSTANCE + 1) .. "_TYPE=9") + return + end + + initialised = true + last_update_ms = millis():tofloat() + gcs:send_text(MAV_SEVERITY.INFO, "MountDriver: started") +end + +-- send target angles (in degrees) to gimbal +local function send_target_angles(roll_ef_deg, pitch_ef_deg, yaw_deg, yaw_is_ef) + -- default argument values + roll_ef_deg = roll_ef_deg or 0 + pitch_ef_deg = pitch_ef_deg or 0 + yaw_deg = yaw_deg or 0 + yaw_is_ef = yaw_is_ef or false + + if yaw_is_ef then + -- convert to body-frame + yaw_deg = wrap_180(yaw_deg - math.deg(ahrs:get_yaw_rad())) + end + + sim_state.roll_ef_deg = roll_ef_deg + sim_state.pitch_ef_deg = pitch_ef_deg + sim_state.yaw_bf_deg = yaw_deg +end + +-- send target rates (in deg/sec) to gimbal +local function send_target_rates(roll_degs, pitch_degs, yaw_degs, yaw_is_ef, dt_s) + -- default argument values + roll_degs = roll_degs or 0 + pitch_degs = pitch_degs or 0 + yaw_degs = yaw_degs or 0 + yaw_is_ef = yaw_is_ef or false + + if yaw_is_ef then + yaw_degs = yaw_degs - math.deg(ahrs:get_gyro():z()) + end + + send_target_angles( + sim_state.roll_ef_deg + roll_degs * dt_s, + sim_state.pitch_ef_deg + pitch_degs * dt_s, + sim_state.yaw_bf_deg + yaw_degs * dt_s, + false + ) +end + +-- the main update function +local function update() + + -- initialise connection to gimbal + if not initialised then + init() + return + end + + -- calculate dt + local now_ms = millis():tofloat() + local dt_s = (now_ms - last_update_ms) / 1000.0 + last_update_ms = now_ms + + -- report gimbal attitude. Must be called periodically or the backend reports + -- unhealthy. Ideally, populate this from a gimbal attitude message. If your + -- gimbal doesn't report attitude but you can detect it is alive, stop calling + -- this when it stops responding so ArduPilot gets real health feedback. Here + -- we just report our sim state directly since there is no real gimbal. + mount:set_attitude_euler(MOUNT_INSTANCE, sim_state.roll_ef_deg, sim_state.pitch_ef_deg, sim_state.yaw_bf_deg) + + -- send angle target + local roll_deg, pitch_deg, yaw_deg, yaw_is_ef = mount:get_angle_target(MOUNT_INSTANCE) + if roll_deg and pitch_deg and yaw_deg then + send_target_angles(roll_deg, pitch_deg, yaw_deg, yaw_is_ef) + return + end + + -- send rate target + local roll_degs, pitch_degs, yaw_degs + roll_degs, pitch_degs, yaw_degs, yaw_is_ef = mount:get_rate_target(MOUNT_INSTANCE) + if roll_degs and pitch_degs and yaw_degs then + send_target_rates(roll_degs, pitch_degs, yaw_degs, yaw_is_ef, dt_s) + return + end +end + +local function protected_wrapper() + local success, err = pcall(update) + if not success then + gcs:send_text(MAV_SEVERITY.ERROR, "MountDriver: " .. err) + return protected_wrapper, 1000 + end + if not initialised then + return protected_wrapper, INIT_INTERVAL_MS + end + return protected_wrapper, UPDATE_INTERVAL_MS +end + +return protected_wrapper() diff --git a/libraries/AP_Scripting/examples/plane-wind-fs.lua b/libraries/AP_Scripting/examples/plane-wind-fs.lua index 87755bd44789f..bde0774160d86 100644 --- a/libraries/AP_Scripting/examples/plane-wind-fs.lua +++ b/libraries/AP_Scripting/examples/plane-wind-fs.lua @@ -64,7 +64,7 @@ value = param:get('RTL_RADIUS') if value then value = math.abs(value) if value > 0 then - home_reached_rad = math.abs(value) * 2 + home_reached_rad = value * 2 else value = param:get('WP_LOITER_RAD') if value then diff --git a/libraries/AP_Scripting/examples/rover-motor-driver.lua b/libraries/AP_Scripting/examples/rover-motor-driver.lua index 1000df8bcd049..b692e5aaa07d7 100644 --- a/libraries/AP_Scripting/examples/rover-motor-driver.lua +++ b/libraries/AP_Scripting/examples/rover-motor-driver.lua @@ -29,6 +29,10 @@ function update() if throttle < 0 then steering = -steering end + + -- Language checker thinks steering could be nil, reassure it that it cannot + ---@cast steering number + SRV_Channels:set_output_norm(K_SCRIPTING1, steering) SRV_Channels:set_output_norm(K_SCRIPTING3, throttle) end diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index a46de27611d82..f6ff4a8e6ef7d 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -1064,10 +1064,10 @@ singleton AC_Fence method get_margin_breaches uint8_t singleton AC_Fence method get_margin_breach_time uint32_t singleton AC_Fence method get_breach_distance float uint8_t'skip_check singleton AC_Fence method get_breach_direction_NED boolean uint8_t'skip_check Vector3f'Null Location'Null -singleton AC_Fence method get_safe_alt_min_m float -singleton AC_Fence method get_safe_alt_min_m rename get_safe_alt_min -singleton AC_Fence method get_safe_alt_max_m float -singleton AC_Fence method get_safe_alt_max_m rename get_safe_alt_max +singleton AC_Fence method get_safe_alt_min_m_and_frame void float'Ref uint8_t'Ref +singleton AC_Fence method get_safe_alt_min_m_and_frame rename get_safe_alt_min +singleton AC_Fence method get_safe_alt_max_m_and_frame void float'Ref uint8_t'Ref +singleton AC_Fence method get_safe_alt_max_m_and_frame rename get_safe_alt_max singleton AC_Fence method present uint8_t singleton AC_Fence method get_enabled_fences uint8_t diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 1c58fdbab4f3a..dfd637c6d1f70 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3291,6 +3291,18 @@ MAV_RESULT GCS_MAVLINK::handle_command_request_message(const mavlink_command_int set_ap_message_interval(MSG_AVAILABLE_MODES_MONITOR, 5000); break; +#if AP_CAMERA_ENABLED + case MSG_CAMERA_INFORMATION: + // param2 selects a specific camera instance (1-based); 0 means all + if (packet.param2 >= 1) { + AP_Camera *camera = AP::camera(); + if (camera != nullptr) { + camera->set_camera_information_send_instance((int16_t)(packet.param2 - 1)); + } + } + break; +#endif + default: break; } @@ -6046,7 +6058,11 @@ void GCS_MAVLINK::send_sys_status() errors1, errors2, dropped_logmessage_count, // errors3 - errors4); // errors4 + errors4, // errors4 + 0, // control_sensors_present2, + 0, // control_sensors_enabled2, + 0 // control_sensors_health2 + ); } void GCS_MAVLINK::send_extended_sys_state() const diff --git a/libraries/RC_Channel/RC_Channels.cpp b/libraries/RC_Channel/RC_Channels.cpp index 82bb5de3c4e8d..212fb2b233c93 100644 --- a/libraries/RC_Channel/RC_Channels.cpp +++ b/libraries/RC_Channel/RC_Channels.cpp @@ -86,7 +86,8 @@ uint8_t RC_Channels::get_radio_in(uint16_t *chans, const uint8_t num_channels) // update all the input channels bool RC_Channels::read_input(void) { - if (hal.rcin->new_input()) { + if (hal.rcin->new_input() && + !rc().option_is_enabled(RC_Channels::Option::IGNORE_RECEIVER)) { _has_had_rc_receiver = true; } else if (!has_new_overrides) { return false; diff --git a/libraries/SITL/SIM_AVT_CM62.cpp b/libraries/SITL/SIM_AVT_CM62.cpp new file mode 100644 index 0000000000000..ae521c9cdbc82 --- /dev/null +++ b/libraries/SITL/SIM_AVT_CM62.cpp @@ -0,0 +1,11 @@ +/* + Simulator for an AVT CM62 MAVLink (Gimbal Protocol v2) gimbal +*/ + +#include "SIM_config.h" + +#if AP_SIM_AVT_CM62_ENABLED + +#include "SIM_AVT_CM62.h" + +#endif // AP_SIM_AVT_CM62_ENABLED diff --git a/libraries/SITL/SIM_AVT_CM62.h b/libraries/SITL/SIM_AVT_CM62.h new file mode 100644 index 0000000000000..4635f11547130 --- /dev/null +++ b/libraries/SITL/SIM_AVT_CM62.h @@ -0,0 +1,33 @@ +/* + Simulator for an AVT CM62 MAVLink (Gimbal Protocol v2) gimbal +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_AVT_CM62_ENABLED + +#include "SIM_MAVLinkGimbalv2.h" +#include + +namespace SITL { + +class AVT_CM62 : public MAVLinkGimbalv2 { + const char *get_vendor_name() const override { return "AVTA"; } + const char *get_model_name() const override { return "SIM_AVTA"; } + // firmware_version encoding: major=1 | (minor=2)<<8 | (patch=3)<<16 + uint32_t get_firmware_version() const override { return (3U << 16) | (2U << 8) | 1U; } + uint16_t get_cap_flags() const override { + return GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS | + GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS; + } + float get_pitch_min_rad() const override { return radians(-45.0f); } + float get_pitch_max_rad() const override { return radians( 45.0f); } + float get_yaw_min_rad() const override { return radians(-180.0f); } + float get_yaw_max_rad() const override { return radians( 180.0f); } +}; + +} // namespace SITL + +#endif // AP_SIM_AVT_CM62_ENABLED diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 28be7e2baeebb..00cf87694209d 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -1205,6 +1205,14 @@ void Aircraft::update_external_payload(const struct sitl_input &input) #if AP_SIM_GPIO_LED_RGB_ENABLED sim_ledrgb.update(*this); #endif + +#if AP_SIM_MOUNT_ENABLED + for (uint8_t i = 0; i < GIMBAL_SIM_MAX; i++) { + if (gimbal_sims[i] != nullptr) { + gimbal_sims[i]->update(*this); + } + } +#endif } void Aircraft::add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel) diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 13992dfb2b35b..43ebd2a1bf619 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -43,6 +43,10 @@ #include "SIM_GPIO_LED_2.h" #include "SIM_GPIO_LED_3.h" #include "SIM_GPIO_LED_RGB.h" +#include "SIM_Siyi.h" +#include "SIM_Topotek.h" +#include "SIM_Viewpro.h" +#include "SIM_Mount.h" #define MAX_SIM_INSTANCES 16 @@ -178,6 +182,18 @@ class Aircraft { float ambient_temperature_degC() const; +#if AP_SIM_MOUNT_ENABLED + void add_gimbal_sim(Mount &sim) { + for (uint8_t i = 0; i < GIMBAL_SIM_MAX; i++) { + if (gimbal_sims[i] == nullptr) { + gimbal_sims[i] = ∼ + return; + } + } + AP_HAL::panic("Too many gimbal simulators"); + } +#endif // AP_SIM_MOUNT_ENABLED + ADSB *adsb; // takes a PWM range between 1000 and 2000 and returns a floating @@ -425,6 +441,11 @@ class Aircraft { static Aircraft *instances[MAX_SIM_INSTANCES]; HAL_Semaphore pose_sem; + +#if AP_SIM_MOUNT_ENABLED + static constexpr uint8_t GIMBAL_SIM_MAX = 8; + Mount *gimbal_sims[GIMBAL_SIM_MAX]; +#endif }; } // namespace SITL diff --git a/libraries/SITL/SIM_Gimbal.cpp b/libraries/SITL/SIM_Gimbal.cpp index a905a38d731d0..2388ee9593966 100644 --- a/libraries/SITL/SIM_Gimbal.cpp +++ b/libraries/SITL/SIM_Gimbal.cpp @@ -50,6 +50,7 @@ void Gimbal::update(const class Aircraft &aircraft) const Matrix3f &vehicle_dcm = aircraft.get_dcm(); if (!init_done) { dcm = vehicle_dcm; + init_done = true; } const Vector3f &vehicle_gyro = AP::ins().get_gyro(); diff --git a/libraries/SITL/SIM_Gimbal.h b/libraries/SITL/SIM_Gimbal.h index af6da4af7f149..b860664036bb4 100644 --- a/libraries/SITL/SIM_Gimbal.h +++ b/libraries/SITL/SIM_Gimbal.h @@ -36,7 +36,19 @@ class Gimbal { } void get_deltas(Vector3f &_delta_angle, Vector3f &_delta_velocity, uint32_t &_delta_time_us); - void get_joint_angles(Vector3f &_angles) { _angles = joint_angles; } + void get_joint_angles(Vector3f &_angles) const { _angles = joint_angles; } + + // copy the gimbal body-to-earth rotation matrix + void get_dcm(Matrix3f &out) const { out = dcm; } + + // override the physical joint angle limits (roll, pitch, azimuth) in radians + void set_joint_limits(const Vector3f &lower_limits, const Vector3f &upper_limits) { + lower_joint_limits = lower_limits; + upper_joint_limits = upper_limits; + } + + // force the gimbal body-to-earth rotation matrix to a specific value + void set_dcm(const Matrix3f &m) { dcm = m; init_done = true; } private: diff --git a/libraries/SITL/SIM_MAVLinkGimbalv2.cpp b/libraries/SITL/SIM_MAVLinkGimbalv2.cpp new file mode 100644 index 0000000000000..37e39f5476961 --- /dev/null +++ b/libraries/SITL/SIM_MAVLinkGimbalv2.cpp @@ -0,0 +1,358 @@ +/* + Simulator for MAVLink Gimbal Protocol v2 peripherals +*/ + +#include "SIM_config.h" + +#if AP_SIM_MAVLINKGIMBALV2_ENABLED + +#include "SIM_MAVLinkGimbalv2.h" +#include "SIM_Aircraft.h" +#include +#include + +namespace SITL { + +void MAVLinkGimbalv2::set_instance(uint8_t instance) +{ + Mount::set_instance(instance); + // select the component ID matching this gimbal instance: + // instance 0 → MAV_COMP_ID_GIMBAL (154) + // instance 1 → MAV_COMP_ID_GIMBAL2 (171), 2 → 172, … + _compid = (instance == 0) ? MAV_COMP_ID_GIMBAL + : MAV_COMP_ID_GIMBAL2 + (instance - 1); + + // configure joint limits from subclass declarations (roll kept at ±40°) + _gimbal.set_joint_limits( + Vector3f(radians(-40.0f), get_pitch_min_rad(), get_yaw_min_rad()), + Vector3f(radians( 40.0f), get_pitch_max_rad(), get_yaw_max_rad()) + ); +} + +void MAVLinkGimbalv2::send_mavlink_message(const mavlink_message_t &msg) +{ + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); + if (len > 0) { + write_to_autopilot((char*)buf, len); + } +} + +void MAVLinkGimbalv2::update_input() +{ + uint8_t buf[128]; + const ssize_t nread = read_from_autopilot((char*)buf, sizeof(buf)); + for (ssize_t i = 0; i < nread; i++) { + mavlink_message_t msg; + mavlink_status_t status; + if (mavlink_frame_char_buffer(&mav.rxmsg, &mav.status, + buf[i], &msg, &status) == MAVLINK_FRAMING_OK) { + handle_message(msg); + } + } +} + +void MAVLinkGimbalv2::handle_message(const mavlink_message_t &msg) +{ + switch (msg.msgid) { + case MAVLINK_MSG_ID_HEARTBEAT: { + if (!_seen_autopilot_heartbeat) { + _seen_autopilot_heartbeat = true; + _vehicle_system_id = msg.sysid; + _vehicle_component_id = msg.compid; + ::printf("MAVLinkGimbalv2[%u]: using sysid=%u compid=%u\n", + (unsigned)_instance, + (unsigned)_vehicle_system_id, + (unsigned)_compid); + } + break; + } + case MAVLINK_MSG_ID_COMMAND_LONG: { + mavlink_command_long_t cmd; + mavlink_msg_command_long_decode(&msg, &cmd); + // only handle messages addressed to this gimbal + if (cmd.target_system != _vehicle_system_id || + cmd.target_component != _compid) { + break; + } + if (cmd.command == MAV_CMD_REQUEST_MESSAGE) { + const uint32_t requested_msgid = (uint32_t)cmd.param1; + if (requested_msgid == MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION) { + send_gimbal_device_information(); + send_command_ack(msg.sysid, msg.compid, + MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_ACCEPTED); + } + } + break; + } + case MAVLINK_MSG_ID_COMMAND_INT: { + mavlink_command_int_t cmd; + mavlink_msg_command_int_decode(&msg, &cmd); + if (cmd.target_system != _vehicle_system_id || + cmd.target_component != _compid) { + break; + } + if (cmd.command == MAV_CMD_DO_SET_ROI_LOCATION) { + _roi.loc.lat = cmd.x; + _roi.loc.lng = cmd.y; + _roi.loc.set_alt_m(cmd.z, (Location::AltFrame)cmd.frame); + _roi.valid = true; + } else if (cmd.command == MAV_CMD_DO_SET_ROI_NONE) { + _roi.valid = false; + } + break; + } + case MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE: { + mavlink_gimbal_device_set_attitude_t cmd; + mavlink_msg_gimbal_device_set_attitude_decode(&msg, &cmd); + if (cmd.target_system != _vehicle_system_id || + cmd.target_component != _compid) { + break; + } + const uint16_t flags = cmd.flags; + if (flags & GIMBAL_DEVICE_FLAGS_RETRACT) { + _target.valid = false; + _roi.valid = false; + break; + } + const bool yaw_ef = (flags & GIMBAL_DEVICE_FLAGS_YAW_LOCK) != 0; + const bool rates_valid = !isnan(cmd.angular_velocity_x); + const bool q_valid = !isnan(cmd.q[0]); + if (rates_valid || q_valid) { + // any explicit attitude/rate command cancels ROI tracking + _roi.valid = false; + } + if (rates_valid) { + _target.valid = true; + _target.is_rate = true; + _target.yaw_is_ef = yaw_ef; + _target.rates_rads = Vector3f(cmd.angular_velocity_x, + cmd.angular_velocity_y, + cmd.angular_velocity_z); + } else if (q_valid) { + _target.valid = true; + _target.is_rate = false; + _target.yaw_is_ef = yaw_ef; + _target.attitude = Quaternion(cmd.q[0], cmd.q[1], cmd.q[2], cmd.q[3]); + } + break; + } + default: + break; + } +} + +void MAVLinkGimbalv2::send_heartbeat() +{ + if (!_seen_autopilot_heartbeat) { + return; + } + + mavlink_heartbeat_t hb {}; + hb.type = MAV_TYPE_GIMBAL; + hb.autopilot = MAV_AUTOPILOT_INVALID; + hb.base_mode = 0; + hb.system_status = MAV_STATE_ACTIVE; + hb.mavlink_version = 3; + + mavlink_message_t msg; + mavlink_msg_heartbeat_encode_status( + _vehicle_system_id, _compid, + &mav.status, &msg, &hb); + send_mavlink_message(msg); +} + +void MAVLinkGimbalv2::send_gimbal_device_information() +{ + mavlink_gimbal_device_information_t info {}; + info.time_boot_ms = AP_HAL::millis(); + info.firmware_version = get_firmware_version(); + info.hardware_version = 0; + info.roll_min = NAN; + info.roll_max = NAN; + info.pitch_min = get_pitch_min_rad(); + info.pitch_max = get_pitch_max_rad(); + info.yaw_min = get_yaw_min_rad(); + info.yaw_max = get_yaw_max_rad(); + info.cap_flags = get_cap_flags(); + strncpy_noterm(info.vendor_name, get_vendor_name(), sizeof(info.vendor_name)); + strncpy_noterm(info.model_name, get_model_name(), sizeof(info.model_name)); + + mavlink_message_t msg; + mavlink_msg_gimbal_device_information_encode_status( + _vehicle_system_id, _compid, + &mav.status, &msg, &info); + send_mavlink_message(msg); +} + +// update gimbal physics and compute demanded rates from the current target +void MAVLinkGimbalv2::update_gimbal(const Aircraft &aircraft) +{ + _vehicle_dcm = aircraft.get_dcm(); + + Matrix3f gimbal_dcm; + _gimbal.get_dcm(gimbal_dcm); + + // if tracking an ROI, continuously recompute the earth-frame target + // (replicates AP_Mount_Backend::get_angle_target_to_location()) + if (_roi.valid) { + const Location &veh = aircraft.get_location(); + const float gps_x = Location::diff_longitude(_roi.loc.lng, veh.lng) + * cosf(radians((veh.lat + _roi.loc.lat) * 0.00000005f)) * 0.01113195f; + const float gps_y = (_roi.loc.lat - veh.lat) * 0.01113195f; + int32_t target_alt_cm = 0, veh_alt_cm = 0; + if (_roi.loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, target_alt_cm) && + veh.get_alt_cm(Location::AltFrame::ABOVE_HOME, veh_alt_cm)) { + const float gps_z = (float)(target_alt_cm - veh_alt_cm); // cm + const float horiz_cm = 100.0f * norm(gps_x, gps_y); + float pitch_ef = atan2f(gps_z, horiz_cm); + pitch_ef = constrain_float(pitch_ef, get_pitch_min_rad(), get_pitch_max_rad()); + const float yaw_ef = atan2f(gps_x, gps_y); + _target.valid = true; + _target.is_rate = false; + _target.yaw_is_ef = true; + _target.attitude.from_euler(0.0f, pitch_ef, yaw_ef); + } + } + + if (_target.valid) { + Vector3f demanded_rates; + if (_target.is_rate) { + demanded_rates = _target.rates_rads; + if (_target.yaw_is_ef) { + // transform earth-frame rates to gimbal body frame + demanded_rates = gimbal_dcm.transposed() * demanded_rates; + } + // add vehicle body rates so that rate=0 tracks the vehicle body + // (body-relative stabilisation); matches neutral-mode behaviour + demanded_rates += gimbal_dcm.transposed() * _vehicle_dcm * aircraft.get_gyro(); + _gimbal.set_demanded_rates(demanded_rates); + } else if (_target.yaw_is_ef) { + // earth-frame angle control: P-controller + // clamp target pitch to hardware joint limits before computing error + float t_r, t_p, t_y; + _target.attitude.to_euler(t_r, t_p, t_y); + t_p = constrain_float(t_p, get_pitch_min_rad(), get_pitch_max_rad()); + Quaternion target_clamped; + target_clamped.from_euler(t_r, t_p, t_y); + Quaternion q_current; + q_current.from_rotation_matrix(gimbal_dcm); + Quaternion q_error = q_current.inverse() * target_clamped; + q_error.normalize(); + Vector3f av(q_error.q2, q_error.q3, q_error.q4); + if (q_error.q1 < 0.0f) { + av = -av; + } + const float attitude_gain = 2.0f; // rad/s per radian of error + demanded_rates = av * (2.0f * attitude_gain); + _gimbal.set_demanded_rates(demanded_rates); + } else { + // body-frame angle control: snap gimbal DCM to the commanded + // body-relative orientation directly + Matrix3f body_dcm; + _target.attitude.rotation_matrix(body_dcm); + _gimbal.set_dcm(_vehicle_dcm * body_dcm); + _gimbal.set_demanded_rates(Vector3f{}); + } + } else { + // neutral: track vehicle body using vehicle angular rate feedforward + const Vector3f vehicle_rate_gimbal = gimbal_dcm.transposed() * _vehicle_dcm * aircraft.get_gyro(); + _gimbal.set_demanded_rates(vehicle_rate_gimbal); + } + _gimbal.update(aircraft); +} + +void MAVLinkGimbalv2::send_attitude_status() +{ + if (!_seen_autopilot_heartbeat) { + return; + } + + // report actual gimbal attitude from physics model + Matrix3f gimbal_dcm; + _gimbal.get_dcm(gimbal_dcm); + + uint16_t flags; + Quaternion q; + if (_target.valid && _target.yaw_is_ef) { + // earth-frame target: report actual gimbal DCM so convergence is visible + flags = GIMBAL_DEVICE_FLAGS_ROLL_LOCK | + GIMBAL_DEVICE_FLAGS_PITCH_LOCK | + GIMBAL_DEVICE_FLAGS_YAW_LOCK; + q.from_rotation_matrix(gimbal_dcm); + } else if (_target.valid && !_target.yaw_is_ef && !_target.is_rate) { + // body-frame angle target: report the commanded body-relative attitude + // directly, matching how servo mounts report commanded angles + flags = 0; + q = _target.attitude; + } else { + // neutral or rate control: report body-relative attitude from DCM + flags = 0; + const Matrix3f body_to_gimbal = _vehicle_dcm.transposed() * gimbal_dcm; + q.from_rotation_matrix(body_to_gimbal); + } + + mavlink_gimbal_device_attitude_status_t status {}; + status.target_system = _vehicle_system_id; + status.target_component = _vehicle_component_id; + status.time_boot_ms = AP_HAL::millis(); + status.flags = flags; + status.q[0] = q.q1; // w + status.q[1] = q.q2; // x + status.q[2] = q.q3; // y + status.q[3] = q.q4; // z + status.angular_velocity_x = 0.0f; + status.angular_velocity_y = 0.0f; + status.angular_velocity_z = 0.0f; + status.failure_flags = 0; + + mavlink_message_t msg; + mavlink_msg_gimbal_device_attitude_status_encode_status( + _vehicle_system_id, _compid, + &mav.status, &msg, &status); + send_mavlink_message(msg); +} + +void MAVLinkGimbalv2::send_command_ack(uint8_t target_sysid, uint8_t target_compid, + MAV_CMD command, MAV_RESULT result) +{ + mavlink_command_ack_t ack {}; + ack.command = (uint16_t)command; + ack.result = (uint8_t)result; + ack.progress = 255; + ack.result_param2 = 0; + ack.target_system = target_sysid; + ack.target_component = target_compid; + + mavlink_message_t msg; + mavlink_msg_command_ack_encode_status( + _vehicle_system_id, _compid, + &mav.status, &msg, &ack); + send_mavlink_message(msg); +} + +void MAVLinkGimbalv2::update(const Aircraft &aircraft) +{ + if (!init_sitl_pointer()) { + return; + } + + update_input(); + update_gimbal(aircraft); + + const uint32_t now_ms = AP_HAL::millis(); + + if (now_ms - _last_heartbeat_ms >= 1000) { + _last_heartbeat_ms = now_ms; + send_heartbeat(); + } + + if (now_ms - _last_attitude_status_ms >= 100) { + _last_attitude_status_ms = now_ms; + send_attitude_status(); + } +} + +} // namespace SITL + +#endif // AP_SIM_MAVLINKGIMBALV2_ENABLED diff --git a/libraries/SITL/SIM_MAVLinkGimbalv2.h b/libraries/SITL/SIM_MAVLinkGimbalv2.h new file mode 100644 index 0000000000000..0dd4aad27611a --- /dev/null +++ b/libraries/SITL/SIM_MAVLinkGimbalv2.h @@ -0,0 +1,101 @@ +/* + Simulator for MAVLink Gimbal Protocol v2 peripherals + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter \ + -A --serial5=sim:avt_cm62_gimbal --speedup=1 + +param set MNT1_TYPE 6 # MAVLink +param set CAM1_TYPE 4 # mount +param set SERIAL5_PROTOCOL 2 # MAVLink2 +reboot + +long REQUEST_MESSAGE 259 # CAMERA_INFORMATION +status *CAM* +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_MAVLINKGIMBALV2_ENABLED + +#include "SIM_Mount.h" +#include "SIM_Gimbal.h" +#include +#include +#include + +namespace SITL { + +/* + Base class for Gimbal Protocol v2 simulators. Handles all MAVLink + framing; subclasses supply device-specific identity data via + pure-virtual accessors. +*/ +class MAVLinkGimbalv2 : public Mount { +public: + void update(const class Aircraft &aircraft) override; + void set_instance(uint8_t instance) override; + +protected: + // device identity — every subclass must implement all of these + virtual const char *get_vendor_name() const = 0; + virtual const char *get_model_name() const = 0; + virtual uint32_t get_firmware_version() const = 0; + virtual uint16_t get_cap_flags() const = 0; + virtual float get_pitch_min_rad() const = 0; + virtual float get_pitch_max_rad() const = 0; + virtual float get_yaw_min_rad() const = 0; + virtual float get_yaw_max_rad() const = 0; + +protected: + // physics model shared by all MAVLink gimbal simulators + Gimbal _gimbal; + +private: + void update_input(); + void update_gimbal(const class Aircraft &aircraft); + void handle_message(const mavlink_message_t &msg); + + void send_heartbeat(); + void send_gimbal_device_information(); + void send_attitude_status(); + void send_command_ack(uint8_t target_sysid, uint8_t target_compid, + MAV_CMD command, MAV_RESULT result); + void send_mavlink_message(const mavlink_message_t &msg); + + uint8_t _compid {MAV_COMP_ID_GIMBAL}; + + bool _seen_autopilot_heartbeat; + uint8_t _vehicle_system_id; + uint8_t _vehicle_component_id; + + uint32_t _last_heartbeat_ms; + uint32_t _last_attitude_status_ms; + + // current demand from the autopilot, set by GIMBAL_DEVICE_SET_ATTITUDE + struct { + bool valid; // true once a command has been received + bool is_rate; // true = rate command, false = angle command + bool yaw_is_ef; // true = rates/attitude expressed in earth frame + Vector3f rates_rads; // demanded body rates (is_rate == true) + Quaternion attitude; // desired attitude (is_rate == false) + } _target; + + struct { + mavlink_message_t rxmsg; + mavlink_status_t status; + } mav; + + Matrix3f _vehicle_dcm; + + // ROI location tracking: set by COMMAND_INT DO_SET_ROI_LOCATION + struct { + bool valid; + Location loc; + } _roi; +}; + +} // namespace SITL + +#endif // AP_SIM_MAVLINKGIMBALV2_ENABLED diff --git a/libraries/SITL/SIM_Mount.h b/libraries/SITL/SIM_Mount.h new file mode 100644 index 0000000000000..b945dc3ae8ce5 --- /dev/null +++ b/libraries/SITL/SIM_Mount.h @@ -0,0 +1,36 @@ +/* + Base class for simulated mount (gimbal) serial peripherals +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_MOUNT_ENABLED + +#include "SIM_SerialDevice.h" + +namespace SITL { + +/* + common base for all simulated mount backends. Inherits SerialDevice + so subclasses can be registered with create_serial_sim() and driven + via a serial port. +*/ +class Mount : public SerialDevice { +public: + + // called each simulation step + virtual void update(const class Aircraft &aircraft) = 0; + + // called before add_gimbal_sim() so subclasses know their + // 0-based instance index (used, e.g., to select a MAVLink component ID) + virtual void set_instance(uint8_t instance) { _instance = instance; } + +protected: + uint8_t _instance; +}; + +} // namespace SITL + +#endif // AP_SIM_MOUNT_ENABLED diff --git a/libraries/SITL/SIM_SensAItion.cpp b/libraries/SITL/SIM_SensAItion.cpp new file mode 100644 index 0000000000000..b55bd74ff0663 --- /dev/null +++ b/libraries/SITL/SIM_SensAItion.cpp @@ -0,0 +1,398 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +/* + Simulate SensAItion serial IMU device + Converts SITL flight dynamics to SensAItion protocol packets +*/ + +#include "SIM_config.h" + +#if AP_SIM_EAHRS_SENSATION_ENABLED + +#include "SIM_SensAItion.h" +#include +#include +#include +#include +#include // Required for put_beXX_ptr +#include // Required for struct timeval + +using namespace SITL; + +// Constants +// Gyro: SITL provides deg/s. Driver expects uDeg/s. +// Factor = 1e6. (NOT RAD_TO_UDEG!) +const float GYRO_NOISE_DEG = 0.02f; + +// Baro: SITL provides mhPa +// std 100 mhPa +const float BARO_NOISE_MHPA = 100.0f; + +int sim_log_counter = 0; +static float rand_float_noise() +{ + return ((float)rand() / (float)RAND_MAX) * 2.0f - 1.0f; +} +// [CPO FIX] Time Sync Helper +// Anchors the simulation timestamp to the absolute UTC start time of the SITL run. +static void simulation_timeval(struct timeval *tv) +{ + uint64_t now = AP_HAL::micros64(); + static uint64_t first_usec; + static struct timeval first_tv; + if (first_usec == 0) { + first_usec = now; + first_tv.tv_sec = AP::sitl()->start_time_UTC; + } + *tv = first_tv; + tv->tv_sec += now / 1000000ULL; + uint64_t new_usec = tv->tv_usec + (now % 1000000ULL); + tv->tv_sec += new_usec / 1000000ULL; + tv->tv_usec = new_usec % 1000000ULL; +} + +static uint32_t get_gps_tow_ms() +{ + struct timeval tv; + gettimeofday(&tv, nullptr); // Get host system time (simulated wall clock) + + // Unix Epoch (1970) vs GPS Epoch (1980) offset is ~315964800 seconds + // But simplistic SITL often just needs "seconds since Sunday". + // Unix Epoch was a Thursday. + // +3 days (259200 sec) aligns the modulo to Sunday. + // 18 leap seconds (approx) for current time. + + // However, the most robust way in ArduPilot SITL is to use the + // simulation start time which is aligned to the host clock. + + uint64_t now_us = AP_HAL::micros64(); + double sim_time_sec = (double)AP::sitl()->start_time_UTC + (now_us * 1.0e-6); + + // GPS leap seconds (18 as of 2024) + // GPS time is ahead of UTC. + sim_time_sec += 18; + + // Seconds in week + uint32_t seconds_in_week = (uint32_t)sim_time_sec % 604800; + uint32_t ms_part = (now_us / 1000) % 1000; + + return (seconds_in_week * 1000) + ms_part; +} + +SensAItion::SensAItion(bool interleaved_mode) : SerialDevice::SerialDevice() +{ + _interleaved_mode = interleaved_mode; +} + +void SensAItion::update(void) +{ + int tick1kHz = (AP_HAL::micros() + 500) / 1000; + if (tick1kHz <= _tick) { + return; + } + _tick = tick1kHz; + + char trash_buf[64]; + read_from_autopilot(trash_buf, sizeof(trash_buf)); + + // [CPO FIX] The "Gentle Start" Delay + // Wait 2 seconds after boot to allow ArduPilot to initialize + // serial ports and parameters before we flood it with data. + // This prevents race conditions during the reboot test. + if (AP_HAL::millis() < 1000) { + return; + } + + if (!init_sitl_pointer()) { + return; + } + + const auto &fdm = _sitl->state; + + // 1. Packet 0: IMU + if ((_tick % _periodMessage0) == _phaseMessage0) { + send_packet_0_imu(fdm); + } + + // 2. Interleaved-only Packets + if (_interleaved_mode) { + + // Packet 1: Orientation + if ((_tick % _periodMessage1) == _phaseMessage1) { + send_packet_1_orientation(fdm); + } + + // Packet 2: INS + if ((_tick % _periodMessage2) == _phaseMessage2) { + send_packet_2_ins(fdm); + } + } + flush_packets(); +} + +void SensAItion::send_packet_0_imu(const struct sitl_fdm &fdm) +{ + + // Convert from SI units to SensAItion units + // Acceleration: m/s² to µg (1e-6 g) + const float gravity = 9.80665f; + int32_t accel_x = (int32_t)(fdm.xAccel / gravity * 1e6); + int32_t accel_y = (int32_t)(fdm.yAccel / gravity * 1e6); + int32_t accel_z = (int32_t)(fdm.zAccel / gravity * 1e6); + + // Angular rates: deg/s to µdeg/s (1e-6 deg/s) + int32_t gyro_x = (int32_t)((fdm.rollRate + (rand_float_noise() * GYRO_NOISE_DEG)) * 1e6); + int32_t gyro_y = (int32_t)((fdm.pitchRate + (rand_float_noise() * GYRO_NOISE_DEG)) * 1e6); + int32_t gyro_z = (int32_t)((fdm.yawRate + (rand_float_noise() * GYRO_NOISE_DEG)) * 1e6); + + // Temperature: special conversion formula + float temp_c = 25.0f; + int16_t temperature = (int16_t)((temp_c - 20.0f) / 0.008f); + + // Magnetometer: Gauss to mGauss + int16_t mag_x = (int16_t)(fdm.bodyMagField.x * 1000); + int16_t mag_y = (int16_t)(fdm.bodyMagField.y * 1000); + int16_t mag_z = (int16_t)(fdm.bodyMagField.z * 1000); + + // Barometer: Calculate pressure from altitude (Pa to 0.1 Pa units) + const float pressure_pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude); + int32_t baro = (int32_t)(pressure_pa * 10.0f + rand_float_noise() * BARO_NOISE_MHPA); + + // Periodic status output to verify simulator operation + static uint32_t imu_count = 0; + if (++imu_count % 1000 == 0) { + ::printf("SensAItion: IMU packet #%" PRIu32 " sent - accel_x=%" PRId32 " µg, gyro_x=%" PRId32 " µdeg/s\n", + imu_count, accel_x, gyro_x); + } + + // Pack data in big-endian format (36 bytes data) + uint8_t pkt[36]; + put_be32_ptr(&pkt[0], accel_x); + put_be32_ptr(&pkt[4], accel_y); + put_be32_ptr(&pkt[8], accel_z); + put_be32_ptr(&pkt[12], gyro_x); + put_be32_ptr(&pkt[16], gyro_y); + put_be32_ptr(&pkt[20], gyro_z); + put_be16_ptr(&pkt[24], temperature); + put_be16_ptr(&pkt[26], mag_x); + put_be16_ptr(&pkt[28], mag_y); + put_be16_ptr(&pkt[30], mag_z); + put_be32_ptr(&pkt[32], baro); + + if (_interleaved_mode) { + write_packet(0x00, pkt, sizeof(pkt)); + // --- DETAILED LOGGING (SIM SIDE) --- + // if (sim_log_counter++ % 400 == 0) { + // fprintf(stderr, "[SIM-OUT] IMU Packet (Interleaved Mode: %d)\n", _interleaved_mode); + // fprintf(stderr, " Acc(ug): X=%d Y=%d Z=%d\n", (int)accel_x, (int)accel_y, (int)accel_z); + // fprintf(stderr, " Gyr(ud): X=%d Y=%d Z=%d\n", (int)gyro_x, (int)gyro_y, (int)gyro_z); + // fprintf(stderr, " Mag(mG): X=%d Y=%d Z=%d\n", (int)mag_x, (int)mag_y, (int)mag_z); + // fprintf(stderr, " Bar(0.1Pa): %d | Temp(raw): %d\n", (int)baro, (int)temperature); + // } + } else { + write_legacy_packet(pkt, sizeof(pkt)); + //--- DETAILED LOGGING (SIM SIDE) --- + //if (sim_log_counter++ % 400 == 0) { + // fprintf(stderr, "[SIM-OUT] IMU Packet (Legacy Mode: %d)\n", _interleaved_mode); + // fprintf(stderr, " Acc(ug): X=%d Y=%d Z=%d\n", (int)accel_x, (int)accel_y, (int)accel_z); + // fprintf(stderr, " Gyr(ud): X=%d Y=%d Z=%d\n", (int)gyro_x, (int)gyro_y, (int)gyro_z); + // fprintf(stderr, " Mag(mG): X=%d Y=%d Z=%d\n", (int)mag_x, (int)mag_y, (int)mag_z); + // fprintf(stderr, " Bar(0.1Pa): %d | Temp(raw): %d\n", (int)baro, (int)temperature); + //} + } + +} + +void SensAItion::send_packet_1_orientation(const struct sitl_fdm &fdm) +{ + // Quaternion scaled by 1e6 + int32_t q0 = (int32_t)(fdm.quaternion.q1 * 1.0e6f); // W + int32_t q1 = (int32_t)(fdm.quaternion.q2 * 1.0e6f); // X + int32_t q2 = (int32_t)(fdm.quaternion.q3 * 1.0e6f); // Y + int32_t q3 = (int32_t)(fdm.quaternion.q4 * 1.0e6f); // Z + + uint8_t pkt[16]; + put_be32_ptr(&pkt[0], (uint32_t)q0); + put_be32_ptr(&pkt[4], (uint32_t)q1); + put_be32_ptr(&pkt[8], (uint32_t)q2); + put_be32_ptr(&pkt[12], (uint32_t)q3); + + // --- LOGGING PROBE (SIM SIDE) --- + // if (sim_log_counter % 100 == 0) { // Match Parser 1Hz rate + // fprintf(stderr, "[SIM-OUT] AHRS Packet (ID 0x01)\n"); + // fprintf(stderr, " Quat(1e-6): W=%d X=%d Y=%d Z=%d\n", q0, q1, q2, q3); + // } + + write_packet(0x01, pkt, sizeof(pkt)); +} + +// NOTE: calculate_itow removed in favor of simulation_timeval inline logic + +void SensAItion::send_packet_2_ins(const struct sitl_fdm &fdm) +{ + // --- 1. PREPARE DATA --- + uint8_t align_status = 1; // 1 = Aligned + uint8_t gnss1_fix = 3; // 3D Fix + uint8_t gnss2_fix = 3; + + uint32_t num_sats = 0x0C0C0C0C; + + // [CPO FIX] Calculate accurate GPS Time of Week using the SITL wall clock + // Replaces previous calculate_itow() + struct timeval tv; + simulation_timeval(&tv); + + // Convert TV (Seconds+Micros) to GPS Time of Week (ms) + // We assume SITL starts somewhat recently. + // Calculate seconds since Sunday 00:00:00 UTC + //uint32_t seconds_in_week = 604800; + //uint32_t tow_ms = (tv.tv_sec % seconds_in_week) * 1000 + (tv.tv_usec / 1000); + uint32_t itow = get_gps_tow_ms(); + + // Position (deg -> 1e-7 deg) + int32_t lat = (int32_t)(fdm.latitude * 1.0e7); + int32_t lon = (int32_t)(fdm.longitude * 1.0e7); + + // Velocity (m/s -> mm/s) + int32_t vel_n = (int32_t)(fdm.speedN * 1000.0f); + int32_t vel_e = (int32_t)(fdm.speedE * 1000.0f); + int32_t vel_d = (int32_t)(fdm.speedD * 1000.0f); + + // Altitude (m -> mm) + int32_t alt_mm = (int32_t)(fdm.altitude * 1000.0); + + // Accuracy (mm, mm/s) + int32_t acc_lat_mm = 100; // 0.1m + int32_t acc_lon_mm = 100; // 0.1m + int32_t acc_vn_mm = 20; + int32_t acc_ve_mm = 20; + int32_t acc_vd_mm = 20; + int32_t acc_vd_pos_mm = 100; + uint32_t err_flags = 0; + uint8_t sensor_valid = 0xFF; // All valid + + // Date + uint16_t year = 2025; + uint16_t month = 12; + uint8_t day = 7; + + // --- 2. PACKING (Big Endian - 69 Bytes) --- + uint8_t pkt[69]; + + // Bytes 0-3: Num Sats (4B) + put_be32_ptr(&pkt[0], num_sats); + + // Bytes 4-7: Error Flags (4B) + put_be32_ptr(&pkt[4], err_flags); + + // Byte 8: Sensor Valid (1B) + pkt[8] = sensor_valid; + + // Bytes 9-12: Latitude (4B) + put_be32_ptr(&pkt[9], (uint32_t)lat); + + // Bytes 13-16: Longitude (4B) + put_be32_ptr(&pkt[13], (uint32_t)lon); + + // Bytes 17-28: Velocity N, E, D (12B) + put_be32_ptr(&pkt[17], (uint32_t)vel_n); + put_be32_ptr(&pkt[21], (uint32_t)vel_e); + put_be32_ptr(&pkt[25], (uint32_t)vel_d); + + // Bytes 29-32: Altitude MSL (4B) + put_be32_ptr(&pkt[29], (uint32_t)alt_mm); + + // Byte 33: Alignment Status (1B) + pkt[33] = align_status; + + // Bytes 34-37: Time iTOW (4B) + put_be32_ptr(&pkt[34], itow); + + // 38-39: GNSS Fix + pkt[38] = gnss1_fix; + pkt[39] = gnss2_fix; + + // 40-44: UTC Date/Time + put_be16_ptr(&pkt[40], year); + put_be16_ptr(&pkt[42], month); + pkt[44] = day; + + // 45-68: Accuracy Metrics (mm or mm/s) + put_be32_ptr(&pkt[45], acc_lat_mm); + put_be32_ptr(&pkt[49], acc_lon_mm); + put_be32_ptr(&pkt[53], acc_vn_mm); + put_be32_ptr(&pkt[57], acc_ve_mm); + put_be32_ptr(&pkt[61], acc_vd_mm); + put_be32_ptr(&pkt[65], acc_vd_pos_mm); + + // --- LOGGING PROBE (SIM SIDE) --- + // if (sim_log_counter % 100 == 0) { + // fprintf(stderr, "[SIM-OUT] INS Packet (iTOW: %u)\n", itow); + // fprintf(stderr, " Pos: Lat=%d Lon=%d Alt=%d mm\n", lat, lon, alt_mm); + // fprintf(stderr, " Vel: N=%d E=%d D=%d mm/s\n", vel_n, vel_e, vel_d); + // fprintf(stderr, " Stat: Align=%d Valid=0x%02X\n", align_status, sensor_valid); + // } + + + write_packet(0x02, pkt, sizeof(pkt)); +} + + +void SensAItion::flush_packets() +{ + if (_buffert_cnt > 0) { + write_to_autopilot((const char *)&_buffert, _buffert_cnt); + _buffert_cnt = 0; + } +} + +void SensAItion::write_to_autopilot_buf(const char *data, int length) +{ + memcpy(&_buffert[_buffert_cnt], data, length); + _buffert_cnt += length; +} + +void SensAItion::write_packet(uint8_t msg_id, const uint8_t* payload, uint16_t length) +{ + const uint8_t header = 0xFA; + write_to_autopilot_buf((const char *)&header, 1); + write_to_autopilot_buf((const char *)&msg_id, 1); + write_to_autopilot_buf((const char *)payload, length); + uint8_t crc = (uint8_t)calculate_crc(msg_id, payload, length, true); + write_to_autopilot_buf((const char *)&crc, 1); +} + +void SensAItion::write_legacy_packet(const uint8_t* payload, uint16_t length) +{ + const uint8_t header = 0xFA; + write_to_autopilot_buf((const char *)&header, 1); + write_to_autopilot_buf((const char *)payload, length); + uint8_t crc = (uint8_t)calculate_crc(0, payload, length, false); + write_to_autopilot_buf((const char *)&crc, 1); +} + +uint16_t SensAItion::calculate_crc(uint8_t msg_id, const uint8_t* payload, uint16_t length, bool use_id) +{ + uint8_t crc = 0; + if (use_id) { + crc ^= msg_id; + } + for (uint16_t i = 0; i < length; i++) { + crc ^= payload[i]; + } + return crc; +} + +#endif // AP_SIM_EAHRS_SENSATION_ENABLED \ No newline at end of file diff --git a/libraries/SITL/SIM_SensAItion.h b/libraries/SITL/SIM_SensAItion.h new file mode 100644 index 0000000000000..6f95d440e475a --- /dev/null +++ b/libraries/SITL/SIM_SensAItion.h @@ -0,0 +1,67 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +/* + Simulate SensAItion serial IMU/AHRS device + Generates high-rate sensor data for ArduPilot testing + +*/ + +#pragma once + +#include "SIM_SerialDevice.h" +#include + +namespace SITL +{ + +class SensAItion : public SerialDevice +{ +public: + SensAItion(bool interleved_mode); + void update(void); + +private: + int _tick = 0; + int _periodMessage0 = 5; + int _phaseMessage0 = 0; + int _periodMessage1 = 100; + int _phaseMessage1 = 0; + int _periodMessage2 = 100; + int _phaseMessage2 = 5; + + uint8_t _buffert[512]; + int _buffert_cnt = 0; + + + void send_packet_0_imu(const struct sitl_fdm &fdm); + void send_packet_1_orientation(const struct sitl_fdm &fdm); + void send_packet_2_ins(const struct sitl_fdm &fdm); + uint32_t calculate_itow(uint64_t now_us, uint32_t start_time_utc); + + + void flush_packets(); + void write_to_autopilot_buf(const char *data, int length); + + void write_packet(uint8_t msg_id, const uint8_t* payload, uint16_t length); + void write_legacy_packet(const uint8_t* payload, uint16_t length); + uint16_t calculate_crc(uint8_t msg_id, const uint8_t* payload, uint16_t length, bool use_id); + + uint32_t last_update_us = 0; + uint32_t tick_count = 0; + + bool _interleaved_mode = false; // Now mutable, Shall be driven by EAHRS_OPTIONS. TODO +}; + +} // namespace SITL diff --git a/libraries/SITL/SIM_Siyi.cpp b/libraries/SITL/SIM_Siyi.cpp new file mode 100644 index 0000000000000..139a7989c72e8 --- /dev/null +++ b/libraries/SITL/SIM_Siyi.cpp @@ -0,0 +1,248 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + Base class for for Siyi camera simulaors +*/ + +#include "SIM_config.h" + +#if AP_SIM_SIYI_ENABLED + +#include "SIM_Siyi.h" + +#include +#include +#include + +using namespace SITL; + +void Siyi::move_preamble_in_buffer(uint8_t search_start_pos) +{ + uint8_t i; + for (i=search_start_pos; i 60) { + // the number is arbitrary and based off our current + // driver; if you're adding support for longer messages + // just change it. + AP_HAL::panic("Bad length %u", received_length); + } + } + FALLTHROUGH; + case 4: + case 3: // validate control field + if (msg.packed_empty.ctrl != 1) { + if (strict_parsing) { + AP_HAL::panic("Bad control"); + } + move_preamble_in_buffer(3); + return; + } + FALLTHROUGH; + case 2: // validate HEADER2 + if (msg.packed_empty.header2 != HEADER2) { + if (strict_parsing) { + AP_HAL::panic("bad HEADER2"); + } + move_preamble_in_buffer(1); + return; + } + FALLTHROUGH; + case 1: // validate HEADER1 + if (msg.packed_empty.header1 != HEADER1) { + if (strict_parsing) { + AP_HAL::panic("bad HEADER1"); + } + move_preamble_in_buffer(1); + return; + } + break; + case 0: + return; + } + + // this sanity check so we don't use the length fields when + // they're not valid: + if (buflen < sizeof(msg.packed_empty)) { + return; + } + + const uint16_t datalen = le16toh(msg.packed_empty.datalen); + const uint16_t totallen = sizeof(msg.packed_empty) + datalen; + if (totallen > sizeof(msg.buffer)) { + if (strict_parsing) { + AP_HAL::panic("bad datalen"); + } + move_preamble_in_buffer(1); + return; + } + + if (buflen < totallen) { + if (strict_parsing) { + AP_HAL::panic("bad datalen"); + } + move_preamble_in_buffer(1); + return; + } + + const uint16_t calculated_checksum = msg.packed_empty.calculate_checksum(totallen-2); // -2 to omit checksum bytes + const uint16_t received_checksum = UINT16_VALUE(msg.buffer[totallen-1], msg.buffer[totallen-2]); + if (calculated_checksum != received_checksum) { + if (strict_parsing) { + AP_HAL::panic("bad checksum"); + } + move_preamble_in_buffer(1); + return; + } + + if (strict_parsing) { + const uint16_t received_seq = le16toh(msg.packed_empty.seq); + // printf("received_seq=%u\n", unsigned(received_seq)); + if (received_seq != expected_seq) { + AP_HAL::panic("Bad sequence (got=%u vs want=%u)", msg.packed_empty.seq, expected_seq+1); + } + expected_seq = received_seq + 1; + } + + handle_received_message(); + + move_preamble_in_buffer(totallen); +} + +void Siyi::update(const class Aircraft &aircraft) +{ + update_input(); + // update_output(location); +} + +#endif // AP_SIM_SIYI_ENABLED diff --git a/libraries/SITL/SIM_Siyi.h b/libraries/SITL/SIM_Siyi.h new file mode 100644 index 0000000000000..bf5740d5624c5 --- /dev/null +++ b/libraries/SITL/SIM_Siyi.h @@ -0,0 +1,220 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + Base class for Siyi camera simulators +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_SIYI_ENABLED + +#include "SIM_Mount.h" + +#include +#include +#include + +namespace SITL { + +class Siyi : public Mount { +public: + + void update(const class Aircraft &aircraft) override; + +protected: + + static constexpr uint8_t HEADER1 { 0x55 }; + static constexpr uint8_t HEADER2 { 0x66 }; + + template + class PACKED PackedMessage { + public: + PackedMessage(T _msg) : + msg(_msg) + { + update_checksum(); + } + uint8_t header1 { HEADER1 }; + uint8_t header2 { HEADER2 }; + uint8_t ctrl; + uint16_t datalen {sizeof(T)-1 }; // -1 to exclude command ID + uint16_t seq; + T msg; + uint16_t checksum; + + uint16_t calculate_checksum(uint16_t len) const WARN_IF_UNUSED { + return crc16_ccitt((uint8_t*)this, len, 0); + } + uint16_t calculate_checksum() const WARN_IF_UNUSED { + return calculate_checksum(sizeof(*this)-2); + } + + void update_checksum() { + checksum = calculate_checksum(); + } + + bool response_requested() const { return (ctrl & 0x1) == 0x1; } + }; + + // message ids + enum class CommandID : uint8_t { + ACQUIRE_FIRMWARE_VERSION = 0x01, + HARDWARE_ID = 0x02, + // AUTO_FOCUS = 0x04, + // MANUAL_ZOOM_AND_AUTO_FOCUS = 0x05, + // MANUAL_FOCUS = 0x06, + GIMBAL_ROTATION = 0x07, + // CENTER = 0x08, + ACQUIRE_GIMBAL_CONFIG_INFO = 0x0A, + // FUNCTION_FEEDBACK_INFO = 0x0B, + PHOTO = 0x0C, + ACQUIRE_GIMBAL_ATTITUDE = 0x0D, + // ABSOLUTE_ZOOM = 0x0F, + SET_CAMERA_IMAGE_TYPE = 0x11, + GET_TEMP_FULL_IMAGE = 0x14, + READ_RANGEFINDER = 0x15, + EXTERNAL_ATTITUDE = 0x22, + SET_TIME = 0x30, + POSITION_DATA = 0x3e, + }; + + class PACKED BaseMessage { + public: + BaseMessage(CommandID _cmdid) : + cmdid{uint8_t(_cmdid)} + { } + uint8_t cmdid; + }; + + class PACKED FirmwareVersionRequest : public BaseMessage { }; + virtual void handle_message(const PackedMessage &request) = 0; + + class PACKED HardwareIDRequest : public BaseMessage { }; + virtual void handle_message(const PackedMessage &request) = 0; + + class PACKED GimbalAttitudeRequest : public BaseMessage { }; + virtual void handle_message(const PackedMessage &request) = 0; + + class PACKED ExternalAttitude : public BaseMessage { + public: + uint32_t time_boot_ms; + float roll, pitch, yaw; + float rollspeed, pitchspeed, yawspeed; + }; + virtual void handle_message(const PackedMessage &request) { + AP_HAL::panic("Unhandled ExternalAttitude message"); + } + + class PACKED SetCameraImageType : public BaseMessage { }; + virtual void handle_message(const PackedMessage &request) { + AP_HAL::panic("Unhandled SetCameraImageType message"); + } + + class PACKED Photo : public BaseMessage { + public: + uint8_t command; + }; + virtual void handle_message(const PackedMessage &request) { + AP_HAL::panic("Unhandled Photo message"); + } + + class PACKED AcquireGimbalConfigInfo : public BaseMessage { + public: + }; + virtual void handle_message(const PackedMessage &request) { + AP_HAL::panic("Unhandled AcquireGimbalConfigInfo message"); + } + + class PACKED GetTempFullImageRequest : public BaseMessage { + public: + uint8_t mode; + }; + virtual void handle_message(const PackedMessage &request) { } + + class PACKED ReadRangefinderRequest : public BaseMessage { }; + virtual void handle_message(const PackedMessage &request) { } + + class PACKED GimbalRotation : public BaseMessage { + public: + uint8_t yaw_rate_pct; + uint8_t pitch_rate_pct; + }; + virtual void handle_message(const PackedMessage &request) { + AP_HAL::panic("Unhandled GimbalRotation message"); + } + + class PACKED PositionData : public BaseMessage { + public: + uint32_t time_boot_ms; + int32_t lat, lon; + int32_t alt_msl, alt_ellipsoid; + int32_t velocity_ned_int32[3]; + }; + virtual void handle_message(const PackedMessage &request) { + AP_HAL::panic("Unhandled PositionData message"); + } + + class PACKED SetTime : public BaseMessage { + public: + uint64_t epoch_us; + }; + virtual void handle_message(const PackedMessage &request) { + AP_HAL::panic("Unhandled SetTime message"); + } + + /* + * Input Handling + */ + void update_input(); + void handle_received_message(); + uint32_t expected_message_length(CommandID id) const; + + union u { + u() {} + uint8_t buffer[256]; // from-autopilot + PackedMessage packed_empty; + PackedMessage packed_firmwareversionrequest; + PackedMessage packed_hardwareidrequest; + PackedMessage packed_gimbalattituderequest; + PackedMessage packed_externalattitude; + PackedMessage packed_setcameraimgetype; + PackedMessage packed_photo; + PackedMessage packed_acquiregimbalconfiginfo; + PackedMessage packed_gettempfullimage; + PackedMessage packed_readrangefinder; + PackedMessage packed_gimbalrotation; + PackedMessage packed_positiondata; + PackedMessage packed_settime; + } msg; + uint8_t buflen; + + void move_preamble_in_buffer(uint8_t search_start_pos=0); + + /* + * Output Handling + */ + void update_output(const Location &location); + + uint16_t expected_seq; + + static constexpr bool strict_parsing = true; + +}; // end class Siyi + +}; // end namespace SITL + +#endif // AP_SIM_SIYI_ENABLED diff --git a/libraries/SITL/SIM_Siyi_ZT30.cpp b/libraries/SITL/SIM_Siyi_ZT30.cpp new file mode 100644 index 0000000000000..96e709c00502e --- /dev/null +++ b/libraries/SITL/SIM_Siyi_ZT30.cpp @@ -0,0 +1,192 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + Simulator for Siyi ZT30 + + */ + +#include "SIM_config.h" + +#if AP_SIM_SIYI_ZT30_ENABLED + +#include "SIM_Siyi_ZT30.h" + +using namespace SITL; + +void Siyi_ZT30::update(const Aircraft &aircraft) +{ + gimbal.update(aircraft); + + // base class update function: + Siyi::update(aircraft); +} + +void Siyi_ZT30::handle_message(const PackedMessage &request) +{ + if (!request.response_requested()) { + // weird + AP_HAL::panic("response not requested?"); + } + const PackedMessage response{ + FirmwareVersion{ + camera_major, camera_minor, camera_patch, + gimbal_major, gimbal_minor, gimbal_patch, + zoom_major, zoom_minor, zoom_patch + }, + }; + write_to_autopilot((char*)&response, sizeof(response)); +} + +void Siyi_ZT30::handle_message(const PackedMessage &request) +{ + if (!request.response_requested()) { + // weird + AP_HAL::panic("response not requested?"); + } + const PackedMessage response{ + HardwareID{ + hwid_a, hwid_b + }, + }; + write_to_autopilot((char*)&response, sizeof(response)); +} + +void Siyi_ZT30::handle_message(const PackedMessage &request) +{ + if (!request.response_requested()) { + // weird + AP_HAL::panic("response not requested?"); + } + + uint32_t delta_time_us; + Vector3f delta_angles; + Vector3f delta_velocities; + gimbal.get_deltas(delta_angles, delta_velocities, delta_time_us); + const Vector3 rates_decidegrees( + (delta_angles.x * RAD_TO_DEG * 1e6 / delta_time_us), + delta_angles.y * RAD_TO_DEG * 1e6 / delta_time_us, + delta_angles.z * RAD_TO_DEG * 1e6 / delta_time_us + ); + + Vector3f joint_angles; + gimbal.get_joint_angles(joint_angles); + const Vector3 joint_angles_decidegrees( + joint_angles.x * 10 * RAD_TO_DEG, + joint_angles.y * 10 * RAD_TO_DEG, + joint_angles.z * 10 * RAD_TO_DEG + ); + + const PackedMessage response{ + GimbalAttitude{ + joint_angles_decidegrees.x, joint_angles_decidegrees.y, joint_angles_decidegrees.z, + rates_decidegrees.x, rates_decidegrees.y, rates_decidegrees.z + }, + }; + write_to_autopilot((char*)&response, sizeof(response)); +} + +void Siyi_ZT30::handle_message(const PackedMessage &request) +{ + // MAVProxy and the ArduPilot driver both request an ACK but the + // cameras don't seem to supply it. + + // if (request.ctrl & 0x1) { + // AP_HAL::panic("Response requested for ExternalAttitude?"); + // } +} + +void Siyi_ZT30::handle_message(const PackedMessage &request) +{ + // MAVProxy and the ArduPilot driver both request an ACK but the + // cameras don't seem to supply it. + + // if (request.ctrl & 0x1) { + // AP_HAL::panic("Response requested for SetCameraImageType?"); + // } +} + +void Siyi_ZT30::handle_message(const PackedMessage &request) +{ + // MAVProxy and the ArduPilot driver both request an ACK but the + // cameras don't seem to supply it. + + // if (request.ctrl & 0x1) { + // AP_HAL::panic("Response requested for SetCameraImageType?"); + // } +} + +void Siyi_ZT30::handle_message(const PackedMessage &request) +{ + // MAVProxy and the ArduPilot driver both request an ACK but the + // cameras don't seem to supply it. + + // if (request.ctrl & 0x1) { + // AP_HAL::panic("Response requested for AcquireGimbalConfigInfo?"); + // } +} + +void Siyi_ZT30::handle_message(const PackedMessage &request) +{ + const float max_demanded_yaw_rate_rad = 2*M_PI; + const float max_demanded_pitch_rate_rad = 2*M_PI; + + const Vector3f demanded_rates{ + max_demanded_yaw_rate_rad * request.msg.yaw_rate_pct * 0.01f, + max_demanded_pitch_rate_rad * request.msg.pitch_rate_pct * 0.01f, + 0 + }; + gimbal.set_demanded_rates(demanded_rates); + + // MAVProxy and the ArduPilot driver both request an ACK but the + // cameras don't seem to supply it. + + // if (request.ctrl & 0x1) { + // AP_HAL::panic("Response requested for GimbalRotation?"); + // } +} + +void Siyi_ZT30::handle_message(const PackedMessage &request) +{ + // MAVProxy and the ArduPilot driver both request an ACK but the + // cameras don't seem to supply it. + + // if (request.ctrl & 0x1) { + // AP_HAL::panic("Response requested for GimbalRotation?"); + // } +} + +void Siyi_ZT30::handle_message(const PackedMessage &request) +{ + const PackedMessage response{ + RangefinderDistance{50}, // 50m simulated distance + }; + write_to_autopilot((char*)&response, sizeof(response)); +} + +void Siyi_ZT30::handle_message(const PackedMessage &request) +{ + epoch_us = request.msg.epoch_us; + + if (!request.response_requested()) { + // weird + return; + } + + const PackedMessage response{ + SetTimeResponse{1}, + }; + write_to_autopilot((char*)&response, sizeof(response)); +} + +#endif // AP_SIM_SIYI_ZT30_ENABLED diff --git a/libraries/SITL/SIM_Siyi_ZT30.h b/libraries/SITL/SIM_Siyi_ZT30.h new file mode 100644 index 0000000000000..3ed6a3be50176 --- /dev/null +++ b/libraries/SITL/SIM_Siyi_ZT30.h @@ -0,0 +1,185 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + Simulator for Siyi ZT30 + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:siyi_zt30 --speedup=1 + +param set MNT1_TYPE 8 # siyi +param set CAM1_TYPE 4 # mount +param set SERIAL5_PROTOCOL 8 # gimbal +reboot + +long REQUEST_MESSAGE CAMERA_INFORMATION +status *CAM* + +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_SIYI_ZT30_ENABLED + +#include "SIM_Siyi.h" +#include "SIM_Gimbal.h" + +namespace SITL { + +class Siyi_ZT30 : public Siyi { +public: + + void update(const Aircraft &aircraft) override; + +protected: + + void handle_message(const PackedMessage &request) override; + void handle_message(const PackedMessage &request) override; + void handle_message(const PackedMessage &request) override; + void handle_message(const PackedMessage &request) override; + void handle_message(const PackedMessage &request) override; + void handle_message(const PackedMessage &request) override; + void handle_message(const PackedMessage &request) override; + void handle_message(const PackedMessage &request) override; + void handle_message(const PackedMessage &request) override; + void handle_message(const PackedMessage &request) override; + void handle_message(const PackedMessage &request) override; + +private: + + // the physical gimbal: + Gimbal gimbal; + + class PACKED FirmwareVersion : Siyi::BaseMessage { + public: + FirmwareVersion( + uint8_t _camera_major, uint8_t _camera_minor, uint8_t _camera_patch, + uint8_t _gimbal_major, uint8_t _gimbal_minor, uint8_t _gimbal_patch, + uint8_t _zoom_major, uint8_t _zoom_minor, uint8_t _zoom_patch + ) : BaseMessage(Siyi::CommandID::ACQUIRE_FIRMWARE_VERSION), + camera(_camera_major << 16 | _camera_minor << 8 | _camera_patch), + gimbal(_gimbal_major << 16 | _gimbal_minor << 8 | _gimbal_patch), + zoom(_zoom_major << 16 | _zoom_minor << 8 | _zoom_patch) + { } + + uint32_t camera; + uint32_t gimbal; + uint32_t zoom; + }; + + class PACKED HardwareID : Siyi::BaseMessage { + public: + HardwareID(uint8_t _a, uint8_t _b) : + BaseMessage(Siyi::CommandID::HARDWARE_ID), + major{_a}, + minor{_b} + { } + + uint8_t major; + uint8_t minor; + }; + + class PACKED GimbalAttitude : Siyi::BaseMessage { + public: + GimbalAttitude(int16_t _yaw_decidegrees, + int16_t _pitch_decidegrees, + int16_t _roll_decidegrees, + int16_t _yaw_rate_decidegrees, + int16_t _pitch_rate_decidegrees, + int16_t _roll_rate_decidegrees + ) : + BaseMessage(Siyi::CommandID::ACQUIRE_GIMBAL_ATTITUDE), + yaw_decidegrees{_yaw_decidegrees}, + pitch_decidegrees{_pitch_decidegrees}, + roll_decidegrees{_roll_decidegrees}, + yaw_rate_decidegreess{_yaw_rate_decidegrees}, + pitch_rate_decidegreess{_pitch_rate_decidegrees}, + roll_rate_decidegreess{_roll_rate_decidegrees} + { } + + int16_t yaw_decidegrees; + int16_t pitch_decidegrees; + int16_t roll_decidegrees; + + int16_t yaw_rate_decidegreess; + int16_t pitch_rate_decidegreess; + int16_t roll_rate_decidegreess; + }; + + class PACKED GimbalConfigInfo : Siyi::BaseMessage { + public: + GimbalConfigInfo(uint8_t _hdr_status, + uint8_t _record_status, + uint8_t _motion_mode, + uint8_t _mounting_dir, + uint8_t _video_mode + ) : + BaseMessage(Siyi::CommandID::ACQUIRE_GIMBAL_CONFIG_INFO), + hdr_status{_hdr_status}, + record_status{_record_status}, + motion_mode{_motion_mode}, + mounting_dir{_mounting_dir}, + video_mode{_video_mode} + { } + + uint8_t reserved1; + uint8_t hdr_status; + uint8_t reserved3; + uint8_t record_status; + uint8_t motion_mode; + uint8_t mounting_dir; + uint8_t video_mode; + }; + + class PACKED RangefinderDistance : Siyi::BaseMessage { + public: + RangefinderDistance(uint16_t _dist_m) : + BaseMessage(Siyi::CommandID::READ_RANGEFINDER), + dist_m{_dist_m} + { } + uint16_t dist_m; + }; + + class PACKED SetTimeResponse : Siyi::BaseMessage { + public: + SetTimeResponse(uint8_t _result) : + BaseMessage(Siyi::CommandID::SET_TIME), + result{_result} + { } + + uint8_t result; + }; + + static constexpr uint8_t hwid_a { '7' }; + static constexpr uint8_t hwid_b { 'A' }; + + static constexpr uint8_t camera_major { 1 }; + static constexpr uint8_t camera_minor { 2 }; + static constexpr uint8_t camera_patch { 3 }; + + static constexpr uint8_t gimbal_major { 4 }; + static constexpr uint8_t gimbal_minor { 5 }; + static constexpr uint8_t gimbal_patch { 6 }; + + static constexpr uint8_t zoom_major { 7 }; + static constexpr uint8_t zoom_minor { 8 }; + static constexpr uint8_t zoom_patch { 9 }; + + uint64_t epoch_us; // from SET_TIME + +}; // end class Siyi_ZT30 +}; // end namespace SITL + +#endif // AP_SIM_SIYI_ZT30_ENABLED diff --git a/libraries/SITL/SIM_Topotek.cpp b/libraries/SITL/SIM_Topotek.cpp new file mode 100644 index 0000000000000..412401e733d70 --- /dev/null +++ b/libraries/SITL/SIM_Topotek.cpp @@ -0,0 +1,268 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + Simulator for Topotek gimbal +*/ + +#include "SIM_config.h" + +#if AP_SIM_TOPOTEK_ENABLED + +#include "SIM_Topotek.h" +#include "SIM_Aircraft.h" +#include +#include + +using namespace SITL; + +void Topotek::update(const Aircraft &aircraft) +{ + // Drive GimbalSim toward the angles commanded via GIP/GIY packets. + // Wire encoding: pitch_cd = -pitch_deg * 100 (driver negates on read), + // yaw_cd = yaw_deg * 100. + { + const float target_pitch_rad = -radians(_commanded_pitch_cd * 0.01f); + const float target_yaw_rad = radians(_commanded_yaw_cd * 0.01f); + + Vector3f ja; + gimbal.get_joint_angles(ja); + + Matrix3f gimbal_dcm; + gimbal.get_dcm(gimbal_dcm); + const Vector3f vehicle_rate_gimbal = gimbal_dcm.transposed() * aircraft.get_dcm() * aircraft.get_gyro(); + + static constexpr float GAIN = 10.0f; + gimbal.set_demanded_rates(Vector3f( + vehicle_rate_gimbal.x, + vehicle_rate_gimbal.y + (target_pitch_rad - ja.y) * GAIN, + vehicle_rate_gimbal.z + (target_yaw_rad - ja.z) * GAIN)); + } + + gimbal.update(aircraft); + update_input(); + + // send attitude at 10 Hz + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - _last_attitude_ms >= 100) { + _last_attitude_ms = now_ms; + send_attitude(); + } +} + +void Topotek::send_attitude() +{ + // Report actual GimbalSim joint angles. + // joint_angles.y = pitch (negative = down), .z = azimuth. + // Wire encoding: pitch_cd = -pitch_deg * 100 (driver negates on read). + // yaw_cd = yaw_deg * 100. + Vector3f ja; + gimbal.get_joint_angles(ja); + const int16_t yaw_cd = (int16_t)(degrees(ja.z) * 100.0f); + const int16_t pitch_cd = (int16_t)(-degrees(ja.y) * 100.0f); + const int16_t roll_cd = (int16_t)(degrees(ja.x) * 100.0f); + + uint8_t data[12]; + uint16_to_hex4((uint16_t)yaw_cd, &data[0]); + uint16_to_hex4((uint16_t)pitch_cd, &data[4]); + uint16_to_hex4((uint16_t)roll_cd, &data[8]); + + send_packet('G', "GIA", false, data, sizeof(data)); +} + +/* + read bytes from autopilot into _buf, then scan for complete packets. + Packet format: + [0] '#' + [1] 'T' or 't' + [2] 'P' or 'p' + [3] 'U' (addr1) + [4] addr2 ('G','D','M','E','P') + [5] data_len as a single ASCII hex char + [6] 'r' or 'w' + [7..9] 3-char command ID + [10..10+data_len-1] data bytes + [10+data_len..11+data_len] 2-byte CRC + Total packet length = 12 + data_len +*/ +void Topotek::move_preamble_in_buffer(uint8_t search_start_pos) +{ + uint8_t i; + for (i = search_start_pos; i < _buflen; i++) { + if (_buf[i] == '#') { + break; + } + } + if (i == 0) { + return; + } + memmove(_buf, &_buf[i], _buflen - i); + _buflen -= i; +} + +void Topotek::update_input() +{ + const ssize_t n = read_from_autopilot((char*)&_buf[_buflen], ARRAY_SIZE(_buf) - _buflen - 1); + if (n < 0) { + if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) { + AP_HAL::panic("Failed to read from autopilot"); + } + return; + } + _buflen += n; + + while (_buflen >= 3) { + // search for '#' at the start + if (_buf[0] != '#') { + move_preamble_in_buffer(1); + continue; + } + if (_buf[1] != 'T' && _buf[1] != 't') { + move_preamble_in_buffer(1); + continue; + } + if (_buf[2] != 'P' && _buf[2] != 'p') { + move_preamble_in_buffer(1); + continue; + } + + // need at least 6 bytes to read the data_len field + if (_buflen < 6) { + break; + } + + // parse data length from ASCII hex char at [5] + const uint8_t data_len = char_to_hex(_buf[5]); + if (data_len == 255) { + // invalid data length — discard '#' + move_preamble_in_buffer(1); + continue; + } + + const uint8_t pkt_len = 12 + data_len; + if (pkt_len > PACKETLEN_MAX) { + move_preamble_in_buffer(1); + continue; + } + + // wait for the full packet + if (_buflen < pkt_len) { + break; + } + + // verify and dispatch the packet + handle_packet(data_len); + move_preamble_in_buffer(pkt_len); + } +} + +void Topotek::handle_packet(uint8_t data_len) +{ + // verify CRC + const uint8_t pkt_len = 12 + data_len; + const uint8_t crc = crc_sum_of_bytes(_buf, pkt_len - 2); + const uint8_t expected_hi = hex2char((crc >> 4) & 0x0f); + const uint8_t expected_lo = hex2char(crc & 0x0f); + if (_buf[pkt_len - 2] != expected_hi || _buf[pkt_len - 1] != expected_lo) { + return; + } + + // ID is at bytes [7..9] + const char *id = (const char*)&_buf[7]; + + if (strncmp(id, "GIA", 3) == 0) { + // attitude request + send_attitude(); + + } else if (strncmp(id, "VSN", 3) == 0) { + const uint8_t data[] { '1', '.', '0', '.', '0' }; + send_packet('D', "VSN", false, data, sizeof(data)); + + } else if (strncmp(id, "PA2", 3) == 0) { + const uint8_t data[] { 'S', 'I', 'M', '_', 'T', 'P' }; + send_packet('G', "PA2", false, data, sizeof(data)); + + } else if (strncmp(id, "SDC", 3) == 0) { + // card present: any 4 bytes that are NOT all 'N' + const uint8_t data[] { '1', '0', '0', '0' }; + send_packet('D', "SDC", false, data, sizeof(data)); + + } else if (strncmp(id, "TRC", 3) == 0) { + // tracking stopped: driver reads _msg_buff[11] as TrackingStatus; + // [11] = data[1] = '0' = 0x30 = STOPPED_TRACKING + const uint8_t data[] { '0', '0' }; + send_packet('D', "TRC", false, data, sizeof(data)); + + } else if (strncmp(id, "GIP", 3) == 0 && data_len >= 4) { + // pitch angle command: data[0..3] = 4 uppercase hex chars for int16 centidegrees + // Wire value is (uint16_t)(-degrees(pitch_rad)*100); store as-is for echo in send_attitude() + _commanded_pitch_cd = (int16_t)( + ((uint16_t)char_to_hex(_buf[10]) << 12) | + ((uint16_t)char_to_hex(_buf[11]) << 8) | + ((uint16_t)char_to_hex(_buf[12]) << 4) | + (uint16_t)char_to_hex(_buf[13])); + + } else if (strncmp(id, "GIY", 3) == 0 && data_len >= 4) { + // body-frame yaw angle command + _commanded_yaw_cd = (int16_t)( + ((uint16_t)char_to_hex(_buf[10]) << 12) | + ((uint16_t)char_to_hex(_buf[11]) << 8) | + ((uint16_t)char_to_hex(_buf[12]) << 4) | + (uint16_t)char_to_hex(_buf[13])); + } + // all other commands (YPR, GIR, PTZ, LAT, LON, ALT, etc.) absorbed silently +} + +void Topotek::send_packet(char addr2, const char id[3], bool write, const uint8_t *data, uint8_t len) +{ + const uint8_t total = 12 + len; + if (total > PACKETLEN_MAX) { + return; + } + + uint8_t pkt[PACKETLEN_MAX]; + uint8_t ofs = 0; + + pkt[ofs++] = '#'; + pkt[ofs++] = 'T'; + pkt[ofs++] = 'P'; + pkt[ofs++] = 'U'; + pkt[ofs++] = (uint8_t)addr2; + pkt[ofs++] = hex2char(len & 0x0f); // data length as single ASCII hex char + pkt[ofs++] = write ? 'w' : 'r'; + pkt[ofs++] = (uint8_t)id[0]; + pkt[ofs++] = (uint8_t)id[1]; + pkt[ofs++] = (uint8_t)id[2]; + + for (uint8_t i = 0; i < len; i++) { + pkt[ofs++] = data[i]; + } + + // checksum: byte sum of all preceding bytes, encoded as 2 uppercase ASCII hex chars + const uint8_t crc = crc_sum_of_bytes(pkt, ofs); + pkt[ofs++] = hex2char((crc >> 4) & 0x0f); + pkt[ofs++] = hex2char(crc & 0x0f); + + write_to_autopilot((const char*)pkt, ofs); +} + +void Topotek::uint16_to_hex4(uint16_t val, uint8_t buf[4]) +{ + buf[0] = hex2char((val >> 12) & 0x0f); + buf[1] = hex2char((val >> 8) & 0x0f); + buf[2] = hex2char((val >> 4) & 0x0f); + buf[3] = hex2char((val ) & 0x0f); +} + +#endif // AP_SIM_TOPOTEK_ENABLED diff --git a/libraries/SITL/SIM_Topotek.h b/libraries/SITL/SIM_Topotek.h new file mode 100644 index 0000000000000..8a8df59d901ea --- /dev/null +++ b/libraries/SITL/SIM_Topotek.h @@ -0,0 +1,84 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + Simulator for Topotek gimbal + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:topotek --speedup=1 + +param set MNT1_TYPE 12 # topotek +param set SERIAL5_PROTOCOL 8 # gimbal +reboot + +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_TOPOTEK_ENABLED + +#include "SIM_Mount.h" +#include "SIM_Gimbal.h" + +namespace SITL { + +class Topotek : public Mount { +public: + + void update(const Aircraft &aircraft) override; + +private: + + // the physical gimbal: + Gimbal gimbal; + + // input accumulation buffer; also used as working buffer by handle_packet() + static constexpr uint8_t PACKETLEN_MAX = 36; + uint8_t _buf[PACKETLEN_MAX]; + uint8_t _buflen; + + uint32_t _last_attitude_ms; // time of last attitude packet sent + + // last commanded angles from GIP/GIY packets (wire centidegrees, same sign as sent by driver) + int16_t _commanded_pitch_cd; + int16_t _commanded_yaw_cd; + + // read and dispatch incoming packets from autopilot + void update_input(); + + // scan forward from search_start_pos for '#' and move it to _buf[0] + void move_preamble_in_buffer(uint8_t search_start_pos); + + // send gimbal attitude packet to the driver + void send_attitude(); + + // dispatch a complete packet beginning at _buf[0], data_len data bytes + void handle_packet(uint8_t data_len); + + // build and send a response packet + void send_packet(char addr2, const char id[3], bool write, const uint8_t *data, uint8_t len); + + // encode a uint16 as 4 uppercase ASCII hex chars + static void uint16_to_hex4(uint16_t val, uint8_t buf[4]); + + // convert a nibble (0-15) to an uppercase ASCII hex character + static uint8_t hex2char(uint8_t nibble) { + return nibble < 10 ? ('0' + nibble) : ('A' + nibble - 10); + } +}; + +} // namespace SITL + +#endif // AP_SIM_TOPOTEK_ENABLED diff --git a/libraries/SITL/SIM_Viewpro.cpp b/libraries/SITL/SIM_Viewpro.cpp new file mode 100644 index 0000000000000..2984bcccca262 --- /dev/null +++ b/libraries/SITL/SIM_Viewpro.cpp @@ -0,0 +1,378 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + Simulator for Viewpro gimbal +*/ + +#include "SIM_config.h" + +#if AP_SIM_VIEWPRO_ENABLED + +#include "SIM_Viewpro.h" +#include "SIM_Aircraft.h" +#include +#include + +using namespace SITL; + +// scalar from AP_Mount_Viewpro.cpp +#define VIEWPRO_DEG_TO_OUTPUT (65536.0f / 360.0f) + +void Viewpro::update(const Aircraft &aircraft) +{ + // Drive GimbalSim toward the angles commanded via A1 packets. + // The A1 wire encoding is: pitch_raw = -pitch_deg * DEG_TO_OUTPUT, + // yaw_raw = yaw_deg * DEG_TO_OUTPUT. Invert to recover target radians. + { + static constexpr float OUTPUT_TO_RAD = (360.0f / 65536.0f) * (float)DEG_TO_RAD; + const float target_pitch_rad = -_target_pitch_raw * OUTPUT_TO_RAD; + const float target_yaw_rad = _target_yaw_raw * OUTPUT_TO_RAD; + + Vector3f ja; + gimbal.get_joint_angles(ja); + + // P-gain: drives gimbal to commanded angle with ~0.1 s time constant. + // joint_angles.y = pitch (negative = down), .z = azimuth. + Matrix3f gimbal_dcm; + gimbal.get_dcm(gimbal_dcm); + const Vector3f vehicle_rate_gimbal = gimbal_dcm.transposed() * aircraft.get_dcm() * aircraft.get_gyro(); + + static constexpr float GAIN = 10.0f; + gimbal.set_demanded_rates(Vector3f( + vehicle_rate_gimbal.x, + vehicle_rate_gimbal.y + (target_pitch_rad - ja.y) * GAIN, + vehicle_rate_gimbal.z + (target_yaw_rad - ja.z) * GAIN)); + } + + gimbal.update(aircraft); + update_input(); + + // send attitude at 10 Hz + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - _last_attitude_ms >= 100) { + _last_attitude_ms = now_ms; + send_t1_f1_b1_d1(); + } +} + +/* + read bytes from the autopilot and process them byte-by-byte through the + Viewpro protocol state machine. + + The driver discards header bytes without storing them. After all three + header bytes are consumed the internal packet buffer starts fresh: + _buf[0] = length+frame_counter byte (bits 0-5 = body_length) + _buf[1] = frame_id byte + _buf[2+] = data bytes + _buf[N] = CRC byte (last) + + body_length = 1(length byte) + 1(frame_id) + data_len + 1(crc) + => data_len = body_length - 3 +*/ +void Viewpro::update_input() +{ + // read new bytes into a scratch buffer; process one at a time + uint8_t scratch[128]; + const ssize_t n = read_from_autopilot((char*)scratch, sizeof(scratch)); + if (n < 0) { + if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) { + AP_HAL::panic("Failed to read from autopilot"); + } + return; + } + + for (ssize_t i = 0; i < n; i++) { + const uint8_t b = scratch[i]; + + switch (_parse_state) { + case ParseState::HEADER1: + if (b == HEADER1) { + _buflen = 0; + _parse_state = ParseState::HEADER2; + } + break; + + case ParseState::HEADER2: + if (b == HEADER2) { + _buflen = 0; + _parse_state = ParseState::HEADER3; + } else { + _buflen = 0; + _parse_state = ParseState::HEADER1; + } + break; + + case ParseState::HEADER3: + if (b == HEADER3) { + _buflen = 0; + _parse_state = ParseState::LENGTH; + } else { + _buflen = 0; + _parse_state = ParseState::HEADER1; + } + break; + + case ParseState::LENGTH: { + const uint8_t body_length = b & 0x3F; + if (body_length < 3) { + _buflen = 0; + _parse_state = ParseState::HEADER1; + break; + } + _data_len = body_length - 3; + _buf[_buflen++] = b; // _buf[0] = length+fc byte + _parse_state = ParseState::FRAMEID; + break; + } + + case ParseState::FRAMEID: + _data_bytes_received = 0; + _buf[_buflen++] = b; // _buf[1] = frame_id + _parse_state = (_data_len > 0) ? ParseState::DATA : ParseState::CHECKSUM; + break; + + case ParseState::DATA: + if (_buflen < PACKETLEN_MAX) { + _buf[_buflen++] = b; + } + _data_bytes_received++; + if (_data_bytes_received >= _data_len) { + _parse_state = ParseState::CHECKSUM; + } + break; + + case ParseState::CHECKSUM: { + if (_buflen < PACKETLEN_MAX) { + _buf[_buflen++] = b; + } + // verify CRC: XOR of _buf[0.._buflen-2] (all bytes except the crc byte itself) + const uint8_t expected_crc = crc_xor_of_bytes(_buf, _buflen - 1); + if (expected_crc == b) { + dispatch_packet(); + } + _buflen = 0; + _parse_state = ParseState::HEADER1; + break; + } + } + } +} + +void Viewpro::dispatch_packet() +{ + // _buf[1] = frame_id; _buf[2+] = data (data_start = 2) + const FrameId frame_id = (FrameId)_buf[1]; + + switch (frame_id) { + case FrameId::HANDSHAKE: + send_t1_f1_b1_d1(); + break; + + case FrameId::U: + // _buf[2] = data[0] = CommConfigCmd + if (_data_len >= 1) { + send_v_response((CommConfigCmd)_buf[2]); + } + break; + + case FrameId::A1: + // _buf layout: [0]=length_fc, [1]=frame_id, [2]=servo_status, + // [3..4]=yaw big-endian, [5..6]=pitch big-endian + // Store raw int16 values so the driver round-trips them without + // any additional float rounding. + if (_data_len >= 5) { + _target_yaw_raw = (int16_t)(((uint16_t)_buf[3] << 8) | _buf[4]); + _target_pitch_raw = (int16_t)(((uint16_t)_buf[5] << 8) | _buf[6]); + } + break; + case FrameId::C1: + case FrameId::C2: + case FrameId::E1: + case FrameId::E2: + case FrameId::M_AHRS: + case FrameId::HEARTBEAT: + // absorb; gimbal physics is updated via update() + break; + + default: + break; + } +} + +/* + send T1_F1_B1_D1 (0x40) packet with current gimbal attitude. + + Driver reads angles using _msg_buff_data_start=2: + data[22] = tracking status (bits 3-4; 0 = stopped) + data[23] = servo_status(upper 4 bits) | roll_12bit_MSByte(lower 4 bits) + data[24] = roll LSByte → roll_deg = 12-bit * 180/4095 - 90 + data[25..26] = yaw int16 big-endian → yaw_deg = val * 360/65536 + data[27..28] = pitch int16 big-endian → pitch_deg = val * 360/65536 (driver negates) + data[29] = image sensor (bits 0-2; stored as val+1; 0→EO1) + data[32] = recording status (bits 0-2; 0=stopped) + data[33..34] = rangefinder × 0.1 m + data[39..40] = zoom × 0.1 +*/ +void Viewpro::send_t1_f1_b1_d1() +{ + static constexpr uint8_t DATA_LEN = 41; + // databuff: frame_id (1 byte) + data (DATA_LEN bytes) + uint8_t databuff[1 + DATA_LEN] {}; + databuff[0] = (uint8_t)FrameId::T1_F1_B1_D1; + + uint8_t *data = &databuff[1]; + + // Report actual GimbalSim joint angles. + // joint_angles.y = pitch (negative = down), .z = azimuth. + // Wire encoding: pitch_out = -pitch_deg * DEG_TO_OUTPUT (driver negates on read). + // yaw_out = yaw_deg * DEG_TO_OUTPUT. + // roll: 12-bit value where 2048 (midpoint of 0..4095) ≈ 0 deg (actual = +0.022 deg, + // within the neutral_tol_deg=0.05 used by MountViewPro). + // Pitch clamp to ±16380 (±89.978 deg): at exactly ±90 deg (raw ±16384) the float32 + // from_euler quaternion with the small roll (~0.022 deg) has DCM[2][0] > 1.0 in + // float64 (Python), triggering pymavlink gimbal-lock → 180 deg instead of ±90 deg. + Vector3f ja; + gimbal.get_joint_angles(ja); + const int16_t yaw_out = (int16_t)(degrees(ja.z) * VIEWPRO_DEG_TO_OUTPUT); + const int16_t pitch_out = (int16_t)constrain_int16( + (int16_t)(-degrees(ja.y) * VIEWPRO_DEG_TO_OUTPUT), + -16380, 16380); + const uint16_t roll_out = 2048U; + + // bytes 0..21: T1/F1 fields — leave as zero + data[22] = 0x00; // tracking stopped + + data[23] = (uint8_t)((roll_out >> 8) & 0x0F); // lower nibble = roll MSB; upper nibble = 0 (servo status) + data[24] = (uint8_t)(roll_out & 0xFF); + + data[25] = (uint8_t)((uint16_t)yaw_out >> 8); + data[26] = (uint8_t)((uint16_t)yaw_out & 0xFF); + + data[27] = (uint8_t)((uint16_t)pitch_out >> 8); + data[28] = (uint8_t)((uint16_t)pitch_out & 0xFF); + + data[29] = 0x00; // image sensor bits 0-2 = 0 → driver stores as EO1 + data[32] = 0x00; // recording stopped + data[33] = 0x00; // rangefinder MSB + data[34] = 0x00; // rangefinder LSB + data[39] = 0x00; // zoom MSB + data[40] = 0x0A; // zoom LSB: 10 × 0.1 = 1.0× zoom + + send_packet(databuff, sizeof(databuff)); +} + +/* + send V (0x02) response to a U communication config command. + + Firmware version response (QUERY_FIRMWARE_VER = 0xD0): + data[0] = 0xD0 (echo of CommConfigCmd) + data[1] = 'S' prefix + data[2..9] = "yyyymmdd" date string + driver reads: major=atoi(data[4..5]), minor=atoi(data[6..7]), patch=atoi(data[8..9]) + Using "20220301": major=22, minor=03, patch=01 + + Model name response (QUERY_MODEL = 0xE4): + data[0] = 0xE4 + data[1+] = model name bytes + driver: memcpy(_model_name, &data[1], data_bytes_received-1) +*/ +void Viewpro::send_v_response(CommConfigCmd cmd) +{ + uint8_t databuff[32] {}; + uint8_t total_len; + + databuff[0] = (uint8_t)FrameId::V; + + switch (cmd) { + case CommConfigCmd::QUERY_FIRMWARE_VER: + databuff[1] = (uint8_t)cmd; + databuff[2] = 'S'; // prefix character + databuff[3] = 'y'; + databuff[4] = 's'; + databuff[5] = '2'; // major hi: "20" + databuff[6] = '0'; + databuff[7] = '2'; // minor hi: "22" + databuff[8] = '2'; + databuff[9] = '0'; // patch hi: "01" + databuff[10] = '1'; + total_len = 11; + break; + + case CommConfigCmd::QUERY_MODEL: + // datasheet specifies a 10-byte model name field in the response + databuff[1] = (uint8_t)cmd; + databuff[2] = 'S'; + databuff[3] = 'I'; + databuff[4] = 'M'; + databuff[5] = '_'; + databuff[6] = 'V'; + databuff[7] = 'P'; + databuff[8] = 0; + databuff[9] = 0; + databuff[10] = 0; + databuff[11] = 0; + total_len = 12; + break; + + default: + return; + } + + send_packet(databuff, total_len); +} + +/* + build and write a complete Viewpro packet to the autopilot. + databuff[0] = frame_id, databuff[1+] = data payload. + + Wire format: + 0x55 0xAA 0xDC [length_fc] [databuff...] [crc] + where: + length_fc bits 6-7 = frame counter (0-3, increments per packet) + length_fc bits 0-5 = databuff_len + 2 (= body_length) + crc = XOR of [length_fc .. last_databuff_byte] +*/ +void Viewpro::send_packet(const uint8_t *databuff, uint8_t databuff_len) +{ + // total wire bytes: 3 (header) + 1 (length_fc) + databuff_len + 1 (crc) + const uint8_t wire_len = 5 + databuff_len; + if (wire_len > (uint8_t)(PACKETLEN_MAX + 4)) { + return; + } + + uint8_t pkt[PACKETLEN_MAX + 4]; + uint8_t ofs = 0; + + pkt[ofs++] = HEADER1; + pkt[ofs++] = HEADER2; + pkt[ofs++] = HEADER3; + + _frame_counter = (_frame_counter + 1) & 0x03; + const uint8_t body_length = (uint8_t)(databuff_len + 2); + pkt[ofs++] = (uint8_t)((_frame_counter << 6) | (body_length & 0x3F)); + + for (uint8_t i = 0; i < databuff_len; i++) { + pkt[ofs++] = databuff[i]; + } + + // CRC: XOR of bytes [3]..[ofs-1] (length_fc through last data byte) + pkt[ofs] = crc_xor_of_bytes(&pkt[3], ofs - 3); + ofs++; + + write_to_autopilot((const char*)pkt, ofs); +} + + +#endif // AP_SIM_VIEWPRO_ENABLED diff --git a/libraries/SITL/SIM_Viewpro.h b/libraries/SITL/SIM_Viewpro.h new file mode 100644 index 0000000000000..95fb3c4d1b10d --- /dev/null +++ b/libraries/SITL/SIM_Viewpro.h @@ -0,0 +1,124 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + Simulator for Viewpro gimbal + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:viewpro --speedup=1 + +param set MNT1_TYPE 11 # viewpro +param set SERIAL5_PROTOCOL 8 # gimbal +reboot + +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_VIEWPRO_ENABLED + +#include "SIM_Mount.h" +#include "SIM_Gimbal.h" + +namespace SITL { + +class Viewpro : public Mount { +public: + + void update(const Aircraft &aircraft) override; + +private: + + // the physical gimbal: + Gimbal gimbal; + + // Viewpro header bytes + static constexpr uint8_t HEADER1 = 0x55; + static constexpr uint8_t HEADER2 = 0xAA; + static constexpr uint8_t HEADER3 = 0xDC; + static constexpr uint8_t PACKETLEN_MAX = 63; + + // frame IDs (matching AP_Mount_Viewpro.h) + enum class FrameId : uint8_t { + HANDSHAKE = 0x00, + U = 0x01, + V = 0x02, + HEARTBEAT = 0x10, + A1 = 0x1A, + C1 = 0x1C, + E1 = 0x1E, + C2 = 0x2C, + E2 = 0x2E, + T1_F1_B1_D1 = 0x40, + M_AHRS = 0xB1, + }; + + // U packet communication configuration commands + enum class CommConfigCmd : uint8_t { + QUERY_FIRMWARE_VER = 0xD0, + QUERY_MODEL = 0xE4, + }; + + // packet buffer. + // after header bytes are discarded by the state machine: + // _buf[0] = length+frame_counter byte + // _buf[1] = frame_id + // _buf[2+] = data bytes + // _buf[last] = CRC + uint8_t _buf[PACKETLEN_MAX]; + uint8_t _buflen; + + // parser state + enum class ParseState : uint8_t { + HEADER1, + HEADER2, + HEADER3, + LENGTH, + FRAMEID, + DATA, + CHECKSUM, + } _parse_state; + + uint8_t _data_len; // data bytes expected in current packet + uint8_t _data_bytes_received; // data bytes received so far + + uint8_t _frame_counter; // 2-bit counter for outgoing packets + uint32_t _last_attitude_ms; // time of last T1_F1_B1_D1 packet sent + + // last A1 angle target received from driver, stored as raw int16 wire values + // (same encoding used in T1_F1_B1_D1 replies so the driver round-trips exactly) + int16_t _target_pitch_raw; + int16_t _target_yaw_raw; + + // read and parse incoming bytes from autopilot + void update_input(); + + // dispatch a complete, CRC-verified packet in _buf + void dispatch_packet(); + + // send T1_F1_B1_D1 attitude status packet + void send_t1_f1_b1_d1(); + + // send V response to a U communication config command + void send_v_response(CommConfigCmd cmd); + + // build and write a complete packet; databuff[0] = frame_id + void send_packet(const uint8_t *databuff, uint8_t databuff_len); + +}; + +} // namespace SITL + +#endif // AP_SIM_VIEWPRO_ENABLED diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index beb80e5d31088..78693bfe58a67 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -183,6 +183,9 @@ #define AP_SIM_GPS_UBLOX_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED #endif +#ifndef AP_SIM_EAHRS_SENSATION_ENABLED +#define AP_SIM_EAHRS_SENSATION_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif // simulated compass support; currently only in SITL, not SimOnHW: #ifndef AP_SIM_COMPASS_BACKEND_DEFAULT_ENABLED @@ -197,8 +200,30 @@ #define AP_SIM_SOLOGIMBAL_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL && HAL_MAVLINK_BINDINGS_ENABLED) #endif -#ifndef AP_SIM_GIMBAL_ENABLED -#define AP_SIM_GIMBAL_ENABLED (AP_SIM_SOLOGIMBAL_ENABLED) +#ifndef AP_SIM_SIYI_ZT30_ENABLED +#define AP_SIM_SIYI_ZT30_ENABLED 1 +#endif + +#ifndef AP_SIM_TOPOTEK_ENABLED +#define AP_SIM_TOPOTEK_ENABLED 1 +#endif + +#ifndef AP_SIM_VIEWPRO_ENABLED +#define AP_SIM_VIEWPRO_ENABLED 1 +#endif + +#ifndef AP_SIM_AVT_CM62_ENABLED +#define AP_SIM_AVT_CM62_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL && HAL_MAVLINK_BINDINGS_ENABLED) +#endif + +// base class for MAVLink (Gimbal Protocol v2) mount simulations: +#ifndef AP_SIM_MAVLINKGIMBALV2_ENABLED +#define AP_SIM_MAVLINKGIMBALV2_ENABLED AP_SIM_AVT_CM62_ENABLED +#endif + +// base class for all simulated gimbal backends: +#ifndef AP_SIM_MOUNT_ENABLED +#define AP_SIM_MOUNT_ENABLED (AP_SIM_SIYI_ENABLED || AP_SIM_TOPOTEK_ENABLED || AP_SIM_VIEWPRO_ENABLED || AP_SIM_MAVLINKGIMBALV2_ENABLED) #endif #ifndef AP_SIM_AIRSPEED_DLVR_ENABLED @@ -352,3 +377,12 @@ #ifndef AP_SIM_XPLANE_ENABLED #define AP_SIM_XPLANE_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif // AP_SIM_XPLANE_ENABLED + +// base class for Siyi simulations: +#ifndef AP_SIM_SIYI_ENABLED +#define AP_SIM_SIYI_ENABLED AP_SIM_SIYI_ZT30_ENABLED +#endif // AP_SIM_SIYI_ENABLED + +#ifndef AP_SIM_GIMBAL_ENABLED +#define AP_SIM_GIMBAL_ENABLED (AP_SIM_SOLOGIMBAL_ENABLED || AP_SIM_SIYI_ZT30_ENABLED || AP_SIM_TOPOTEK_ENABLED || AP_SIM_VIEWPRO_ENABLED || AP_SIM_MOUNT_ENABLED) +#endif // AP_SIM_GIMBAL_ENABLED diff --git a/modules/mavlink b/modules/mavlink index 375f640979b0c..288b907c384a8 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 375f640979b0c8e0e5764a38467bf6da09f41142 +Subproject commit 288b907c384a892c8519bfe271682424b1e1a3a0