diff --git a/.gitignore b/.gitignore index b3d5003c68bde..debfef06f055f 100644 --- a/.gitignore +++ b/.gitignore @@ -171,7 +171,3 @@ ENV/ env.bak/ venv.bak/ autotest_result_*_junit.xml - -# Ignore ESP-IDF SDK defines -/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig -/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index d8e3c11f7549a..42cdcb3d45673 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -83,9 +83,12 @@ void ModeTakeoff::update() if (!takeoff_mode_setup) { plane.auto_state.takeoff_altitude_rel_cm = alt * 100; const uint16_t altitude = plane.relative_ground_altitude(false,true); - const float direction = degrees(ahrs.get_yaw()); + const Vector2f &groundspeed2d = ahrs.groundspeed_vector(); + const float direction = wrap_360(degrees(groundspeed2d.angle())); + const float groundspeed = groundspeed2d.length(); + // see if we will skip takeoff as already flying - if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && ahrs.groundspeed() > 3) { + if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && groundspeed > 3) { if (altitude >= alt) { gcs().send_text(MAV_SEVERITY_INFO, "Above TKOFF alt - loitering"); plane.next_WP_loc = plane.current_loc; @@ -115,7 +118,15 @@ void ModeTakeoff::update() plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF); - if (!plane.throttle_suppressed) { + /* + don't lock in the takeoff loiter location until we reach + a ground speed of AIRSPEED_MIN*0.3 to cope with aircraft + with no compass or poor compass. If flying in a very + strong headwind then the is_flying() check above will + eventually trigger + */ + if (!plane.throttle_suppressed && + groundspeed > plane.aparm.airspeed_min*0.3) { gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg", alt, dist, direction); plane.takeoff_state.start_time_ms = millis(); diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index 9bfa9087ffc40..e8e6b7cb249f7 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -331,6 +331,7 @@ AP_HW_MountainEagleH743 1444 AP_HW_StellarF4 1500 AP_HW_GEPRCF745BTHD 1501 +AP_HW_GEPRC_TAKER_H743 1502 AP_HW_HGLRCF405V4 1524 diff --git a/Tools/AP_Periph/imu.cpp b/Tools/AP_Periph/imu.cpp index c0c5aaf40128d..06e961e44b135 100644 --- a/Tools/AP_Periph/imu.cpp +++ b/Tools/AP_Periph/imu.cpp @@ -11,8 +11,15 @@ extern const AP_HAL::HAL &hal; void AP_Periph_FW::can_imu_update(void) { while (true) { - // sleep for a bit to avoid flooding the CPU - hal.scheduler->delay_microseconds(100); + // we need to delay by a ms value as hal->schedule->delay_microseconds_boost + // used in wait_for_sample() takes uint16_t + const uint32_t delay_ms = 1000U / g.imu_sample_rate; + hal.scheduler->delay(delay_ms); + + if (delay_ms == 0) { + // sleep for a bit to avoid flooding the CPU + hal.scheduler->delay_microseconds(100); + } imu.update(); @@ -38,6 +45,11 @@ void AP_Periph_FW::can_imu_update(void) pkt.accelerometer_latest[1] = tmp.y; pkt.accelerometer_latest[2] = tmp.z; + tmp = imu.get_gyro(); + pkt.rate_gyro_latest[0] = tmp.x; + pkt.rate_gyro_latest[1] = tmp.y; + pkt.rate_gyro_latest[2] = tmp.z; + uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_RAWIMU_MAX_SIZE]; uint16_t total_size = uavcan_equipment_ahrs_RawIMU_encode(&pkt, buffer, !canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_AHRS_RAWIMU_SIGNATURE, diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 0e692be45a6fe..017c779b283e0 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -949,6 +949,7 @@ def configure_env(self, cfg, env): HAL_PERIPH_ENABLE_AIRSPEED = 1, HAL_PERIPH_ENABLE_MAG = 1, HAL_PERIPH_ENABLE_BARO = 1, + HAL_PERIPH_ENABLE_IMU = 1, HAL_PERIPH_ENABLE_RANGEFINDER = 1, HAL_PERIPH_ENABLE_BATTERY = 1, HAL_PERIPH_ENABLE_EFI = 1, @@ -963,6 +964,7 @@ def configure_env(self, cfg, env): HAL_WITH_ESC_TELEM = 1, AP_EXTENDED_ESC_TELEM_ENABLED = 1, AP_TERRAIN_AVAILABLE = 1, + HAL_GYROFFT_ENABLED = 0, ) class sitl_periph_gps(sitl_periph): diff --git a/Tools/ardupilotwaf/esp32.py b/Tools/ardupilotwaf/esp32.py index 5e30e39ce3a8e..11df54d6ceae9 100644 --- a/Tools/ardupilotwaf/esp32.py +++ b/Tools/ardupilotwaf/esp32.py @@ -57,6 +57,24 @@ def bldpath(path): #env.append_value('GIT_SUBMODULES', 'esp_idf') +# delete the output sdkconfig file when the input defaults changes. we take the +# stamp as the output so we can compute the path to the sdkconfig, yet it +# doesn't have to exist when we're done. +class clean_sdkconfig(Task.Task): + def keyword(self): + return "delete sdkconfig generated from" + + def run(self): + prefix = ".clean-stamp-" + for out in self.outputs: + if not out.name.startswith(prefix): + raise ValueError("not a stamp file: "+out) + dest = out.parent.abspath()+"/"+out.name[len(prefix):] + if os.path.exists(dest): + os.unlink(dest) + + # waf needs the output to exist after the task, so touch it + open(out.abspath(), "w").close() def pre_build(self): """Configure esp-idf as lib target""" @@ -74,8 +92,21 @@ def pre_build(self): ) esp_idf_showinc = esp_idf.build('showinc', target='esp-idf_build/includes.list') + + # task to delete the sdkconfig (thereby causing it to be regenerated) when + # the .defaults changes. it uses a stamp to find the sdkconfig. changing + # the sdkconfig WILL NOT cause it to be deleted as it's not an input. this + # is by design so the user can tweak it for testing purposes. + clean_sdkconfig_task = esp_idf_showinc.create_task("clean_sdkconfig", + src=self.srcnode.find_or_declare(self.env.AP_HAL_ESP32+"/sdkconfig.defaults"), + tgt=self.bldnode.find_or_declare("esp-idf_build/.clean-stamp-sdkconfig")) + esp_idf_showinc.post() + # ensure the sdkconfig will be deleted before the cmake configure occurs + # that regenerates it + esp_idf_showinc.cmake_config_task.set_run_after(clean_sdkconfig_task) + from waflib import Task class load_generated_includes(Task.Task): """After includes.list generated include it in env""" diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 8b46a62473ec3..2d968b900d9fa 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -8304,6 +8304,57 @@ def WatchAlts(self): self.do_RTL() + def TestTetherStuck(self): + """Test tethered vehicle stuck because of tether""" + # Enable tether simulation + self.set_parameters({ + "SIM_TETH_ENABLE": 1, + }) + self.delay_sim_time(2) + self.reboot_sitl() + + # Set tether line length + self.set_parameters({ + "SIM_TETH_LINELEN": 10, + }) + self.delay_sim_time(2) + + # Prepare and take off + self.wait_ready_to_arm() + self.arm_vehicle() + self.takeoff(10, mode='LOITER') + + # Simulate vehicle getting stuck by increasing RC throttle + self.set_rc(3, 1900) + self.delay_sim_time(5, reason='let tether get stuck') + + # Monitor behavior for 10 seconds + tstart = self.get_sim_time() + initial_alt = self.get_altitude() + stuck = True # Assume it's stuck unless proven otherwise + + while self.get_sim_time() - tstart < 10: + # Fetch current altitude + current_alt = self.get_altitude() + self.progress(f"current_alt={current_alt}") + + # Fetch and log battery status + battery_status = self.mav.recv_match(type='BATTERY_STATUS', blocking=True, timeout=1) + if battery_status: + self.progress(f"Battery: {battery_status}") + + # Check if the vehicle is stuck. + # We assume the vehicle is stuck if the current is high and the altitude is not changing + if battery_status and (battery_status.current_battery < 6500 or abs(current_alt - initial_alt) > 2): + stuck = False # Vehicle moved or current is abnormal + break + + if not stuck: + raise NotAchievedException("Vehicle did not get stuck as expected") + + # Land and disarm the vehicle to ensure we can go down + self.land_and_disarm() + def fly_rangefinder_drivers_fly(self, rangefinders): '''ensure rangefinder gives height-above-ground''' self.change_mode('GUIDED') @@ -12330,6 +12381,7 @@ def tests2b(self): # this block currently around 9.5mins here self.MAV_CMD_MISSION_START_p1_p2, self.ScriptingAHRSSource, self.CommonOrigin, + self.TestTetherStuck, ]) return ret diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 7fabd6cd07816..8e49b0f2038cc 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4805,6 +4805,41 @@ def TakeoffTakeoff4(self): self.fly_home_land_and_disarm() + def TakeoffTakeoff5(self): + '''Test the behaviour of a takeoff with no compass''' + self.set_parameters({ + "COMPASS_USE": 0, + "COMPASS_USE2": 0, + "COMPASS_USE3": 0, + }) + import copy + start_loc = copy.copy(SITL_START_LOCATION) + start_loc.heading = 175 + self.customise_SITL_commandline(["--home=%.9f,%.9f,%.2f,%.1f" % ( + start_loc.lat, start_loc.lng, start_loc.alt, start_loc.heading)]) + self.reboot_sitl() + self.change_mode("TAKEOFF") + + # waiting for the EKF to be happy won't work + self.delay_sim_time(20) + self.arm_vehicle() + + target_alt = self.get_parameter("TKOFF_ALT") + self.wait_altitude(target_alt-5, target_alt, relative=True) + + # Wait a bit for the Takeoff altitude to settle. + self.delay_sim_time(5) + + bearing_margin = 35 + loc = self.mav.location() + bearing_from_home = self.get_bearing(start_loc, loc) + if bearing_from_home < 0: + bearing_from_home += 360 + if abs(bearing_from_home - start_loc.heading) > bearing_margin: + raise NotAchievedException(f"Did not takeoff in the right direction {bearing_from_home}") + + self.fly_home_land_and_disarm() + def TakeoffGround(self): '''Test a rolling TAKEOFF.''' @@ -5326,6 +5361,40 @@ def AerobaticsScripting(self): else: raise NotAchievedException("Missing trick %s" % t) + def UniversalAutoLandScript(self): + '''Test UniversalAutoLandScript''' + applet_script = "UniversalAutoLand.lua" + self.customise_SITL_commandline(["--home", "-35.362938,149.165085,585,173"]) + + self.install_applet_script_context(applet_script) + self.context_collect('STATUSTEXT') + self.set_parameters({ + "SCR_ENABLE" : 1, + "SCR_VM_I_COUNT" : 1000000, + "RTL_AUTOLAND" : 2 + }) + self.reboot_sitl() + self.wait_text("Loaded UniversalAutoLand.lua", check_context=True) + self.set_parameters({ + "AUTOLAND_ENABLE" : 1, + "AUTOLAND_WP_ALT" : 55, + "AUTOLAND_WP_DIST" : 400 + }) + self.scripting_restart() + self.wait_text("Scripting: restarted", check_context=True) + + self.wait_ready_to_arm() + self.arm_vehicle() + self.change_mode("AUTO") + self.wait_text("Captured initial takeoff direction", check_context=True) + + self.wait_disarmed(120) + self.progress("Check the landed heading matches takeoff") + self.wait_heading(173, accuracy=5, timeout=1) + loc = mavutil.location(-35.362938, 149.165085, 585, 173) + if self.get_distance(loc, self.mav.location()) > 35: + raise NotAchievedException("Did not land close to home") + def SDCardWPTest(self): '''test BRD_SD_MISSION support''' spiral_script = "mission_spiral.lua" @@ -6457,6 +6526,7 @@ def tests1a(self): self.Soaring, self.Terrain, self.TerrainMission, + self.UniversalAutoLandScript, ]) return ret @@ -6490,6 +6560,7 @@ def tests1b(self): self.TakeoffTakeoff2, self.TakeoffTakeoff3, self.TakeoffTakeoff4, + self.TakeoffTakeoff5, self.TakeoffGround, self.TakeoffIdleThrottle, self.TakeoffBadLevelOff, diff --git a/Tools/bootloaders/GEPRC_TAKER_H743_bl.bin b/Tools/bootloaders/GEPRC_TAKER_H743_bl.bin new file mode 100644 index 0000000000000..6a5152f03c945 Binary files /dev/null and b/Tools/bootloaders/GEPRC_TAKER_H743_bl.bin differ diff --git a/Tools/bootloaders/GEPRC_TAKER_H743_bl.hex b/Tools/bootloaders/GEPRC_TAKER_H743_bl.hex new file mode 100644 index 0000000000000..2d751afcb6266 --- /dev/null +++ b/Tools/bootloaders/GEPRC_TAKER_H743_bl.hex @@ -0,0 +1,1168 @@ +:020000040800F2 +:1000000000060020E1020008E3020008E302000805 +:10001000E3020008E3020008E3020008E30200082C +:10002000E3020008E3020008E30200083D2A00089A +:10003000E3020008E3020008E3020008E30200080C +:10004000E3020008E3020008E3020008E3020008FC +:10005000E3020008E30200089D410008C9410008CE +:10006000F5410008214200084D42000885100008B3 +:10007000AD100008D9100008051100083111000862 +:100080005911000885110008E3020008E302000886 +:10009000E3020008E3020008E302000879420008D6 +:1000A000E3020008E3020008E3020008E30200089C +:1000B000E3020008E3020008E3020008E30200088C +:1000C000E3020008E3020008E3020008E30200087C +:1000D000E3020008E3020008E3020008E30200086C +:1000E000DD420008E3020008E3020008E302000822 +:1000F000E3020008E3020008E3020008B11100086F +:10010000E3020008E302000851430008E30200088C +:10011000E3020008E3020008E3020008E30200082B +:10012000DD11000805120008311200085D120008F8 +:1001300089120008E3020008E3020008E302000855 +:10014000E3020008E3020008E3020008E3020008FB +:10015000B1120008DD12000809130008E3020008CC +:10016000E3020008E3020008E3020008E3020008DB +:10017000E302000829360008E3020008E302000851 +:10018000E3020008E3020008E3020008E3020008BB +:10019000E3020008E3020008E3020008E3020008AB +:1001A000E3020008E3020008E3020008E30200089B +:1001B000E3020008E3020008E3020008E30200088B +:1001C000E3020008E3020008E3020008E30200087B +:1001D000E302000815360008E3020008E302000805 +:1001E000E3020008E3020008E3020008E30200085B +:1001F000E3020008E3020008E3020008E30200084B +:10020000E3020008E3020008E3020008E30200083A +:10021000E3020008E3020008E3020008E30200082A +:10022000E3020008E3020008E3020008E30200081A +:10023000E3020008E3020008E3020008E30200080A +:10024000E3020008E3020008E3020008E3020008FA +:10025000E3020008E3020008E3020008E3020008EA +:10026000E3020008E3020008E3020008E3020008DA +:10027000E3020008E3020008E3020008E3020008CA +:10028000E3020008E3020008E3020008E3020008BA +:10029000E3020008E3020008E3020008E3020008AA +:1002A000E3020008E3020008E3020008E30200089A +:1002B000E3020008E3020008E3020008E30200088A +:1002C000E3020008E3020008E3020008E30200087A +:1002D000E3020008E3020008E3020008E30200086A +:1002E00002E000F000F8FEE772B6374880F30888B5 +:1002F000364880F3098836483649086040F20000E5 +:10030000CCF200004EF63471CEF200010860BFF36B +:100310004F8FBFF36F8F40F20000C0F2F0004EF637 +:100320008851CEF200010860BFF34F8FBFF36F8F8B +:100330004FF00000E1EE100A4EF63C71CEF20001E3 +:100340000860062080F31488BFF36F8F02F0D8FB9B +:1003500003F0DEFC4FF055301F491B4A91423CBF71 +:1003600041F8040BFAE71D49184A91423CBF41F895 +:10037000040BFAE71A491B4A1B4B9A423EBF51F83D +:10038000040B42F8040BF8E700201849184A914280 +:100390003CBF41F8040BFAE702F0F0FB03F03CFD30 +:1003A000144C154DAC4203DA54F8041B8847F9E7A6 +:1003B00000F042F8114C124DAC4203DA54F8041B21 +:1003C0008847F9E702F0D8BB000600200022002091 +:1003D0000000000808ED00E00000002000060020FA +:1003E00078480008002200205C22002060220020C3 +:1003F00084440020E0020008E0020008E002000857 +:10040000E00200082DE9F04F2DED108AC1F80CD064 +:10041000D0F80CD0BDEC108ABDE8F08F002383F338 +:1004200011882846A047002001F0D2FDFEE701F028 +:100430005DFD00DFFEE7000038B500F02FFC00F0A6 +:10044000ABFD02F0BBFA054602F0EEFA0446C0B975 +:100450000E4B9D4217D001339D4241F2883512BFA9 +:10046000044600250124002002F0B2FA0CB100F08D +:1004700075F800F065FD284600F01EF900F06EF8F2 +:10048000F9E70025EFE70546EDE700BF010007B0FB +:1004900008B500F0D5FBA0F120035842584108BD33 +:1004A00007B541F21203022101A8ADF8043000F0B3 +:1004B000E5FB03B05DF804FB38B5302383F3118806 +:1004C000174803680BB101F04FFE0023154A4FF4A3 +:1004D0007A71134801F03EFE002383F31188124C19 +:1004E000236813B12368013B2360636813B1636819 +:1004F000013B63600D4D2B7833B963687BB90220F3 +:1005000000F088FC322363602B78032B07D16368EB +:100510002BB9022000F07EFC4FF47A73636038BD83 +:1005200060220020B90400088023002078220020E7 +:10053000084B187003280CD8DFE800F00805020803 +:10054000022000F05DBC022000F050BC024B0022F3 +:100550005A6070477822002080230020F8B5504B65 +:10056000504A1C461968013100F0998004339342C7 +:10057000F8D162684C4B9A4240F291804B4B9B6899 +:1005800003F1006303F5C0239A4280F088800020C5 +:1005900000F09EFB0220FFF7CBFF454B0021D3F874 +:1005A000E820C3F8E810D3F81021C3F81011D3F8ED +:1005B0001021D3F8EC20C3F8EC10D3F81421C3F8C1 +:1005C0001411D3F81421D3F8F020C3F8F010D3F8A5 +:1005D0001821C3F81811D3F81821D3F8802042F05D +:1005E0000062C3F88020D3F8802022F00062C3F8B4 +:1005F0008020D3F88020D3F8802042F00072C3F826 +:100600008020D3F8802022F00072C3F88020D3F835 +:10061000803072B64FF0E023C3F8084DD4E90004EF +:10062000BFF34F8FBFF36F8F224AC2F88410BFF31E +:100630004F8F536923F480335361BFF34F8FD2F848 +:10064000803043F6E076C3F3C905C3F34E335B0154 +:1006500003EA060C29464CEA81770139C2F8747224 +:10066000F9D2203B13F1200FF2D1BFF34F8FBFF32C +:100670006F8FBFF34F8FBFF36F8F536923F4003336 +:1006800053610023C2F85032BFF34F8FBFF36F8F17 +:10069000302383F31188854680F308882047F8BD0E +:1006A0000000060820000608FFFF050800220020C1 +:1006B0000044025800ED00E02DE9F04F93B0B44B38 +:1006C0002022FF2100900AA89D6800F0E3FBB14AB8 +:1006D0001378A3B90121B04811700360302383F36C +:1006E000118803680BB101F03FFD0023AB4A4FF4C2 +:1006F0007A71A94801F02EFD002383F31188009B35 +:1007000013B1A74B009A1A60A64A1378032B03D0A3 +:1007100000231370A24A53604FF0000A009CD34696 +:100720005646D146012000F06BFB24B19C4B1B6860 +:10073000002B00F02682002000F082FA0390039B39 +:10074000002BF2DB012000F051FB039B213B1F2B10 +:10075000E8D801A252F823F0D907000801080008E0 +:100760009508000825070008250700082507000848 +:1007700027090008F70A0008110A0008730A000890 +:100780009B0A0008C10A000825070008D30A0008D0 +:1007900025070008450B0008790800082507000810 +:1007A000890B0008E50700087908000825070008FC +:1007B000730A000825070008250700082507000818 +:1007C0002507000825070008250700082507000859 +:1007D00025070008950800080220FFF759FE0028A9 +:1007E00040F0F981009B022105A8BAF1000F08BF73 +:1007F0001C4641F21233ADF8143000F03FFA91E795 +:100800004FF47A7000F01CFA071EEBDB0220FFF7B2 +:100810003FFE0028E6D0013F052F00F2DE81DFE831 +:1008200007F0030A0D1013360523042105A80593CC +:1008300000F024FA17E004215548F9E704215A484A +:10084000F6E704215948F3E74FF01C08404608F149 +:10085000040800F03FFA0421059005A800F00EFA04 +:10086000B8F12C0FF2D101204FF0000900FA07F780 +:1008700047EA0B0B5FFA8BFB00F058FB26B10BF03D +:100880000B030B2B08BF0024FFF70AFE4AE70421E5 +:100890004748CDE7002EA5D00BF00B030B2BA1D1C1 +:1008A0000220FFF7F5FD074600289BD00120002617 +:1008B00000F00EFA0220FFF73BFE1FFA86F84046D2 +:1008C00000F016FA0446B0B1039940460136A1F192 +:1008D00040025142514100F01BFA0028EDD1BA46C6 +:1008E000044641F21213022105A83E46ADF8143029 +:1008F00000F0C4F916E725460120FFF719FE244B46 +:100900009B68AB4207D9284600F0E4F9013040F07B +:1009100067810435F3E70025224BBA463E461D7039 +:100920001F4B5D60A8E7002E3FF45CAF0BF00B039C +:100930000B2B7FF457AF0220FFF7FAFD322000F0B7 +:100940007FF9B0F10008FFF64DAF18F003077FF410 +:1009500049AF0F4A08EB0503926893423FF642AF56 +:10096000B8F5807F3FF73EAF124BB845019323DDCA +:100970004FF47A7000F064F90390039A002AFFF6AE +:1009800031AF039A0137019B03F8012BEDE700BF5C +:10099000002200207C23002060220020B9040008EF +:1009A000802300207822002004220020082200203A +:1009B0000C2200207C220020C820FFF769FD07469A +:1009C00000283FF40FAF1F2D11D8C5F120020AAB4C +:1009D00025F0030084494245184428BF424601924D +:1009E00000F032FA019AFF217F4800F053FA4FEAF3 +:1009F000A803C8F387027C492846019300F052FA05 +:100A0000064600283FF46DAF019B05EB830533E7F5 +:100A10000220FFF73DFD00283FF4E4AE00F096F918 +:100A200000283FF4DFAE0027B846704B9B68BB42FE +:100A300018D91F2F11D80A9B01330ED027F00303BA +:100A400012AA134453F8203C05934046042205A9FA +:100A5000043700F001FB8046E7E7384600F03AF93A +:100A60000590F2E7CDF81480042105A800F006F9FE +:100A700002E70023642104A8049300F0F5F800289D +:100A80007FF4B0AE0220FFF703FD00283FF4AAAECA +:100A9000049800F051F90590E6E70023642104A8CA +:100AA000049300F0E1F800287FF49CAE0220FFF7E9 +:100AB000EFFC00283FF496AE049800F03FF9EAE717 +:100AC0000220FFF7E5FC00283FF48CAE00F04EF961 +:100AD000E1E70220FFF7DCFC00283FF483AE05A924 +:100AE000142000F049F907460421049004A800F0FE +:100AF000C5F83946B9E7322000F0A2F8071EFFF624 +:100B000071AEBB077FF46EAE384A07EB09039268FB +:100B100093423FF667AE0220FFF7BAFC00283FF48D +:100B200061AE27F003074F44B9453FF4A5AE4846F0 +:100B300009F1040900F0CEF80421059005A800F0A1 +:100B40009DF8F1E74FF47A70FFF7A2FC00283FF41C +:100B500049AE00F0FBF8002844D00A9B01330BD0CB +:100B600008220AA9002000F09DF900283AD020228E +:100B7000FF210AA800F08EF9FFF792FC1C4801F053 +:100B80002DFA13B0BDE8F08F002E3FF42BAE0BF022 +:100B90000B030B2B7FF426AE0023642105A80593DD +:100BA00000F062F8074600287FF41CAE0220FFF731 +:100BB0006FFC804600283FF415AEFFF771FC41F250 +:100BC000883001F00BFA059800F0E4F946463C46FF +:100BD00000F0ACF9A6E506464EE64FF0000901E646 +:100BE000BA467EE637467CE67C22002000220020C2 +:100BF000A08601000F4B70B51B780C460133DBB2A9 +:100C0000012B11D80C4D4FF47A732968A2FB0332E3 +:100C100022460E6A01462846B047844204D1074B5B +:100C2000002201201A7070BD4FF4FA7001F0D6F95D +:100C30000020F8E710220020B0260020B423002076 +:100C4000002307B5024601210DF107008DF807309A +:100C5000FFF7D0FF20B19DF8070003B05DF804FB5B +:100C60004FF0FF30F9E700000A46042108B5FFF70E +:100C7000C1FF80F00100C0B2404208BD30B4054C55 +:100C80000A46014623682046DD69034BAC4630BC6A +:100C9000604700BFB0260020A086010070B50A4E54 +:100CA00000240A4D01F0B4FC308028683388834268 +:100CB00008D901F0A9FC2B6804440133B4F5C02F16 +:100CC0002B60F2D370BD00BFB62300208823002024 +:100CD00001F06CBD00F1006000F5C02000687047B5 +:100CE00000F10060920000F5C02001F0EDBC0000B2 +:100CF000054B1A68054B1B889B1A834202D9104486 +:100D000001F082BC0020704788230020B623002019 +:100D100038B50446074D29B128682044BDE838405D +:100D200001F08ABC2868204401F074FC0028F3D04C +:100D300038BD00BF882300200020704700F1FF501D +:100D400000F58F10D0F8000870470000064991F8B0 +:100D5000243033B100230822086A81F82430FFF7D9 +:100D6000BFBF0120704700BF8C230020014B1868D3 +:100D7000704700BF0010005C194B01380322084483 +:100D800070B51D68174BC5F30B042D0C1E88A642C9 +:100D90000BD15C680A46013C824213460FD214F91B +:100DA000016F4EB102F8016BF6E7013A03F1080357 +:100DB000ECD181420B4602D22C2203F8012B0424F1 +:100DC000094A1688AE4204D1984284BF967803F847 +:100DD000016B013C02F10402F3D1581A70BD00BF4F +:100DE0000010005C1422002048440008022803D1AF +:100DF000024B4FF000529A61704700BF0008025842 +:100E0000022803D1024B4FF400529A61704700BF91 +:100E100000080258022804D1024A536983F400539F +:100E2000536170470008025870B504464FF47A7653 +:100E30004CB1412C254628BF412506FB05F0641B1B +:100E400001F0CCF8F4E770BD002310B5934203D055 +:100E5000CC5CC4540133F9E710BD0000013810B573 +:100E600010F9013F3BB191F900409C4203D11AB106 +:100E70000131013AF4E71AB191F90020981A10BD36 +:100E80001046FCE703460246D01A12F9011B00295E +:100E9000FAD1704702440346934202D003F8011B83 +:100EA000FAE770472DE9F8431F4D14460746884678 +:100EB00095F8242052BBDFF870909CB395F824304D +:100EC0002BB92022FF2148462F62FFF7E3FF95F858 +:100ED00024004146C0F1080205EB8000A24228BF71 +:100EE0002246D6B29200FFF7AFFF95F82430A41B3C +:100EF00017441E449044E4B2F6B2082E85F82460EC +:100F0000DBD1FFF723FF0028D7D108E02B6A03EBE2 +:100F100082038342CFD0FFF719FF0028CBD10020F6 +:100F2000BDE8F8830120FBE78C230020024B1A78F0 +:100F3000024B1A70704700BFB4230020102200201B +:100F400010B5114C114800F079FB21460F4800F014 +:100F5000A1FB24684FF47A70D4F89020D2F80438BA +:100F600043F00203C2F80438FFF75EFF0849204649 +:100F700000F09EFCD4F89020D2F8043823F002034D +:100F8000C2F8043810BD00BF04460008B026002097 +:100F90000C46000870470000F8B571B60023012028 +:100FA0001A46194600F058FA0446802001F09CFCCD +:100FB000002849D00025254A80274FF4D06C3D26D3 +:100FC000136913F0C06F26D1D2F8103113F0C06F3F +:100FD00021D1236805F1006199602368D8602368F6 +:100FE0005F602368C3F800C021680B6843F0010309 +:100FF0000B6021680B6823F01E030B6021680B68EF +:10100000DB07FCD4237B8035616806FA03F3B5F572 +:10101000001F0B60D4D1204600F05EFAB5F5001F2A +:1010200011D000240A4E0B4D012001F0BFFB338884 +:10103000A34205D928682044013401F0FDFAF6E7FF +:10104000002001F0B3FB61B6F8BD00BF00200052E4 +:10105000B62300208823002030B50A44084D914271 +:101060000DD011F8013B5840082340F30004013B28 +:101070002C4013F0FF0384EA5000F6D1EFE730BDB7 +:101080002083B8ED08B5074B074A196801F03D0108 +:10109000996053680BB190689847BDE8084001F02B +:1010A00005BD00BF00000240B823002008B5084B72 +:1010B0001968890901F03D018A019A60054AD368DF +:1010C0000BB110699847BDE8084001F0EFBC00BFC4 +:1010D00000000240B823002008B5084B1968090C2D +:1010E00001F03D010A049A60054A53690BB1906909 +:1010F0009847BDE8084001F0D9BC00BF000002409D +:10110000B823002008B5084B1968890D01F03D018E +:101110008A059A60054AD3690BB1106A9847BDE801 +:10112000084001F0C3BC00BF00000240B82300200B +:1011300008B5074B074A596801F03D01D960536A69 +:101140000BB1906A9847BDE8084001F0AFBC00BF02 +:1011500000000240B823002008B5084B59688909EF +:1011600001F03D018A01DA60054AD36A0BB1106BC8 +:101170009847BDE8084001F099BC00BF000002405C +:10118000B823002008B5084B5968090C01F03D014F +:101190000A04DA60054A536B0BB1906B9847BDE8BF +:1011A000084001F083BC00BF00000240B8230020CB +:1011B00008B5084B5968890D01F03D018A05DA60D0 +:1011C000054AD36B0BB1106C9847BDE8084001F09D +:1011D0006DBC00BF00000240B823002008B5074BDB +:1011E000074A196801F03D019960536C0BB1906C8E +:1011F0009847BDE8084001F059BC00BF0004024018 +:10120000B823002008B5084B1968890901F03D0191 +:101210008A019A60054AD36C0BB1106D9847BDE8FE +:10122000084001F043BC00BF00040240B823002086 +:1012300008B5084B1968090C01F03D010A049A60D1 +:10124000054A536D0BB1906D9847BDE8084001F019 +:101250002DBC00BF00040240B823002008B5084B95 +:101260001968890D01F03D018A059A60054AD36D20 +:101270000BB1106E9847BDE8084001F017BC00BFE5 +:1012800000040240B823002008B5074B074A5968FC +:1012900001F03D01D960536E0BB1906E9847BDE8E7 +:1012A000084001F003BC00BF00040240B823002046 +:1012B00008B5084B5968890901F03D018A01DA60D7 +:1012C000054AD36E0BB1106F9847BDE8084001F096 +:1012D000EDBB00BF00040240B823002008B5084B56 +:1012E0005968090C01F03D010A04DA60054A536FA0 +:1012F0000BB1906F9847BDE8084001F0D7BB00BF25 +:1013000000040240B823002008B5084B5968890D35 +:1013100001F03D018A05DA60054AD36F13B1D2F8B6 +:1013200080009847BDE8084001F0C0BB00040240BF +:10133000B823002000230C4910B51A460B4C0B6053 +:1013400054F82300026001EB430004334260402B59 +:10135000F6D1074A4FF0FF339360D360C2F80834E8 +:10136000C2F80C3410BD00BFB82300205844000858 +:10137000000002400F28F8B510D9102810D011280D +:1013800011D0122808D10F240720DFF8C8E0012669 +:10139000DEF80050A04208D9002653E00446F4E7E6 +:1013A0000F240020F1E70724FBE706FA00F73D428F +:1013B0004AD1264C4FEA001C3D4304EB00160EEBCD +:1013C000C000CEF80050C0E90123FBB273B1204841 +:1013D000D0F8D83043F00103C0F8D830D0F800314D +:1013E00043F00103C0F80031D0F8003117F47F4F0B +:1013F0000ED01748D0F8D83043F00203C0F8D830E8 +:10140000D0F8003143F00203C0F80031D0F80031C9 +:1014100054F80C00036823F01F030360056815F0FF +:101420000105FBD104EB0C033D2493F80CC05F686D +:1014300004FA0CF43C6021240560446112B1987BED +:1014400000F054F83046F8BD0130A3E758440008D6 +:1014500000440258B823002010B5302484F31188CA +:10146000FFF788FF002383F3118810BD10B50446F1 +:10147000807B00F051F801231549627B03FA02F2E8 +:101480000B6823EA0203DAB20B6072B9114AD2F890 +:10149000D81021F00101C2F8D810D2F8001121F0C3 +:1014A0000101C2F80011D2F8002113F47F4F0ED1D0 +:1014B000084BD3F8D82022F00202C3F8D820D3F882 +:1014C000002122F00202C3F80021D3F8003110BD40 +:1014D000B82300200044025808B5302383F3118854 +:1014E000FFF7C4FF002383F3118808BD090100F151 +:1014F0006043012203F56143C9B283F8001300F091 +:101500001F039A4043099B0003F1604303F56143C5 +:10151000C3F880211A60704700F01F0301229A402F +:10152000430900F160409B0000F5614003F1604316 +:1015300003F56143C3F88020C3F88021002380F8BD +:1015400000337047026843681143016003B11847D4 +:101550007047000013B5406B00F58054D4F8A438F0 +:101560001A681178042914D1017C022911D1197942 +:10157000012312898B4013420BD101A94C3002F098 +:1015800085F9D4F8A4480246019B2179206800F02F +:10159000DFF902B010BD0000143002F007B90000FE +:1015A0004FF0FF33143002F001B900004C3002F06C +:1015B000D9B900004FF0FF334C3002F0D3B900002E +:1015C000143002F0D5B800004FF0FF31143002F0B3 +:1015D000CFB800004C3002F0A5B900004FF0FF3248 +:1015E0004C3002F09FB900000020704710B500F5A4 +:1015F0008054D4F8A4381A681178042917D1017CD2 +:10160000022914D15979012352898B4013420ED1FA +:10161000143002F067F8024648B1D4F8A4484FF4F9 +:10162000407361792068BDE8104000F07FB910BDBB +:10163000406BFFF7DBBF0000704700007FB5124B27 +:1016400001250426044603600023057400F184028A +:1016500043602946C0E902330C4B029014300193D9 +:101660004FF44073009602F019F8094B04F69442C7 +:10167000294604F14C000294CDE900634FF4407315 +:1016800002F0E0F804B070BD5845000831160008BB +:10169000551500080A68302383F311880B790B3342 +:1016A00042F823004B79133342F823008B7913B1AE +:1016B0000B3342F8230000F58053C3F8A41802232B +:1016C0000374002080F311887047000038B5037F51 +:1016D000044613B190F85430ABB90125201D022106 +:1016E000FFF730FF04F114006FF00101257700F0DF +:1016F0009DFC04F14C0084F854506FF00101BDE8EA +:10170000384000F093BC38BD10B5012104460430C8 +:10171000FFF718FF0023237784F8543010BD000032 +:1017200038B504460025143001F0D0FF04F14C0018 +:10173000257702F09FF8201D84F854500121FFF70F +:1017400001FF2046BDE83840FFF750BF90F88030D9 +:1017500003F06003202B06D190F881200023212A7A +:1017600003D81F2A06D800207047222AFBD1C0E9DF +:101770001D3303E0034A426707228267C3670120E3 +:10178000704700BF2C22002037B500F58055D5F8F2 +:10179000A4381A68117804291AD1017C022917D1BA +:1017A0001979012312898B40134211D100F14C04A5 +:1017B000204602F01FF958B101A9204602F066F850 +:1017C000D5F8A4480246019B2179206800F0C0F8B2 +:1017D00003B030BD01F10B03F0B550F8236085B0C4 +:1017E00004460D46FEB1302383F3118804EB8507D0 +:1017F000301D0821FFF7A6FEFB6806F14C005B696F +:101800001B681BB1019002F04FF8019803A902F088 +:101810003DF8024648B1039B2946204600F098F85F +:10182000002383F3118805B0F0BDFB685A69126884 +:10183000002AF5D01B8A013B1340F1D104F180024C +:10184000EAE70000133138B550F82140ECB13023FD +:1018500083F3118804F58053D3F8A42813685279D0 +:1018600003EB8203DB689B695D6845B10421601866 +:10187000FFF768FE294604F1140001F03DFF204601 +:10188000FFF7B4FE002383F3118838BD70470000D2 +:1018900001F00ABA01234022002110B5044600F8E5 +:1018A000303BFFF7F7FA0023C4E9013310BD000015 +:1018B00010B53023044683F31188242241600021AF +:1018C0000C30FFF7E7FA204601F010FA022300205F +:1018D000237080F3118810BD70B500EB81030546BD +:1018E00050690E461446DA6018B110220021FFF745 +:1018F000D1FAA06918B110220021FFF7CBFA3146C6 +:101900002846BDE8704001F0F7BA00008368202245 +:10191000002103F0011310B5044683601030FFF777 +:10192000B9FA2046BDE8104001F072BBF0B40125C1 +:1019300000EB810447898D40E4683D43A4694581FB +:1019400023600023A2606360F0BC01F08FBB000045 +:10195000F0B4012500EB810407898D40E4683D4324 +:101960006469058123600023A2606360F0BC01F01C +:1019700005BC000070B50223002504462422037034 +:101980002946C0F888500C3040F8045CFFF782FA12 +:10199000204684F8705001F043FA63681B6823B155 +:1019A00029462046BDE87040184770BD0378052BD6 +:1019B00010B504460AD080F88C30052303704368C4 +:1019C0001B680BB1042198470023A36010BD0000E1 +:1019D0000178052906D190F88C20436802701B68B5 +:1019E00003B118477047000070B590F87030044696 +:1019F00013B1002380F8703004F18002204601F01A +:101A00002BFB63689B68B3B994F8803013F06005D2 +:101A100035D00021204601F01DFE0021204601F0B6 +:101A20000DFE63681B6813B1062120469847062304 +:101A300084F8703070BD204698470028E4D0B4F890 +:101A40008630A26F9A4288BFA36794F98030A56F51 +:101A5000002B4FF0300380F20381002D00F0F28064 +:101A6000092284F8702083F3118800212046D4E9EC +:101A70001D23FFF76DFF002383F31188DAE794F845 +:101A8000812003F07F0343EA022340F202329342B3 +:101A900000F0C58021D8B3F5807F48D00DD8012B48 +:101AA0003FD0022B00F09380002BB2D104F18802CA +:101AB00062670222A267E367C1E7B3F5817F00F0A6 +:101AC0009B80B3F5407FA4D194F88230012BA0D144 +:101AD000B4F8883043F0020332E0B3F5006F4DD024 +:101AE00017D8B3F5A06F31D0A3F5C063012B90D800 +:101AF0006368204694F882205E6894F88310B4F8F6 +:101B00008430B047002884D0436863670368A367C4 +:101B10001AE0B3F5106F36D040F6024293427FF4DC +:101B200078AF5C4B63670223A3670023C3E794F895 +:101B30008230012B7FF46DAFB4F8883023F00203BC +:101B4000A4F88830C4E91D55E56778E7B4F880301B +:101B5000B3F5A06F0ED194F88230204684F88A3015 +:101B600001F0BCF963681B6813B101212046984756 +:101B7000032323700023C4E91D339CE704F18B0386 +:101B800063670123C3E72378042B10D1302383F349 +:101B900011882046FFF7BAFE85F311880321636898 +:101BA00084F88B5021701B680BB12046984794F83D +:101BB0008230002BDED084F88B30042323706368DE +:101BC0001B68002BD6D0022120469847D2E794F814 +:101BD000843020461D0603F00F010AD501F02EFACD +:101BE000012804D002287FF414AF2B4B9AE72B4B2B +:101BF00098E701F015FAF3E794F88230002B7FF4B0 +:101C000008AF94F8843013F00F01B3D01A062046C1 +:101C100002D501F037FDADE701F028FDAAE794F801 +:101C20008230002B7FF4F5AE94F8843013F00F016E +:101C3000A0D01B06204602D501F00CFD9AE701F06A +:101C4000FDFC97E7142284F8702083F311882B465B +:101C50002A4629462046FFF769FE85F31188E9E602 +:101C60005DB1152284F8702083F31188002120468D +:101C7000D4E91D23FFF75AFEFDE60B2284F87020FD +:101C800083F311882B462A4629462046FFF760FE3B +:101C9000E3E700BF88450008804500088445000848 +:101CA00038B590F870300446002B3ED0063BDAB2CF +:101CB0000F2A34D80F2B32D8DFE803F03731310840 +:101CC000223231313131313131313737856FB0F82E +:101CD00086309D4214D2C3681B8AB5FBF3F203FB26 +:101CE00012556DB9302383F311882B462A462946B5 +:101CF000FFF72EFE85F311880A2384F870300EE07A +:101D0000142384F87030302383F311880023204695 +:101D10001A461946FFF70AFE002383F3118838BDDF +:101D2000C36F03B198470023E7E70021204601F085 +:101D300091FC0021204601F081FC63681B6813B10F +:101D40000621204698470623D7E7000010B590F8F3 +:101D500070300446142B29D017D8062B05D001D893 +:101D60001BB110BD093B022BFBD80021204601F01E +:101D700071FC0021204601F061FC63681B6813B10F +:101D8000062120469847062319E0152BE9D10B239D +:101D900080F87030302383F3118800231A461946E7 +:101DA000FFF7D6FD002383F31188DAE7C3689B6948 +:101DB0005B68002BD5D1C36F03B19847002384F82B +:101DC0007030CEE700238268037503691B68996849 +:101DD0009142FBD25A6803604260106058607047BD +:101DE00000238268037503691B6899689142FBD8D8 +:101DF0005A680360426010605860704708B5084632 +:101E0000302383F311880A7D0023052A06D8DFE8F2 +:101E100002F00B050503120E826913604FF0FF33C9 +:101E20008361FFF7CFFF002383F3118808BD826928 +:101E3000936801339360D0E9003213605A60EDE794 +:101E4000FFF7C0BF054BD968087518680268536072 +:101E50001A600122D8600275FEF7D4BA402400202F +:101E60000C4B30B5DD684B1C87B004460FD02B46B9 +:101E7000094A684600F06EF92046FFF7E3FF009B31 +:101E800013B1684600F070F9A86907B030BDFFF7DC +:101E9000D9FFF9E740240020FD1D000838B50C4D9E +:101EA00004468161EB6881689A68914203D8BDE875 +:101EB0003840FFF787BF1846FFF792FF012301461E +:101EC000EC6020462375BDE83840FEF79BBA00BFA2 +:101ED00040240020044B1A68DB6890689B68984295 +:101EE00094BF00200120704740240020084B10B50B +:101EF0001C68D868226853601A600122DC60227571 +:101F0000FFF76EFF01462046BDE81040FEF77ABAA3 +:101F10004024002038B50123084C0025237065605B +:101F200001F0A4FD01F0CAFD0549064801F05CFE80 +:101F30000223237085F3118838BD00BFA826002036 +:101F4000904500084024002000F044B9034A51683D +:101F500053685B1A9842FBD8704700BF001000E03E +:101F60008B600223086108468B8270478368A3F167 +:101F7000840243F8142C026943F8442C426943F864 +:101F8000402C094A43F8242CC268A3F1200043F8EE +:101F9000182C022203F80C2C002203F80B2C034A05 +:101FA00043F8102C704700BF1D0400084024002097 +:101FB00008B5FFF7DBFFBDE80840FFF741BF0000B1 +:101FC000024BDB6898610F20FFF73CBF40240020E4 +:101FD000302383F31188FFF7F3BF000008B50146F3 +:101FE000302383F311880820FFF73AFF002383F39F +:101FF000118808BD064BDB6839B1426818605A6029 +:10200000136043600420FFF72BBF4FF0FF30704791 +:10201000402400200368984206D01A68026050608D +:1020200018469961FFF70CBF7047000038B50446A9 +:102030000D462068844200D138BD036823605C608F +:102040008561FFF7FDFEF4E7036810B59C68A242C6 +:102050000CD85C688A600B604C602160596099689C +:102060008A1A9A604FF0FF33836010BD121B1B6801 +:10207000ECE700000A2938BF0A2170B504460D4676 +:102080000A26601901F0ECFC01F0D4FC041BA54207 +:1020900003D8751C04462E46F3E70A2E04D9012006 +:1020A000BDE8704001F0E0BD70BD0000F8B5144B14 +:1020B0000D460A2A4FF00A07D96103F11001826028 +:1020C00038BF0A22416019691446016048601861EE +:1020D000A81801F0B5FC01F0ADFC431B0646A34275 +:1020E00006D37C1C28192746354601F0B9FCF2E7D7 +:1020F0000A2F04D90120BDE8F84001F0B5BDF8BDB4 +:1021000040240020F8B506460D4601F093FC0F4A26 +:10211000134653F8107F9F4206D12A4601463046A7 +:10212000BDE8F840FFF7C2BFD169BB68441A2C195B +:1021300028BF2C46A34202D92946FFF79BFF22461F +:1021400031460348BDE8F840FFF77EBF4024002039 +:1021500050240020C0E90323002310B45DF8044B91 +:102160004361FFF7CFBF000010B5194C23699842B7 +:102170000DD08168D0E9003213605A609A680A4431 +:102180009A60002303604FF0FF33A36110BD026823 +:10219000234643F8102F53600022026022699A42BE +:1021A00003D1BDE8104001F055BC936881680B4431 +:1021B000936001F03FFC2269E1699268441AA242EF +:1021C000E4D91144BDE81040091AFFF753BF00BF1E +:1021D000402400202DE9F047DFF8BC8008F110070B +:1021E0002C4ED8F8105001F025FCD8F81C40AA68F5 +:1021F000031B9A423ED814444FF00009D5E900323F +:10220000C8F81C4013605A60C5F80090D8F8103028 +:10221000B34201D101F01EFC89F31188D5E90331E5 +:1022200028469847302383F311886B69002BD8D058 +:1022300001F000FC6A69A0EB040982464A450DD210 +:10224000022001F011FD0022D8F81030B34208D16D +:1022500051462846BDE8F047FFF728BF121A22442E +:10226000F2E712EB09092946384638BF4A46FFF71C +:10227000EBFEB5E7D8F81030B34208D01444C8F8E4 +:102280001C00211AA960BDE8F047FFF7F3BEBDE8C6 +:10229000F08700BF50240020402400200020704719 +:1022A000FEE70000704700004FF0FF30704700006D +:1022B00002290CD0032904D00129074818BF0020A7 +:1022C0007047032A05D8054800EBC2007047044850 +:1022D00070470020704700BF684600083C2200207D +:1022E0001C46000870B59AB005460846144601A978 +:1022F00000F0C2F801A8FEF7C5FD431C0022C6B2DB +:102300005B001046C5E9003423700323023404F84F +:10231000013C01ABD1B202348E4201D81AB070BD7B +:1023200013F8011B013204F8010C04F8021CF1E758 +:1023300008B5302383F311880348FFF725FA0023FB +:1023400083F3118808BD00BFB026002090F88030CC +:1023500003F01F02012A07D190F881200B2A03D134 +:102360000023C0E91D3315E003F06003202B08D1E2 +:10237000B0F884302BB990F88120212A03D81F2A85 +:1023800004D8FFF7E3B9222AEBD0FAE7034A426701 +:1023900007228267C3670120704700BF33220020F5 +:1023A00007B5052917D8DFE801F019160319192018 +:1023B000302383F31188104A01210190FFF78CFA32 +:1023C000019802210D4AFFF787FA0D48FFF7A8F997 +:1023D000002383F3118803B05DF804FB302383F3FB +:1023E00011880748FFF772F9F2E7302383F3118869 +:1023F0000348FFF789F9EBE7BC450008E045000812 +:10240000B026002038B50C4D0C4C2A460C4904F17E +:102410000800FFF767FF05F1CA0204F1100009493F +:10242000FFF760FF05F5CA7204F118000649BDE820 +:102430003840FFF757BF00BF883F00203C220020F4 +:102440009C450008A6450008B145000870B5044643 +:1024500008460D46FEF716FDC6B220460134037845 +:102460000BB9184670BD32462946FEF7F7FC002826 +:10247000F3D10120F6E700002DE9F04705460C46B0 +:10248000FEF700FD2A49C6B22846FFF7DFFF08B174 +:102490000936F6B227492846FFF7D8FF08B11036AB +:1024A000F6B2632E0BD8DFF88880DFF88890224FD1 +:1024B000DFF890A02E7846B92670BDE8F08729464F +:1024C0002046BDE8F04701F0A1BF252E2CD1072200 +:1024D00041462846FEF7C2FC60B9184B224603F17C +:1024E000100153F8040B8B4242F8040BF9D1073565 +:1024F0001034DFE7082249462846FEF7AFFC98B9BA +:10250000A21C0F4B197802320909C95D02F8041C9C +:1025100013F8011B01F00F015345C95D02F8031CBC +:10252000F0D118340835C5E7013504F8016BC1E76F +:1025300088460008B1450008A146000890460008FA +:1025400000E8F11F0CE8F11FBFF34F8F044B1A692D +:102550005107FCD1D3F810215207F8D1704700BFC2 +:102560000020005208B50D4B1B78ABB9FFF7ECFF0C +:102570000B4BDA68D10704D50A4A5A6002F1883257 +:102580005A60D3F80C21D20706D5064AC3F80421B5 +:1025900002F18832C3F8042108BD00BFE6410020E3 +:1025A000002000522301674508B5114B1B78F3B991 +:1025B000104B1A69510703D5DA6842F04002DA601D +:1025C000D3F81021520705D5D3F80C2142F0400270 +:1025D000C3F80C21FFF7B8FF064BDA6842F001029E +:1025E000DA60D3F80C2142F00102C3F80C2108BDD7 +:1025F000E6410020002000520F289ABF00F58060BD +:1026000040040020704700004FF400307047000085 +:10261000102070470F2808B50BD8FFF7EDFF00F525 +:1026200000330268013204D104308342F9D1012021 +:1026300008BD0020FCE700000F2838B505463FD84C +:10264000FFF782FF1F4CFFF78DFF4FF0FF33072886 +:102650006361C4F814311DD82361FFF775FF0302CD +:1026600043F02403E360E36843F08003E3602369FD +:102670005A07FCD42846FFF767FFFFF7BDFF4FF46A +:10268000003100F0B3F92846FFF78EFFBDE838406F +:10269000FFF7C0BFC4F81031FFF756FFA0F10803E1 +:1026A0001B0243F02403C4F80C31D4F80C3143F07E +:1026B0008003C4F80C31D4F810315B07FBD4D9E7A0 +:1026C000002038BD002000522DE9F84F05460C4689 +:1026D000104645EA0203DE0602D00020BDE8F88F6E +:1026E00020F01F00DFF8BCB0DFF8BCA0FFF73AFF16 +:1026F00004EB0008444503D10120FFF755FFEDE747 +:1027000020222946204601F071FE10B920352034E0 +:10271000F0E72B4605F120021F68791CDDD1043358 +:102720009A42F9D105F178431B481C4EB3F5801F3E +:102730001B4B38BF184603F1F80332BFD946D146C8 +:102740001E46FFF701FF0760A5EB040C336804F198 +:102750001C0143F002033360231FD9F8007017F007 +:102760000507FAD153F8042F8B424CF80320F4D11B +:10277000BFF34F8FFFF7E8FE4FF0FF3320222146D3 +:1027800003602846336823F00203336001F02EFE15 +:102790000028BBD03846B0E7142100520C2000526C +:1027A00014200052102000521021005210B5084C85 +:1027B000237828B11BB9FFF7D5FE0123237010BD84 +:1027C000002BFCD02070BDE81040FFF7EDBE00BF2D +:1027D000E64100202DE9F04F0D4685B0814658B105 +:1027E00011F00D0614BF2022082211F008030193F6 +:1027F00004D0431E03426AD0002435E0002E37D0B7 +:1028000009F11F0121F01F094FF00108324F05F0B7 +:102810000403DFF8D0A005EA080BBBF1000F32D0AB +:102820007869C0072FD408F101080C37B8F1060FFA +:10283000F3D19EB9294D4946A819019201F060F9DA +:10284000044600283AD11836019A782EF3D1494629 +:1028500001F056F90446002830D1019A4946204833 +:1028600001F04EF9044668BB204605B0BDE8F08F84 +:102870000029C9D101462846029201F041F90446D7 +:10288000E0B9029AC0E713B178694107CBD5AC072C +:1028900002D578698007C6D5019911B17869010719 +:1028A000C1D51820494600FB08A0CDE9022301F05C +:1028B00027F90446DDE902230028B4D04A46002166 +:1028C000204601E04A460021FEF7E4FACCE7024642 +:1028D000002E95D198E700BFB446000818420020AA +:1028E000E8410020004200200121FFF773BF0000F3 +:1028F000F8B5144D01241827134E40F2FF32002181 +:102900000120FEF7C7FA07FB046001342A6955F875 +:102910000C1F01F0E1F8062CF5D137254FF4C05417 +:102920002046FFF7E1FF014628B122460748BDE8EF +:10293000F84001F0D1B8C4EBC404013D4FEAD4041F +:10294000EED1F8BDB446000800420020E841002066 +:102950000244074BD2B210B5904200D110BD441CC6 +:1029600000B253F8200041F8040BE0B2F4E700BFD6 +:10297000504000580E4B30B51C6F240405D41C6F1A +:102980001C671C6F44F400441C670A4C0244236813 +:10299000D2B243F480732360074B904200D130BD24 +:1029A000441C51F8045B00B243F82050E0B2F4E755 +:1029B00000440258004802585040005807B5012210 +:1029C00001A90020FFF7C4FF019803B05DF804FBE4 +:1029D00013B50446FFF7F2FFA04205D0012201A97A +:1029E00000200194FFF7C6FF02B010BD0144BFF301 +:1029F0004F8F064B884204D3BFF34F8FBFF36F8FC7 +:102A00007047C3F85C022030F4E700BF00ED00E03F +:102A1000034B1A681AB9034AD2F8D0241A607047D7 +:102A2000904200200040025808B5FFF7F1FF024B2A +:102A30001868C0F3806008BD90420020EFF309835E +:102A4000054968334A6B22F001024A6383F309881F +:102A5000002383F31188704700EF00E0302080F3FB +:102A6000118862B60D4B0E4AD96821F4E061090461 +:102A7000090C0A430B49DA60D3F8FC2042F080725B +:102A8000C3F8FC20084AC2F8B01F116841F00101E8 +:102A900011602022DA7783F82200704700ED00E011 +:102AA0000003FA0555CEACC5001000E0302310B588 +:102AB00083F311880E4B5B6813F4006314D0F1EEBE +:102AC000103AEFF309844FF08073683CE361094BDF +:102AD000DB6B236684F30988FFF7FCF910B1064B22 +:102AE000A36110BD054BFBE783F31188F9E700BF35 +:102AF00000ED00E000EF00E02F04000832040008C1 +:102B000070B5BFF34F8FBFF36F8F1A4A0021C2F821 +:102B10005012BFF34F8FBFF36F8F536943F40033ED +:102B20005361BFF34F8FBFF36F8FC2F88410BFF3B1 +:102B30004F8FD2F8803043F6E074C3F3C900C3F37B +:102B40004E335B0103EA0406014646EA817501390A +:102B5000C2F86052F9D2203B13F1200FF2D1BFF33B +:102B60004F8F536943F480335361BFF34F8FBFF3EB +:102B70006F8F70BD00ED00E0FEE70000214B2248A2 +:102B8000224A70B5904237D3214BC11EDA1C121A6B +:102B900022F003028B4238BF00220021FEF77AF9AF +:102BA0001C4A0023C2F88430BFF34F8FD2F8803024 +:102BB00043F6E074C3F3C900C3F34E335B0103EA89 +:102BC0000406014646EA81750139C2F86C52F9D211 +:102BD000203B13F1200FF2D1BFF34F8FBFF36F8F64 +:102BE000BFF34F8FBFF36F8F0023C2F85032BFF394 +:102BF0004F8FBFF36F8F70BD53F8041B40F8041B59 +:102C0000C0E700BFD448000884440020844400206A +:102C10008444002000ED00E0074BD3F8D81021EAEF +:102C20000001C3F8D810D3F8002122EA0002C3F84B +:102C30000021D3F8003170470044025870B5D0E944 +:102C4000244300224FF0FF359E6804EB42135101EC +:102C5000D3F80009002805DAD3F8000940F08040D5 +:102C6000C3F80009D3F8000B002805DAD3F8000BED +:102C700040F08040C3F8000B013263189642C3F85D +:102C80000859C3F8085BE0D24FF00113C4F81C38B0 +:102C900070BD0000890141F02001016103699B06BC +:102CA000FCD41220FFF752B910B50A4C2046FEF7AB +:102CB000F1FD094BC4F89030084BC4F89430084C2F +:102CC0002046FEF7E7FD074BC4F89030064BC4F8EA +:102CD000943010BD944200200000084020470008B6 +:102CE00030430020000004402C47000870B50378F2 +:102CF0000546012B5CD1434BD0F89040984258D107 +:102D0000414B0E216520D3F8D82042F00062C3F871 +:102D1000D820D3F8002142F00062C3F80021D3F894 +:102D20000021D3F8802042F00062C3F88020D3F85D +:102D3000802022F00062C3F88020D3F88030FEF7B4 +:102D4000D5FB324BE360324BC4F800380023D5F892 +:102D50009060C4F8003EC02323604FF40413A363C3 +:102D60003369002BFCDA01230C203361FFF7EEF806 +:102D70003369DB07FCD41220FFF7E8F83369002B36 +:102D8000FCDA00262846A660FFF758FF6B68C4F8F7 +:102D90001068DB68C4F81468C4F81C6883BB1D4B5A +:102DA000A3614FF0FF336361A36843F00103A360A5 +:102DB00070BD194B9842C9D1134B4FF08060D3F8C6 +:102DC000D82042F00072C3F8D820D3F8002142F096 +:102DD0000072C3F80021D3F80021D3F8802042F01C +:102DE0000072C3F88020D3F8802022F00072C3F86C +:102DF0008020D3F88030FFF70FFF0E214D209EE793 +:102E0000064BCDE794420020004402584014004095 +:102E100003002002003C30C030430020083C30C09A +:102E2000F8B5D0F89040054600214FF000662046E6 +:102E3000FFF730FFD5F8941000234FF001128F6890 +:102E40004FF0FF30C4F83438C4F81C2804EB4312A8 +:102E500001339F42C2F80069C2F8006BC2F808094A +:102E6000C2F8080BF2D20B68D5F89020C5F898305C +:102E7000636210231361166916F01006FBD112204D +:102E8000FFF764F8D4F8003823F4FE63C4F8003880 +:102E9000A36943F4402343F01003A3610923C4F85A +:102EA0001038C4F814380B4BEB604FF0C043C4F833 +:102EB000103B094BC4F8003BC4F81069C4F8003952 +:102EC000D5F8983003F1100243F48013C5F8982028 +:102ED000A362F8BDFC46000840800010D0F89020A6 +:102EE00090F88A10D2F8003823F4FE6343EA011305 +:102EF000C2F80038704700002DE9F84300EB810369 +:102F0000D0F890500C468046DA680FFA81F94801F3 +:102F1000166806F00306731E022B05EB41134FF0F3 +:102F2000000194BFB604384EC3F8101B4FF00101E6 +:102F300004F1100398BF06F1805601FA03F391697A +:102F400098BF06F5004600293AD0578A04F1580187 +:102F5000374349016F50D5F81C180B430021C5F8C1 +:102F60001C382B180127C3F81019A7405369611E9C +:102F70009BB3138A928B9B08012A88BF5343D8F8CE +:102F80009820981842EA034301F140022146C8F80C +:102F90009800284605EB82025360FFF77BFE08EBA2 +:102FA0008900C3681B8A43EA845348341E43640182 +:102FB0002E51D5F81C381F43C5F81C78BDE8F8839E +:102FC00005EB4917D7F8001B21F40041C7F8001B97 +:102FD000D5F81C1821EA0303C0E704F13F030B4AAC +:102FE0002846214605EB83035A60FFF753FE05EBA5 +:102FF0004910D0F8003923F40043C0F80039D5F85F +:103000001C3823EA0707D7E70080001000040002FD +:10301000D0F894201268C0F89820FFF70FBE000087 +:103020005831D0F8903049015B5813F4004004D077 +:1030300013F4001F0CBF0220012070474831D0F864 +:10304000903049015B5813F4004004D013F4001F82 +:103050000CBF02200120704700EB8101CB68196A88 +:103060000B6813604B6853607047000000EB8103EE +:1030700030B5DD68AA691368D36019B9402B84BFE5 +:10308000402313606B8A1468D0F890201C4402EB34 +:103090004110013C09B2B4FBF3F46343033323F062 +:1030A000030343EAC44343F0C043C0F8103B2B681A +:1030B00003F00303012B0ED1D2F8083802EB4110C4 +:1030C00013F4807FD0F8003B14BF43F0805343F0EB +:1030D0000053C0F8003B02EB4112D2F8003B43F032 +:1030E0000443C2F8003B30BD2DE9F041D0F89060B8 +:1030F00005460C4606EB4113D3F8087B3A07C3F8A4 +:10310000087B08D5D6F814381B0704D500EB8103DB +:10311000DB685B689847FA071FD5D6F81438DB07D9 +:103120001BD505EB8403D968CCB98B69488A5A68EA +:10313000B2FBF0F600FB16228AB91868DA689042F2 +:103140000DD2121AC3E90024302383F311882146DB +:103150002846FFF78BFF84F31188BDE8F081012337 +:1031600003FA04F26B8923EA02036B81CB68002B1C +:10317000F3D021462846BDE8F041184700EB810313 +:103180004A0170B5DD68D0F890306C692668E66059 +:1031900056BB1A444FF40020C2F810092A6802F006 +:1031A0000302012A0AB20ED1D3F8080803EB421435 +:1031B00010F4807FD4F8000914BF40F0805040F034 +:1031C0000050C4F8000903EB4212D2F8000940F0A5 +:1031D0000440C2F800090122D3F8340802FA01F1D0 +:1031E0000143C3F8341870BD19B9402E84BF402084 +:1031F000206020681A442E8A8419013CB4FBF6F43E +:1032000040EAC44040F00050C6E700002DE9F84312 +:10321000D0F8906005460C464F0106EB4113D3F8F9 +:10322000088918F0010FC3F808891CD0D6F81038A7 +:10323000DB0718D500EB8103D3F80CC0DCF81430A1 +:10324000D3F800E0DA68964530D2A2EB0E024FF0D8 +:1032500000091A60C3F80490302383F31188FFF744 +:103260008DFF89F3118818F0800F1DD0D6F83438FF +:103270000126A640334217D005EB84030134D5F86C +:103280009050D3F80CC0E4B22F44DCF8142005EBC6 +:103290000434D2F800E05168714514D3D5F83438BD +:1032A00023EA0606C5F83468BDE8F883012303FA6B +:1032B00001F2038923EA02030381DCF80830002BC2 +:1032C000D1D09847CFE7AEEB0103BCF810008342A2 +:1032D00028BF0346D7F8180980B2B3EB800FE3D8B4 +:1032E0009068A0F1040959F8048FC4F80080A0EB9D +:1032F00009089844B8F1040FF5D818440B449060BD +:103300005360C8E72DE9F84FD0F8905004466E6935 +:10331000AB691E4016F480586E6103D0BDE8F84FCB +:10332000FEF728BB002E12DAD5F8003E9B0705D029 +:10333000D5F8003E23F00303C5F8003ED5F8043865 +:10334000204623F00103C5F80438FEF741FB37059A +:1033500005D52046FFF772FC2046FEF727FBB00498 +:103360000CD5D5F8083813F0060FEB6823F470532A +:103370000CBF43F4105343F4A053EB6031071BD54B +:103380006368DB681BB9AB6923F00803AB61237882 +:10339000052B0CD1D5F8003E9A0705D0D5F8003E94 +:1033A00023F00303C5F8003E2046FEF711FB6368D7 +:1033B000DB680BB120469847F30200F1BA80B702F0 +:1033C00026D5D4F8909000274FF0010A09EB471258 +:1033D000D2F8003B03F44023B3F5802F11D1D2F88B +:1033E000003B002B0DDA62890AFA07F322EA030395 +:1033F000638104EB8703DB68DB6813B13946204641 +:1034000098470137D4F89430FFB29B689F42DDD9CA +:10341000F00619D5D4F89000026AC2F30A1702F038 +:103420000F0302F4F012B2F5802F00F0CA80B2F55B +:10343000402F09D104EB8303002200F58050DB68A4 +:103440001B6A974240F0B0803003D5F8185835D544 +:10345000E90303D500212046FFF746FEAA0303D562 +:1034600001212046FFF740FE6B0303D502212046D1 +:10347000FFF73AFE2F0303D503212046FFF734FE62 +:10348000E80203D504212046FFF72EFEA90203D54A +:1034900005212046FFF728FE6A0203D506212046B3 +:1034A000FFF722FE2B0203D507212046FFF71CFE63 +:1034B000EF0103D508212046FFF716FE700340F107 +:1034C000A780E90703D500212046FFF79FFEAA0742 +:1034D00003D501212046FFF799FE6B0703D5022192 +:1034E0002046FFF793FE2F0703D503212046FFF761 +:1034F0008DFEEE0603D504212046FFF787FEA806C1 +:1035000003D505212046FFF781FE690603D5062174 +:103510002046FFF77BFE2A0603D507212046FFF74A +:1035200075FEEB0574D520460821BDE8F84FFFF77E +:103530006DBED4F890904FF0000B4FF0010AD4F814 +:1035400094305FFA8BF79B689F423FF638AF09EBE8 +:103550004713D3F8002902F44022B2F5802F20D17E +:10356000D3F80029002A1CDAD3F8002942F090424F +:10357000C3F80029D3F80029002AFBDB3946D4F828 +:103580009000FFF787FB22890AFA07F322EA030378 +:10359000238104EB8703DB689B6813B1394620461F +:1035A00098470BF1010BCAE7910701D1D0F80080D1 +:1035B000072A02F101029CBF03F8018B4FEA182889 +:1035C0003FE704EB830300F58050DA68D2F818C0B7 +:1035D000DCF80820DCE9001CA1EB0C0C00218F4278 +:1035E00008D1DB689B699A683A449A605A683A4401 +:1035F0005A6029E711F0030F01D1D0F800808C4503 +:1036000001F1010184BF02F8018B4FEA1828E6E7B7 +:10361000BDE8F88F08B50348FFF774FEBDE8084021 +:10362000FFF744BA9442002008B50348FFF76AFE4A +:10363000BDE80840FFF73ABA30430020D0F8903098 +:1036400003EB4111D1F8003B43F40013C1F8003BF8 +:1036500070470000D0F8903003EB4111D1F80039E9 +:1036600043F40013C1F8003970470000D0F89030DF +:1036700003EB4111D1F8003B23F40013C1F8003BE8 +:1036800070470000D0F8903003EB4111D1F80039B9 +:1036900023F40013C1F800397047000030B504333B +:1036A000039C0172002104FB0325C160C0E906539D +:1036B000049B0363059BC0E90000C0E90422C0E944 +:1036C0000842C0E90A11436330BD00000022416A8C +:1036D000C260C0E90411C0E90A226FF00101FEF7DF +:1036E000A5BC0000D0E90432934201D1C2680AB9F6 +:1036F000181D704700207047036919600021C268D7 +:103700000132C260C269134482699342036124BFDB +:10371000436A0361FEF77EBC38B504460D46E36894 +:103720003BB162690020131D1268A3621344E36277 +:1037300007E0237A33B929462046FEF75BFC0028D0 +:10374000EDDA38BD6FF00100FBE70000C368C26925 +:10375000013BC3604369134482699342436124BFC0 +:10376000436A436100238362036B03B118477047C8 +:1037700070B53023044683F31188866A3EB9FFF79B +:10378000CBFF054618B186F31188284670BDA36AA1 +:10379000E26A13F8015B9342A36202D32046FFF76B +:1037A000D5FF002383F31188EFE700002DE9F84FE0 +:1037B00004460E46174698464FF0300989F31188A3 +:1037C0000025AA46D4F828B0BBF1000F09D1414624 +:1037D0002046FFF7A1FF20B18BF311882846BDE8F2 +:1037E000F88FD4E90A12A7EB050B521A934528BFAC +:1037F0009346BBF1400F1BD9334601F1400251F80B +:10380000040B914243F8040BF9D1A36A40364035CA +:103810004033A362D4E90A239A4202D32046FFF739 +:1038200095FF8AF31188BD42D8D289F31188C9E780 +:1038300030465A46FDF708FBA36A5E445D445B448C +:10384000A362E7E710B5029C0433017203FB042175 +:10385000C460C0E906130023C0E90A33039B036375 +:10386000049BC0E90000C0E90422C0E908424363A8 +:1038700010BD0000026A6FF00101C260426AC0E937 +:1038800004220022C0E90A22FEF7D0BBD0E90423BB +:103890009A4201D1C26822B9184650F8043B0B6025 +:1038A000704700231846FAE7C3680021C269013354 +:1038B000C3604369134482699342436124BF436AEE +:1038C0004361FEF7A7BB000038B504460D46E36828 +:1038D0003BB1236900201A1DA262E2691344E3622E +:1038E00007E0237A33B929462046FEF783FB0028F8 +:1038F000EDDA38BD6FF00100FBE7000003691960E5 +:10390000C268013AC260C26913448269934203618A +:1039100024BF436A036100238362036B03B118472A +:103920007047000070B530230D460446114683F3FE +:103930001188866A2EB9FFF7C7FF10B186F3118888 +:1039400070BDA36A1D70A36AE26A01339342A36249 +:1039500004D3E16920460439FFF7D0FF002080F34B +:103960001188EDE72DE9F84F04460D46904699463B +:103970004FF0300A8AF311880026B346A76A4FB980 +:1039800049462046FFF7A0FF20B187F31188304653 +:10399000BDE8F88FD4E90A073A1AA8EB0607974260 +:1039A00028BF1746402F1BD905F1400355F8042BBB +:1039B0009D4240F8042BF9D1A36A40364033A362FC +:1039C000D4E90A239A4204D3E16920460439FFF777 +:1039D00095FF8BF311884645D9D28AF31188CDE73C +:1039E00029463A46FDF730FAA36A3D443E443B443B +:1039F000A362E5E7D0E904239A4217D1C3689BB1DB +:103A0000836A8BB1043B9B1A0ED01360C368013BE1 +:103A1000C360C3691A4483699A42026124BF436A3E +:103A20000361002383620123184670470023FBE7EC +:103A300000F01EBA014B586A704700BF000C0040EE +:103A4000034B002258631A610222DA60704700BFFC +:103A5000000C0040014B0022DA607047000C00406F +:103A6000014B5863704700BF000C0040024B034AF3 +:103A70001A60034A5A607047E443002088440020DB +:103A800000000220074B494210B55C68201A08402C +:103A90001968821A8A4203D3A24201D85A6010BD23 +:103AA0000020FCE7E443002008B5302383F31188AD +:103AB000FFF7E8FF002383F3118808BD04480121C4 +:103AC000044B03600023C0E901330C3000F0D4B88C +:103AD000EC430020A93A0008CB1D083A23F0070365 +:103AE000591A521A012110B4D2080024C0E9004327 +:103AF00084600C301C605A605DF8044B00F0BCB868 +:103B00002DE9F84F364ECD1D0F46002818BF06464A +:103B1000082A4FEAD50538BF082206F10C08341DE3 +:103B20009146404600F0C4F809F10701C9F1000EC2 +:103B3000224624686CB9404600F0C4F83368CBB321 +:103B400008224946E8009847044698B340E90267CE +:103B500030E004EB010CD4F804A00CEA0E0C0AF1DE +:103B60000100ACF1080304EBC0009842E0D9A0EBDF +:103B70000C0CB5EBEC0F4FEAEC0BD9D89C421CD2E5 +:103B800004F10802AB45A3EB02024FEAE2026260D5 +:103B900009D9691CED43206803EBC1025D445560FF +:103BA00043F8310022601C465F60404644F8086BD1 +:103BB00000F088F82046BDE8F88FAA45216802D1B8 +:103BC00011602346EFE7013504EBC50344F83510D7 +:103BD00003F10801401AC01058601360F1E700BFFC +:103BE000EC430020FEE7000070B51B4B00250446A7 +:103BF00086B058600E4685620163FEF72FFF04F120 +:103C00001003A560E562C4E904334FF0FF33C4E953 +:103C10000044C4E90635FFF70DFF2B46024604F1C8 +:103C200034012046C4E9082380230C4A2565FEF7A9 +:103C300097F901230A4AE0600092037568467268AA +:103C40000192B268CDE90223064BCDE90435FEF7B7 +:103C5000AFF906B070BD00BFA826002038470008A5 +:103C60003D470008E53B0008024AD36A1843D0628A +:103C7000704700BF40240020C0E900008160704709 +:103C80008368013B002B10B583600CDA074BDC68BE +:103C90004368A061206063601C6044600520FEF7FB +:103CA000D1F8A06910BD0020FCE700BF402400202F +:103CB00008B5302383F31188FFF7E2FF002383F375 +:103CC000118808BD08B5302383F311888368013358 +:103CD000002B836007DC036800211A6802605060D3 +:103CE0001846FEF7DBF8002383F3118808BD0000B7 +:103CF0004B6843608B688360CB68C3600B6943612A +:103D00004B6903628B6943620B6803607047000074 +:103D100008B53C4B40F2FF713B48D3F888200A437A +:103D2000C3F88820D3F8882022F4FF6222F007022B +:103D3000C3F88820D3F88820D3F8E0200A43C3F8DA +:103D4000E020D3F808210A43C3F808212F4AD3F80A +:103D500008311146FFF7CCFF00F5806002F11C012D +:103D6000FFF7C6FF00F5806002F13801FFF7C0FFE2 +:103D700000F5806002F15401FFF7BAFF00F58060A2 +:103D800002F17001FFF7B4FF00F5806002F18C01D1 +:103D9000FFF7AEFF00F5806002F1A801FFF7A8FF72 +:103DA00000F5806002F1C401FFF7A2FF00F580601A +:103DB00002F1E001FFF79CFF00F5806002F1FC01D9 +:103DC000FFF796FF02F58C7100F58060FFF790FF1A +:103DD00000F01AF90E4BD3F8902242F00102C3F81A +:103DE0009022D3F8942242F00102C3F894220522D3 +:103DF000C3F898204FF06052C3F89C20054AC3F8DE +:103E0000A02008BD004402580000025844470008A2 +:103E100000ED00E01F00080308B500F0C9FAFEF746 +:103E200079F8104BD3F8DC2042F04002C3F8DC20D4 +:103E3000D3F8042122F04002C3F80421D3F804315E +:103E4000094B1A6842F008021A601A6842F004022C +:103E50001A60FEF7DDFDFEF74BFDBDE80840FEF7FA +:103E6000D1BA00BF00440258001802487047000051 +:103E7000114BD3F8E82042F00802C3F8E820D3F849 +:103E8000102142F00802C3F810210C4AD3F8103177 +:103E9000D36B43F00803D363C722094B9A624FF0F8 +:103EA000FF32DA6200229A615A63DA605A600122B4 +:103EB0005A611A60704700BF004402580010005C4D +:103EC000000C0040094A08B51169D3680B40D9B20B +:103ED0009B076FEA0101116107D5302383F3118835 +:103EE000FEF732F8002383F3118808BD000C004070 +:103EF000064BD3F8DC200243C3F8DC20D3F80421BE +:103F00001043C3F80401D3F8043170470044025849 +:103F100008B53C4B4FF0FF31D3F8802062F00042EF +:103F2000C3F88020D3F8802002F00042C3F880203C +:103F3000D3F88020D3F88420C3F88410D3F88420E9 +:103F40000022C3F88420D3F88400D86F40F0FF40EB +:103F500040F4FF0040F4DF4040F07F00D867D86FA6 +:103F600020F0FF4020F4FF0020F4DF4020F07F002D +:103F7000D867D86FD3F888006FEA40506FEA505086 +:103F8000C3F88800D3F88800C0F30A00C3F888009B +:103F9000D3F88800D3F89000C3F89010D3F89000BD +:103FA000C3F89020D3F89000D3F89400C3F894108D +:103FB000D3F89400C3F89420D3F89400D3F8980071 +:103FC000C3F89810D3F89800C3F89820D3F8980055 +:103FD000D3F88C00C3F88C10D3F88C00C3F88C2075 +:103FE000D3F88C00D3F89C00C3F89C10D3F89C1035 +:103FF000C3F89C20D3F89C30FDF79CF9BDE808403D +:1040000000F0AEB90044025808B50122534BC3F882 +:104010000821534BD3F8F42042F00202C3F8F420F5 +:10402000D3F81C2142F00202C3F81C210222D3F86B +:104030001C314C4BDA605A689104FCD54A4A1A602C +:1040400001229A60494ADA6000221A614FF4404224 +:104050009A61444B9A699204FCD51A6842F48072C2 +:104060001A603F4B1A6F12F4407F04D04FF4803235 +:104070001A6700221A671A6842F001021A60384B68 +:104080001A685007FCD500221A611A6912F038022A +:10409000FBD1012119604FF0804159605A67344AC1 +:1040A000DA62344A1A611A6842F480321A602C4B80 +:1040B0001A689103FCD51A6842F480521A601A6893 +:1040C0009204FCD52C4A2D499A6200225A63196346 +:1040D00001F57C01DA6301F2E71199635A64284A19 +:1040E0001A64284ADA621A6842F0A8521A601C4B15 +:1040F0001A6802F02852B2F1285FF9D148229A6179 +:104100004FF48862DA6140221A621F4ADA641F4A59 +:104110001A651F4A5A651F4A9A6532231E4A136060 +:10412000136803F00F03022BFAD10D4A136943F011 +:1041300003031361136903F03803182BFAD14FF00E +:104140000050FFF7D5FE4FF08040FFF7D1FE4FF053 +:104150000040BDE80840FFF7CBBE00BF0080005123 +:10416000004402580048025800C000F0020000015C +:104170000000FF010088900812102000630209016E +:10418000470E0508DD0BBF012000002000000110D4 +:104190000910E00000010110002000524FF0B04271 +:1041A00008B5D2F8883003F00103C2F8883023B193 +:1041B000044A13680BB150689847BDE80840FEF701 +:1041C00075BC00BF044400204FF0B04208B5D2F8DF +:1041D000883003F00203C2F8883023B1044A9368A0 +:1041E0000BB1D0689847BDE80840FEF75FBC00BF40 +:1041F000044400204FF0B04208B5D2F8883003F0F4 +:104200000403C2F8883023B1044A13690BB1506922 +:104210009847BDE80840FEF749BC00BF04440020B1 +:104220004FF0B04208B5D2F8883003F00803C2F866 +:10423000883023B1044A93690BB1D0699847BDE82F +:104240000840FEF733BC00BF044400204FF0B042EA +:1042500008B5D2F8883003F01003C2F8883023B1D3 +:10426000044A136A0BB1506A9847BDE80840FEF74C +:104270001DBC00BF044400204FF0B04310B5D3F87C +:10428000884004F47872C3F88820A30604D5124A43 +:10429000936A0BB1D06A9847600604D50E4A136B37 +:1042A0000BB1506B9847210604D50B4A936B0BB1A9 +:1042B000D06B9847E20504D5074A136C0BB1506CDC +:1042C0009847A30504D5044A936C0BB1D06C98476A +:1042D000BDE81040FEF7EABB044400204FF0B043B5 +:1042E00010B5D3F8884004F47C42C3F888206205F6 +:1042F00004D5164A136D0BB1506D9847230504D5AC +:10430000124A936D0BB1D06D9847E00404D50F4A63 +:10431000136E0BB1506E9847A10404D50B4A936EEF +:104320000BB1D06E9847620404D5084A136F0BB1E5 +:10433000506F9847230404D5044A936F0BB1D06F94 +:104340009847BDE81040FEF7B1BB00BF0444002011 +:1043500008B5FFF7B7FDBDE80840FEF7A7BB0000B2 +:10436000062108B50846FDF7C1F806210720FDF72C +:10437000BDF806210820FDF7B9F806210920FDF750 +:10438000B5F806210A20FDF7B1F806211720FDF740 +:10439000ADF806212820FDF7A9F809217A20FDF7BC +:1043A000A5F807213220BDE80840FDF79FB80000BE +:1043B00008B5FFF7ADFD00F00BF8FDF769FAFDF762 +:1043C0003BF9FFF753FDBDE80840FFF731BB0000A4 +:1043D0000023054A19460133102BC2E9001102F1EE +:1043E0000802F8D1704700BF0444002010B501391D +:1043F0000244904201D1002005E0037811F8014FFA +:10440000A34201D0181B10BD0130F2E7034611F89A +:10441000012B03F8012B002AF9D1704753544D3377 +:104420003248373F3F3F0053544D3332483733789B +:104430002F3732780053544D3332483734332F37C7 +:1044400035332F373530000001105A000310590062 +:1044500001205800032056001000024008000240CE +:104460000008024000000B00280002400800024043 +:104470000408024006010C0040000240080002400F +:104480000808024010020D005800024008000240D7 +:104490000C08024016030E00700002400C0002409F +:1044A0001008024000040F00880002400C00024087 +:1044B0001408024006051000A00002400C00024053 +:1044C0001808024010061100B80002400C0002401B +:1044D0001C08024016072F00100402400804024086 +:1044E0002008024000083800280402400804024066 +:1044F0002408024006093900400402400804024032 +:1045000028080240100A3A005804024008040240F9 +:104510002C080240160B3B00700402400C040240C1 +:1045200030080240000C3C00880402400C040240A9 +:1045300034080240060D4400A00402400C0402406E +:1045400038080240100E4500B80402400C04024036 +:104550003C080240160F460000000000B515000898 +:10456000A1150008DD150008C9150008D5150008BB +:10457000C1150008AD15000899150008E9150008D7 +:104580000000000001000000000000006330000097 +:104590008C45000898240020A826002041726475EC +:1045A00050696C6F740025424F415244252D424C96 +:1045B000002553455249414C2500000002000000EF +:1045C00000000000D5170008451800084000400012 +:1045D000583F0020683F002002000000000000005B +:1045E00003000000000000008D180008000000001B +:1045F00010000000783F00200000000001000000D3 +:10460000000000009442002001010200A1230008E4 +:10461000B12200084D2300083123000843000000A8 +:104620002446000809024300020100C032090400C8 +:10463000000102020100052400100105240100010F +:10464000042402020524060001070582030800FF76 +:1046500009040100020A00000007050102400000F1 +:1046600007058102400000001200000070460008AB +:10467000120110010200004009124157000201021C +:10468000030100000403090425424F415244250060 +:1046900047455052435F54414B45525F483734338E +:1046A00000303132333435363738394142434445AE +:1046B0004600000000000020000002000200000090 +:1046C000000000300000040008000000000000248A +:1046D00000000800040000000004000000FC0000CE +:1046E000020000000000043000800000080000000C +:1046F0000000003800000100010000000000000080 +:10470000E9190008A11C00084D1D000840004000E8 +:10471000CC430020CC43002001000000DC430020FB +:1047200080000000400100000800000000010000BF +:1047300000100000080000006D61696E0069646C83 +:10474000650000000001806A00000000AAAAAAAA71 +:1047500000010064FFFF00000000000000A00A004C +:104760000505000100000000AAAAAAAA0000000195 +:10477000CCFF000000000000000000000000050465 +:1047800000000000AAAAAAAA00000000FFDC0000A6 +:104790000000000000000000100000050000000004 +:1047A000AAAAAAAA20000000FBCF00000000000077 +:1047B000000000000001000000000000AAAAAAAA50 +:1047C00000010000FFFF00000000000000000000EA +:1047D0000000000000000000AAAAAAAA0000000031 +:1047E000FFFF0000000000000000000000000000CB +:1047F00000000000AAAAAAAA00000000FFFF000013 +:1048000000000000000000000000000000000000A8 +:10481000AAAAAAAA00000000FFFF000000000000F2 +:10482000000000000000000000000000AAAAAAAAE0 +:1048300000000000FFFF000000000000000000007A +:104840000000000000000000AAAAAAAA00000000C0 +:10485000FFFF00000000000000000000000000005A +:1048600000000000AAAAAAAA00000000FFFF0000A2 +:104870000000000000000000DE0500000000000055 +:1048800000001A0000000000FF000000000000000F +:104890001C44000883040000274400085004000062 +:1048A0003544000800960000000008009600000053 +:1048B000000800000400000084460008000000001A +:1048C00000000000000000000000000000000000E8 +:0448D00000000000E4 +:00000001FF diff --git a/Tools/scripts/CAN/CAN_playback.py b/Tools/scripts/CAN/CAN_playback.py index bdd4825f4fa22..b3b8cc0ecc66f 100755 --- a/Tools/scripts/CAN/CAN_playback.py +++ b/Tools/scripts/CAN/CAN_playback.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 ''' -playback a set of CAN frames from libraries/AP_Scripting/examples/CAN_logger.lua onto a CAN bus + playback a set of CAN frames + capture frames either using libraries/AP_Scripting/examples/CAN_logger.lua + or the CAN_Pn_OPTIONS bit to enable CAN logging ''' import dronecan @@ -9,6 +11,7 @@ import threading from pymavlink import mavutil from dronecan.driver.common import CANFrame +import struct # get command line arguments @@ -16,6 +19,7 @@ parser = ArgumentParser(description='CAN playback') parser.add_argument("logfile", default=None, type=str, help="logfile") parser.add_argument("canport", default=None, type=str, help="CAN port") +parser.add_argument("--bus", default=0, type=int, help="CAN bus") args = parser.parse_args() @@ -28,8 +32,27 @@ tstart = time.time() first_tstamp = None +def dlc_to_datalength(dlc): + # Data Length Code 9 10 11 12 13 14 15 + # Number of data bytes 12 16 20 24 32 48 64 + if (dlc <= 8): + return dlc + elif (dlc == 9): + return 12 + elif (dlc == 10): + return 16 + elif (dlc == 11): + return 20 + elif (dlc == 12): + return 24 + elif (dlc == 13): + return 32 + elif (dlc == 14): + return 48 + return 64 + while True: - m = mlog.recv_match(type='CANF') + m = mlog.recv_match(type=['CANF','CAFD']) if m is None: print("Rewinding") @@ -38,15 +61,25 @@ first_tstamp = None continue + if getattr(m,'bus',0) != args.bus: + continue + if first_tstamp is None: first_tstamp = m.TimeUS dt = time.time() - tstart dt2 = (m.TimeUS - first_tstamp)*1.0e-6 if dt2 > dt: time.sleep(dt2 - dt) - data = [m.B0, m.B1, m.B2, m.B3, m.B4, m.B5, m.B6, m.B7] - data = data[:m.DLC] + + canfd = m.get_type() == 'CAFD' + if canfd: + data = struct.pack(" #include +#include + +/* + avoid a recursion issue with config defines + */ +#if AP_CAN_LOGGING_ENABLED && !HAL_LOGGING_ENABLED +#undef AP_CAN_LOGGING_ENABLED +#define AP_CAN_LOGGING_ENABLED 0 +#endif #define LOG_TAG "CANMGR" #define LOG_BUFFER_SIZE 1024 @@ -260,6 +269,10 @@ void AP_CANManager::init() _drivers[drv_num]->init(drv_num, enable_filter); } + +#if AP_CAN_LOGGING_ENABLED + hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_CANManager::check_logging_enable, void)); +#endif } #else void AP_CANManager::init() @@ -509,6 +522,7 @@ void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) frame_buffer->push(frame); break; } +#if HAL_CANFD_SUPPORTED case MAVLINK_MSG_ID_CANFD_FRAME: { mavlink_canfd_frame_t p; mavlink_msg_canfd_frame_decode(&msg, &p); @@ -523,6 +537,7 @@ void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) frame_buffer->push(frame); break; } +#endif } process_frame_buffer(); } @@ -684,12 +699,15 @@ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram } } const uint8_t data_len = AP_HAL::CANFrame::dlcToDataLength(frame.dlc); +#if HAL_CANFD_SUPPORTED if (frame.isCanFDFrame()) { if (HAVE_PAYLOAD_SPACE(can_forward.chan, CANFD_FRAME)) { mavlink_msg_canfd_frame_send(can_forward.chan, can_forward.system_id, can_forward.component_id, bus, data_len, frame.id, const_cast(frame.data)); } - } else { + } else +#endif + { if (HAVE_PAYLOAD_SPACE(can_forward.chan, CAN_FRAME)) { mavlink_msg_can_frame_send(can_forward.chan, can_forward.system_id, can_forward.component_id, bus, data_len, frame.id, const_cast(frame.data)); @@ -698,6 +716,61 @@ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram } #endif // HAL_GCS_ENABLED +#if AP_CAN_LOGGING_ENABLED +/* + handler for CAN frames for frame logging + */ +void AP_CANManager::can_logging_callback(uint8_t bus, const AP_HAL::CANFrame &frame) +{ +#if HAL_CANFD_SUPPORTED + if (frame.canfd) { + struct log_CAFD pkt { + LOG_PACKET_HEADER_INIT(LOG_CAFD_MSG), + time_us : AP_HAL::micros64(), + bus : bus, + id : frame.id, + dlc : frame.dlc + }; + memcpy(pkt.data, frame.data, frame.dlcToDataLength(frame.dlc)); + AP::logger().WriteBlock(&pkt, sizeof(pkt)); + return; + } +#endif + struct log_CANF pkt { + LOG_PACKET_HEADER_INIT(LOG_CANF_MSG), + time_us : AP_HAL::micros64(), + bus : bus, + id : frame.id, + dlc : frame.dlc + }; + memcpy(pkt.data, frame.data, frame.dlc); + AP::logger().WriteBlock(&pkt, sizeof(pkt)); +} + +/* + see if we need to enable/disable the CAN logging callback + */ +void AP_CANManager::check_logging_enable(void) +{ + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { + const bool enabled = _interfaces[i].option_is_set(CANIface_Params::Options::LOG_ALL_FRAMES); + uint8_t &logging_id = _interfaces[i].logging_id; + auto *can = hal.can[i]; + if (can == nullptr) { + continue; + } + if (enabled && logging_id == 0) { + can->register_frame_callback( + FUNCTOR_BIND_MEMBER(&AP_CANManager::can_logging_callback, void, uint8_t, const AP_HAL::CANFrame &), + logging_id); + } else if (!enabled && logging_id != 0) { + can->unregister_frame_callback(logging_id); + } + } +} + +#endif // AP_CAN_LOGGING_ENABLED + AP_CANManager& AP::can() { return *AP_CANManager::get_singleton(); diff --git a/libraries/AP_CANManager/AP_CANManager.h b/libraries/AP_CANManager/AP_CANManager.h index f820d8317482d..561c9c468054a 100644 --- a/libraries/AP_CANManager/AP_CANManager.h +++ b/libraries/AP_CANManager/AP_CANManager.h @@ -126,10 +126,23 @@ class AP_CANManager static const struct AP_Param::GroupInfo var_info[]; + enum class Options : uint32_t { + LOG_ALL_FRAMES = (1U<<0), + }; + + bool option_is_set(Options option) const { + return (_options & uint32_t(option)) != 0; + } + private: AP_Int8 _driver_number; AP_Int32 _bitrate; AP_Int32 _fdbitrate; + AP_Int32 _options; + +#if AP_CAN_LOGGING_ENABLED && HAL_LOGGING_ENABLED + uint8_t logging_id; +#endif }; //Parameter Interface for CANDrivers @@ -198,6 +211,14 @@ class AP_CANManager void process_frame_buffer(void); #endif // HAL_GCS_ENABLED + +#if AP_CAN_LOGGING_ENABLED && HAL_LOGGING_ENABLED + /* + handler for CAN frames for logging + */ + void can_logging_callback(uint8_t bus, const AP_HAL::CANFrame &frame); + void check_logging_enable(void); +#endif }; namespace AP diff --git a/libraries/AP_CANManager/AP_CANManager_config.h b/libraries/AP_CANManager/AP_CANManager_config.h index c8b9f8af91cef..383abe0041dc9 100644 --- a/libraries/AP_CANManager/AP_CANManager_config.h +++ b/libraries/AP_CANManager/AP_CANManager_config.h @@ -5,3 +5,8 @@ #ifndef AP_CAN_SLCAN_ENABLED #define AP_CAN_SLCAN_ENABLED HAL_MAX_CAN_PROTOCOL_DRIVERS #endif + +#ifndef AP_CAN_LOGGING_ENABLED +#define AP_CAN_LOGGING_ENABLED HAL_MAX_CAN_PROTOCOL_DRIVERS +#endif + diff --git a/libraries/AP_CANManager/LogStructure.h b/libraries/AP_CANManager/LogStructure.h new file mode 100644 index 0000000000000..3f2e2c33ccbef --- /dev/null +++ b/libraries/AP_CANManager/LogStructure.h @@ -0,0 +1,79 @@ +#pragma once +/* + log structures for AP_CANManager + */ + +#include +#include "AP_CANManager_config.h" + +#define LOG_IDS_FROM_CANMANAGER \ + LOG_CANF_MSG, \ + LOG_CAFD_MSG + +// @LoggerMessage: CANF +// @Description: CAN Frame +// @Field: TimeUS: Time since system startup +// @Field: Bus: bus number +// @Field: Id: frame identifier +// @Field: DLC: data length code +// @Field: B0: byte 0 +// @Field: B1: byte 1 +// @Field: B2: byte 2 +// @Field: B3: byte 3 +// @Field: B4: byte 4 +// @Field: B5: byte 5 +// @Field: B6: byte 6 +// @Field: B7: byte 7 +struct PACKED log_CANF { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t bus; + uint32_t id; + uint8_t dlc; + uint8_t data[8]; +}; + +// @LoggerMessage: CAFD +// @Description: CANFD Frame +// @Field: TimeUS: Time since system startup +// @Field: Bus: bus number +// @Field: Id: frame identifier +// @Field: DLC: data length code +// @Field: D0: data 0 +// @Field: D1: data 1 +// @Field: D2: data 2 +// @Field: D3: data 3 +// @Field: D4: data 4 +// @Field: D5: data 5 +// @Field: D6: data 6 +// @Field: D7: data 7 +struct PACKED log_CAFD { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t bus; + uint32_t id; + uint8_t dlc; + uint64_t data[8]; +}; + +#if !AP_CAN_LOGGING_ENABLED +#define LOG_STRUCTURE_FROM_CANMANAGER +#else +#define LOG_STRUCTURE_FROM_CANMANAGER \ + { LOG_CANF_MSG, sizeof(log_CANF), \ + "CANF", \ + "Q" "B" "I" "B" "B" "B" "B" "B" "B" "B" "B" "B", \ + "TimeUS," "Bus," "Id," "DLC," "B0," "B1," "B2," "B3," "B4," "B5," "B6," "B7", \ + "s" "#" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-", \ + "F" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-", \ + false \ + }, \ + { LOG_CAFD_MSG, sizeof(log_CAFD), \ + "CAFD", \ + "Q" "B" "I" "B" "Q" "Q" "Q" "Q" "Q" "Q" "Q" "Q", \ + "TimeUS," "Bus," "Id," "DLC," "D0," "D1," "D2," "D3," "D4," "D5," "D6," "D7", \ + "s" "#" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-", \ + "F" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-", \ + false \ + }, +#endif // AP_CAN_LOGGING_ENABLED diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/libraries/AP_Camera/AP_Camera_Backend.cpp index c632bf71ab9f2..6954c8b9e2cf2 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.cpp +++ b/libraries/AP_Camera/AP_Camera_Backend.cpp @@ -295,32 +295,46 @@ void AP_Camera_Backend::send_camera_settings(mavlink_channel_t chan) const void AP_Camera_Backend::send_camera_fov_status(mavlink_channel_t chan) const { // getting corresponding mount instance for camera - const AP_Mount* mount = AP::mount(); + AP_Mount* mount = AP::mount(); if (mount == nullptr) { return; } + + // get latest POI from mount Quaternion quat; - Location loc; + Location camera_loc; Location poi_loc; - if (!mount->get_poi(get_mount_instance(), quat, loc, poi_loc)) { - return; + const bool have_poi_loc = mount->get_poi(get_mount_instance(), quat, camera_loc, poi_loc); + + // if failed to get POI, get camera location directly from AHRS + // and attitude directly from mount + bool have_camera_loc = have_poi_loc; + if (!have_camera_loc) { + have_camera_loc = AP::ahrs().get_location(camera_loc); + mount->get_attitude_quaternion(get_mount_instance(), quat); } + + // calculate attitude quaternion in earth frame using AHRS yaw + Quaternion quat_ef; + quat_ef.from_euler(0, 0, AP::ahrs().get_yaw()); + quat_ef *= quat; + // send camera fov status message only if the last calculated values aren't stale const float quat_array[4] = { - quat.q1, - quat.q2, - quat.q3, - quat.q4 + quat_ef.q1, + quat_ef.q2, + quat_ef.q3, + quat_ef.q4 }; mavlink_msg_camera_fov_status_send( chan, AP_HAL::millis(), - loc.lat, - loc.lng, - loc.alt * 10, - poi_loc.lat, - poi_loc.lng, - poi_loc.alt * 10, + have_camera_loc ? camera_loc.lat : INT32_MAX, + have_camera_loc ? camera_loc.lng : INT32_MAX, + have_camera_loc ? camera_loc.alt * 10 : INT32_MAX, + have_poi_loc ? poi_loc.lat : INT32_MAX, + have_poi_loc ? poi_loc.lng : INT32_MAX, + have_poi_loc ? poi_loc.alt * 10 : INT32_MAX, quat_array, horizontal_fov() > 0 ? horizontal_fov() : NaNf, vertical_fov() > 0 ? vertical_fov() : NaNf diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h index 022bce3d39dc6..3a68a349d5a3c 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -161,7 +161,7 @@ #ifndef AP_DDS_DEFAULT_UDP_IP_ADDR #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS -#define AP_DDS_DEFAULT_UDP_IP_ADDR "192.168.13.2" +#define AP_DDS_DEFAULT_UDP_IP_ADDR "192.168.144.2" #else #define AP_DDS_DEFAULT_UDP_IP_ADDR "127.0.0.1" #endif diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h index b92274a6a5d51..629432bd1fac1 100644 --- a/libraries/AP_HAL/CANIface.h +++ b/libraries/AP_HAL/CANIface.h @@ -273,8 +273,8 @@ class AP_HAL::CANIface #ifndef HAL_BOOTLOADER_BUILD HAL_Semaphore sem; #endif - // allow up to 2 callbacks per interface - FrameCb cb[2]; + // allow up to 3 callbacks per interface + FrameCb cb[3]; } callbacks; uint32_t bitrate_; diff --git a/libraries/AP_HAL/DSP.cpp b/libraries/AP_HAL/DSP.cpp index 10b33e38d07ef..932149d9726c7 100644 --- a/libraries/AP_HAL/DSP.cpp +++ b/libraries/AP_HAL/DSP.cpp @@ -286,6 +286,11 @@ float DSP::calculate_jains_estimator(const FFTWindowState* fft, const float* rea float y1 = real_fft[k_max-1]; float y2 = real_fft[k_max]; float y3 = real_fft[k_max+1]; + + if (is_zero(y2) || is_zero(y1)) { + return 0.0f; + } + float d = 0.0f; if (y1 > y3) { diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 5d4274ac706a4..96fb4e1b8870f 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -1329,9 +1329,6 @@ bool RCOutput::get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len) */ void RCOutput::cork(void) { - if (corked) { - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - } corked = true; #if HAL_WITH_IO_MCU if (iomcu_enabled) { diff --git a/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/README.md b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/README.md new file mode 100644 index 0000000000000..171554ad32dd2 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/README.md @@ -0,0 +1,99 @@ +# GEPRC TAKER H743 BT Flight Controller + +The TAKER H743 BT is a flight controller produced by [GEPRC](https://geprc.com/). + +## Features + + - STM32H743 microcontroller + - MPU6000+ICM42688 dual IMU + - SPL06-001 barometer + - microSD based 512MB flash logging + - AT7456E OSD + - 7 UARTs + - 8 PWM outputs + +## Pinout + +![TAKER H743 BT Board](TAKER_H743_BT_Board_Top.jpg "GEPRC_TAKER_H743") +![TAKER H743 BT Board](TAKER_H743_BT_Board_Bottom.jpg "GEPRC_TAKER_H743") + +## UART Mapping + +The UARTs are marked Rn and Tn in the above pinouts. The Rn pin is the +receive pin for UARTn. The Tn pin is the transmit pin for UARTn. + + - SERIAL0 -> USB + - SERIAL1 -> UART1 (DisplayPort, DMA-enabled) + - SERIAL2 -> UART2 (RCIN, DMA-enabled) + - SERIAL3 -> UART3 (connected to internal BT module, not currently usable by ArduPilot) + - SERIAL4 -> UART4 (GPS) + - SERIAL6 -> UART6 (User) + - SERIAL7 -> UART7 (User) + - SERIAL8 -> UART8 (ESC Telemetry) + +## RC Input + +RC input is configured by default via the USAR2 RX input. It supports all unidirectional RC protocols except PPM. FPort and full duplex protocols, like CRSF/ELRS, will need to use TX2 also. + +Note: +If the receiver is FPort or a full duplex protocol, then the receiver must be tied to the USART2 TX pin and [SERIAL2_OPTIONS](https://ardupilot.org/copter/docs/parameters.html#serial2-options) = 7 (invert TX/RX, half duplex), and [RSSI_TYPE](https://ardupilot.org/copter/docs/parameters.html#rssi-type) =3. + +## FrSky Telemetry + +FrSky Telemetry is supported using the Tx pin of any UART excluding SERIAL3UART3. To enable support for FrSky S.PORT (the example shows SERIAL6), you need to set the following parameters. + + - SERIAL6_PROTOCOL 10 + - SERIAL6_OPTIONS 7 + +## OSD Support + +The TAKER H743 BT supports analog OSD using its internal OSD chip and simultaneously HD goggle DisplayPort OSDs via the HD VTX connector. + +## VTX Support + +The SH1.0-6P connector supports a standard DJI HD VTX connection. Pin 1 of the connector is 12v (or VBAT by solder pad selection) so be careful not to connect to devices expecting 5v. + +## PWM Output + +The TAKER H743 BT supports up to 9 PWM/DShot outputs. The pads for motor output +M1 to M4 are on the esc connector, M5-M8 are solder pads, plus M9 is defaulted for serial LED strip or can be used as another PWM output. + +The PWM is in 4 groups: + + - PWM 1-4 in group1 + - PWM 5-6 in group2 + - PWM 7-8 in group3 + - PWM 9 in group4 + +Channels within the same group need to use the same output rate. If +any channel in a group uses DShot then all channels in the group need +to use DShot. Channels 1-8 support bi-directional DShot. + +## Battery Monitoring + +The board has a internal voltage sensor and connections on the ESC connector for an external current sensor input. +The voltage sensor can handle up to 6S. +LiPo batteries. + +The default battery parameters are: + + - BATT_MONITOR 4 + - BATT_VOLT_PIN 13 + - BATT_VOLT_SCALE 11.1 + - BATT_CURR_PIN 12 + - BATT_CURR_SCALE 28.5 + +## Compass + +The TAKER H743 BT does not have a builtin compass, but you can attach an external compass using I2C on the SDA and SCL pads. + +## Loading Firmware + +Initial firmware load can be done with DFU by plugging in USB with the +bootloader button pressed. Then you should load the "with_bl.hex" +firmware, using your favourite DFU loading 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/GEPRC_TAKER_H743/TAKER_H743_BT_Board_Bottom.jpg b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/TAKER_H743_BT_Board_Bottom.jpg new file mode 100644 index 0000000000000..e25a71a2bd56c Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/TAKER_H743_BT_Board_Bottom.jpg differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/TAKER_H743_BT_Board_Top.jpg b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/TAKER_H743_BT_Board_Top.jpg new file mode 100644 index 0000000000000..6c37cf13a78da Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/TAKER_H743_BT_Board_Top.jpg differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/defaults.parm new file mode 100644 index 0000000000000..6455bd215b277 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/defaults.parm @@ -0,0 +1,4 @@ +# setup for LEDs on chan9 +SERVO9_FUNCTION 120 + +OSD_TYPE2 5 \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/hwdef-bl.dat new file mode 100644 index 0000000000000..8987e979a8cf5 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/hwdef-bl.dat @@ -0,0 +1,54 @@ +# hw definition file for processing by chibios_pins.py +# for GEPRFH743-BT-HD bootloader + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# board ID for firmware load +APJ_BOARD_ID AP_HW_GEPRC_TAKER_H743 + +# crystal frequency, setup to use external oscillator +OSCILLATOR_HZ 8000000 + +define STM32_LSECLK 32768U +define STM32_LSEDRV (3U << 3U) + +FLASH_SIZE_KB 2048 + +# bootloader starts at zero offset +FLASH_RESERVE_START_KB 0 + +# the location where the bootloader will put the firmware +FLASH_BOOTLOADER_LOAD_KB 384 + + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 + +# PA10 IO-debug-console +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +PD2 BUZZER OUTPUT LOW PULLDOWN + +PC13 LED_BOOTLOADER OUTPUT LOW +define HAL_LED_ON 0 + +# Motors for esc init +PB0 PWMOUT1 OUTPUT LOW +PB1 PWMOUT2 OUTPUT LOW +PB5 PWMOUT3 OUTPUT LOW +PB4 PWMOUT4 OUTPUT LOW +PD12 PWMOUT5 OUTPUT LOW +PD13 PWMOUT6 OUTPUT LOW +PC8 PWMOUT7 OUTPUT LOW +PC9 PWMOUT8 OUTPUT LOW + +# Add CS pins to ensure they are high in bootloader +PA15 SDCARD_CS CS +PE4 MAX7456_CS CS +PA4 MPU6000_CS CS +PB12 ICM42605_CS CS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/hwdef.dat new file mode 100644 index 0000000000000..2ddc1245395eb --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/hwdef.dat @@ -0,0 +1,182 @@ +# hw definition file for processing by chibios_pins.py +# for TAKER H743 BT hardware. +# thanks to betaflight for pin information + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# board ID for firmware load +APJ_BOARD_ID AP_HW_GEPRC_TAKER_H743 + +# crystal frequency, setup to use external oscillator +OSCILLATOR_HZ 8000000 +MCU_CLOCKRATE_MHZ 480 + +FLASH_SIZE_KB 2048 + +# leave 2 sectors free +FLASH_RESERVE_START_KB 384 + + +# only one I2C bus +I2C_ORDER I2C1 + +# I2C1 for baro +PB8 I2C1_SCL I2C1 +PB9 I2C1_SDA I2C1 + +# order of UARTs (and USB), +SERIAL_ORDER OTG1 USART1 USART2 USART3 UART4 EMPTY USART6 UART7 UART8 + +# buzzer +#define HAL_BUZZER_PIN 80 + +# PA10 IO-debug-console +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# SPI1 for MPU6000 +PA4 MPU6000_CS CS +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 + +# SPI2 for ICM42688 +PB12 ICM42605_CS CS +PB13 SPI2_SCK SPI2 +PB14 SPI2_MISO SPI2 +PB15 SPI2_MOSI SPI2 + +# SPI3 for SDCard +PA15 SDCARD_CS CS +PC10 SPI3_SCK SPI3 +PC11 SPI3_MISO SPI3 +PC12 SPI3_MOSI SPI3 + +# SPI4 for MAX7456 OSD +PE4 MAX7456_CS CS +PE2 SPI4_SCK SPI4 +PE5 SPI4_MISO SPI4 +PE6 SPI4_MOSI SPI4 + +PC3 BATT_VOLTAGE_SENS ADC1 SCALE(1) +PC2 BATT_CURRENT_SENS ADC1 SCALE(1) + +# define default battery setup +define HAL_BATT_MONITOR_DEFAULT 4 +define HAL_BATT_VOLT_PIN 13 +define HAL_BATT_CURR_PIN 12 +define HAL_BATT_VOLT_SCALE 11.13 +define HAL_BATT_CURR_SCALE 28.5 + +PC13 LED0 OUTPUT LOW GPIO(90) # LED +define AP_NOTIFY_GPIO_LED_1_ENABLED 1 +define AP_NOTIFY_GPIO_LED_1_PIN 90 + +# In order to accommodate bi-directional dshot certain devices cannot be DMA enabled +# NODMA indicates these devices, if you remove it they will still not be resolved for DMA + +# USART1 +PA10 USART1_RX USART1 +PA9 USART1_TX USART1 +define DEFAULT_SERIAL1_PROTOCOL SerialProtocol_MSP_DisplayPort + +# USART2 +# RC input defaults to UART to allow for bi-dir dshot +PA2 USART2_TX USART2 +PA3 USART2_RX USART2 + +# USART3 (BT) +PB11 USART3_RX USART3 NODMA +PB10 USART3_TX USART3 NODMA +define DEFAULT_SERIAL3_PROTOCOL SerialProtocol_None + +# UART4 (GPS) +PA0 UART4_TX UART4 +PA1 UART4_RX UART4 + +# UART6 +PC6 USART6_TX USART6 +PC7 USART6_RX USART6 +define DEFAULT_SERIAL6_PROTOCOL SerialProtocol_None + +# UART7 +PE7 UART7_RX UART7 NODMA +PE8 UART7_TX UART7 NODMA +define DEFAULT_SERIAL7_PROTOCOL SerialProtocol_None + +# UART8 +PE0 UART8_RX UART8 NODMA +PE1 UART8_TX UART8 NODMA +define DEFAULT_SERIAL8_PROTOCOL SerialProtocol_ESCTelemetry + +# Motors, bi-directional dshot capable +PB0 TIM3_CH3 TIM3 PWM(1) GPIO(50) BIDIR # M1 +PB1 TIM3_CH4 TIM3 PWM(2) GPIO(51) # M2 +PB5 TIM3_CH2 TIM3 PWM(3) GPIO(52) # M3 +PB4 TIM3_CH1 TIM3 PWM(4) GPIO(53) BIDIR # M4 +PD12 TIM4_CH1 TIM4 PWM(5) GPIO(54) BIDIR # M5 +PD13 TIM4_CH2 TIM4 PWM(6) GPIO(55) # M6 +PC8 TIM8_CH3 TIM8 PWM(7) GPIO(56) BIDIR # M7 +PC9 TIM8_CH4 TIM8 PWM(8) GPIO(57) # M8 + +# extra PWM outs +PA8 TIM1_CH1 TIM1 PWM(9) GPIO(58) # led pin +PD2 BUZZER OUTPUT GPIO(80) LOW +define HAL_BUZZER_PIN 80 +define HAL_BUZZER_ON 1 +define HAL_BUZZER_OFF 0 + +DMA_PRIORITY TIM3* TIM4* USART2* +DMA_NOSHARE SPI1* SPI2* + +define HAL_STORAGE_SIZE 16384 +STORAGE_FLASH_PAGE 1 + +# spi devices +SPIDEV mpu6000 SPI1 DEVID1 MPU6000_CS MODE3 1*MHZ 8*MHZ +SPIDEV icm42688 SPI2 DEVID1 ICM42605_CS MODE3 2*MHZ 16*MHZ +SPIDEV sdcard SPI3 DEVID1 SDCARD_CS MODE0 400*KHZ 25*MHZ +SPIDEV osd SPI4 DEVID4 MAX7456_CS MODE0 10*MHZ 10*MHZ + +# IMU setup +IMU Invensensev3 SPI:icm42688 ROTATION_ROLL_180 +IMU Invensense SPI:mpu6000 ROTATION_YAW_90 +define HAL_DEFAULT_INS_FAST_SAMPLE 3 + +# no built-in compass, but probe the i2c bus for all possible +# external compass types +define ALLOW_ARM_NO_COMPASS +define HAL_PROBE_EXTERNAL_I2C_COMPASSES +define HAL_I2C_INTERNAL_MASK 0 +define HAL_COMPASS_AUTO_ROT_DEFAULT 2 + +# one BARO +#BARO BMP280 I2C:0:0x76 +#BARO DPS310 I2C:0:0x76 +BARO SPL06 I2C:0:0x76 + +define HAL_OS_FATFS_IO 1 +define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS" +define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN" + +# setup for OSD +define OSD_ENABLED 1 +define HAL_OSD_TYPE_DEFAULT 1 +ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin + +#only enable BMP280 Baro +#define AP_BARO_BACKEND_DEFAULT_ENABLED 0 +#undef define AP_BARO_BMP280_ENABLED +#define AP_BARO_BMP280_ENABLED 1 +define AP_BARO_SPL06_ENABLED 1 +define AP_BARO_MS56XX_ENABLED 1 + + + +# need to probe external baros even 'though we're minimised to allow custom build options: +undef AP_BARO_PROBE_EXTERNAL_I2C_BUSES +define AP_BARO_PROBE_EXTERNAL_I2C_BUSES 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat index 895e3703bc86b..9dd7533f6c2a1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat @@ -74,9 +74,9 @@ define HAL_I2C_INTERNAL_MASK 3 I2C_ORDER I2C1 I2C2 # one SPI bus (for IMU, unused) -#PA5 SPI1_SCK SPI1 -#PA6 SPI1_MISO SPI1 -#PA7 SPI1_MOSI SPI1 +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 # SPI CS PC4 GYR_CS CS @@ -95,6 +95,10 @@ COMPASS IST8310 I2C:0:0x0E false ROTATION_NONE # compass RM3100 on can-F9P-v2 COMPASS RM3100 I2C:0:0x20 false ROTATION_NONE +# baro, disabled by default +BARO ICP201XX I2C:0:0x64 +define AP_PERIPH_BARO_ENABLE_DEFAULT 0 + define HAL_USE_ADC FALSE define STM32_ADC_USE_ADC1 FALSE define HAL_DISABLE_ADC_DRIVER TRUE @@ -130,9 +134,17 @@ DMA_NOSHARE USART3* # add support for moving baseline yaw define GPS_MOVING_BASELINE 1 +SPIDEV icm42688 SPI1 DEVID1 ICM_CS MODE0 24*MHZ 24*MHZ + +IMU Invensensev3 SPI:icm42688 ROTATION_YAW_180 + +define HAL_PERIPH_ENABLE_IMU + # GPS+MAG+LEDs define HAL_PERIPH_ENABLE_GPS define HAL_PERIPH_ENABLE_MAG +define HAL_PERIPH_ENABLE_BARO +define HAL_PERIPH_ENABLE_IMU define HAL_PERIPH_ENABLE_NOTIFY define HAL_PERIPH_ENABLE_RC_OUT diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7/hwdef.dat index 5ce9b33c622d0..adddcffdf6505 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7/hwdef.dat @@ -130,6 +130,7 @@ DMA_NOSHARE SPI1* # spi devices SPIDEV mpu6000 SPI4 DEVID1 mpu6000_CS MODE3 1*MHZ 4*MHZ SPIDEV icm42688 SPI4 DEVID1 mpu6000_CS MODE3 1*MHZ 16*MHZ +SPIDEV bmi270 SPI4 DEVID1 mpu6000_CS MODE3 1*MHZ 10*MHZ # Clock is 100Mhz so highest clock <= 10Mhz is 100Mhz/16 SPIDEV sdcard SPI1 DEVID1 SDCARD_CS MODE0 400*KHZ 25*MHZ SPIDEV osd SPI2 DEVID4 MAX7456_CS MODE0 10*MHZ 10*MHZ @@ -147,6 +148,7 @@ define HAL_COMPASS_AUTO_ROT_DEFAULT 2 # one IMU IMU Invensense SPI:mpu6000 ROTATION_YAW_180 IMU Invensensev3 SPI:icm42688 ROTATION_ROLL_180_YAW_270 +IMU BMI270 SPI:bmi270 ROTATION_ROLL_180_YAW_270 # one BARO BARO BMP280 I2C:0:0x76 diff --git a/libraries/AP_HAL_ESP32/README.md b/libraries/AP_HAL_ESP32/README.md index cfd829fcde766..9874574a57a20 100644 --- a/libraries/AP_HAL_ESP32/README.md +++ b/libraries/AP_HAL_ESP32/README.md @@ -68,11 +68,11 @@ https://docs.espressif.com/projects/esp-idf/en/latest/get-started/ - in expansion of macro 'configSUPPORT_STATIC_ALLOCATION' warning: "CONFIG_SUPPORT_STATIC_ALLOCATION" is not defined -this means your 'sdkconfig' file that the IDF relies on is perhaps a bit out of date or out of sync with your IDF. +this means your 'sdkconfig' file that the IDF relies on is perhaps a bit out of date or out of sync with your IDF. This should not happen, please file a bug if it does! -You can simply remove sdkconfig file and idf build system will recreate it using sdkconfig.defaults, which should fix the problem. +Changing the sdkconfig.defaults file will cause the sdkconfig to be deleted and regenerated. The sdkconfig will also be regenerated if it is manually deleted. -If you need to change sdkconfig, you can edit sdkconfig manually or to use ninja menuconfig: +If you need to change sdkconfig (which will not cause it to be deleted), you can edit sdkconfig manually or to use ninja menuconfig: So double check you are using the correct IDF version: ``` @@ -86,7 +86,7 @@ press [tab] then enter on the [exit] box to exit the app done. the 'sdkconfig' file in this folder should have been updated cd ../../../.. -If you want to make changes to sdkconfig (sdkconfig is in git ignore list) permanent and to commit them back in git, you should edit sdkconfig.defaults manually or to use ninja save-defconfig tool after menuconfig. +If you want to make changes to sdkconfig (sdkconfig is in the build dir) permanent and to commit them back in git, you should edit sdkconfig.defaults manually or use ninja save-defconfig tool after menuconfig and replace sdkconfig.defaults with defconfig. 5. Recommanded way to flash the firmware : ``` diff --git a/libraries/AP_HAL_ESP32/targets/esp32/.gitignore b/libraries/AP_HAL_ESP32/targets/esp32/.gitignore deleted file mode 100644 index ec58be23462c0..0000000000000 --- a/libraries/AP_HAL_ESP32/targets/esp32/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -/sdkconfig.old -/board.txt diff --git a/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/CMakeLists.txt b/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/CMakeLists.txt index 10cca76197529..32b77dd94fb90 100644 --- a/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/CMakeLists.txt +++ b/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/CMakeLists.txt @@ -28,8 +28,9 @@ idf_build_process(esp32 esp_system esp_rom esp_timer - - SDKCONFIG ${CMAKE_CURRENT_LIST_DIR}/sdkconfig + + # treat sdkconfig as build product generated by the defaults + SDKCONFIG ${CMAKE_BINARY_DIR}/sdkconfig SDKCONFIG_DEFAULTS ${CMAKE_CURRENT_LIST_DIR}/sdkconfig.defaults BUILD_DIR ${CMAKE_BINARY_DIR}) diff --git a/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig.defaults b/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig.defaults index b94fe2d026695..9027170b1c7e3 100644 --- a/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig.defaults +++ b/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig.defaults @@ -10,6 +10,7 @@ CONFIG_APP_RETRIEVE_LEN_ELF_SHA=16 CONFIG_ESPTOOLPY_FLASHMODE_QIO=y CONFIG_ESPTOOLPY_FLASHFREQ_80M=y CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y +CONFIG_ESP_DEFAULT_CPU_FREQ_MHZ_240=y CONFIG_PARTITION_TABLE_CUSTOM=y CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="../partitions.csv" CONFIG_COMPILER_OPTIMIZATION_PERF=y diff --git a/libraries/AP_HAL_ESP32/targets/esp32s3/.gitignore b/libraries/AP_HAL_ESP32/targets/esp32s3/.gitignore deleted file mode 100644 index ec58be23462c0..0000000000000 --- a/libraries/AP_HAL_ESP32/targets/esp32s3/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -/sdkconfig.old -/board.txt diff --git a/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/CMakeLists.txt b/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/CMakeLists.txt index c00f753174fea..8a63d422c509f 100644 --- a/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/CMakeLists.txt +++ b/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/CMakeLists.txt @@ -29,7 +29,8 @@ idf_build_process(esp32s3 esp_rom esp_timer - SDKCONFIG ${CMAKE_CURRENT_LIST_DIR}/sdkconfig + # treat sdkconfig as build product generated by the defaults + SDKCONFIG ${CMAKE_BINARY_DIR}/sdkconfig SDKCONFIG_DEFAULTS ${CMAKE_CURRENT_LIST_DIR}/sdkconfig.defaults BUILD_DIR ${CMAKE_BINARY_DIR} ) diff --git a/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig.defaults b/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig.defaults index 22da63b62b92a..c346336842fc5 100644 --- a/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig.defaults +++ b/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig.defaults @@ -20,6 +20,7 @@ CONFIG_ESP32S3_UNIVERSAL_MAC_ADDRESSES_TWO=y CONFIG_ESP_SLEEP_FLASH_LEAKAGE_WORKAROUND=n CONFIG_ESP_SLEEP_GPIO_RESET_WORKAROUND=n CONFIG_ESP32S3_INSTRUCTION_CACHE_32KB=y +CONFIG_ESP_DEFAULT_CPU_FREQ_MHZ_240=y CONFIG_ESP_INT_WDT=n CONFIG_ESP_TASK_WDT_INIT=n CONFIG_ESP_IPC_TASK_STACK_SIZE=1536 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 30d06b02b16c1..300125068d169 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -55,6 +55,15 @@ void AP_InertialSensor_Backend::_set_gyro_oversampling(uint8_t instance, uint8_t _imu._gyro_over_sampling[instance] = n; } +/* + while sensors are converging to get the true sample rate we re-init the notch filters. + stop doing this if the user arms + */ +bool AP_InertialSensor_Backend::sensors_converging() const +{ + return AP_HAL::millis64() < HAL_INS_CONVERGANCE_MS && !hal.util->get_soft_armed(); +} + /* update the sensor rate for FIFO sensors diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 7ab5b420d817e..38e0a66b8b0e0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -233,7 +233,7 @@ class AP_InertialSensor_Backend void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const __RAMFUNC__; // return true if the sensors are still converging and sampling rates could change significantly - bool sensors_converging() const { return AP_HAL::millis() < HAL_INS_CONVERGANCE_MS; } + bool sensors_converging() const; // set accelerometer max absolute offset for calibration void _set_accel_max_abs_offset(uint8_t instance, float offset); diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 418b8de1c8492..3d388f9795f50 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -135,6 +135,7 @@ const struct MultiplierStructure log_Multipliers[] = { #include #include #include +#include #include #include #include @@ -1159,6 +1160,7 @@ LOG_STRUCTURE_FROM_GPS \ { LOG_RSSI_MSG, sizeof(log_RSSI), \ "RSSI", "Qff", "TimeUS,RXRSSI,RXLQ", "s-%", "F--", true }, \ LOG_STRUCTURE_FROM_BARO \ +LOG_STRUCTURE_FROM_CANMANAGER \ LOG_STRUCTURE_FROM_PRECLAND \ { LOG_POWR_MSG, sizeof(log_POWR), \ "POWR","QffHHB","TimeUS,Vcc,VServo,Flags,AccFlags,Safety", "svv---", "F00---", true }, \ @@ -1266,6 +1268,7 @@ enum LogMessages : uint8_t { LOG_RCOUT_MSG, LOG_RSSI_MSG, LOG_IDS_FROM_BARO, + LOG_IDS_FROM_CANMANAGER, LOG_POWR_MSG, LOG_MCU_MSG, LOG_IDS_FROM_AHRS, diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 8dc31bbfa33de..0b64b1063bb0e 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -655,18 +655,25 @@ bool AP_Mount::get_poi(uint8_t instance, Quaternion &quat, Location &loc, Locati } #endif -// get mount's current attitude in euler angles in degrees. yaw angle is in body-frame -// returns true on success -bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg) +// get attitude as a quaternion. returns true on success. +// att_quat will be an earth-frame quaternion rotated such that +// yaw is in body-frame. +bool AP_Mount::get_attitude_quaternion(uint8_t instance, Quaternion& att_quat) { auto *backend = get_instance(instance); if (backend == nullptr) { return false; } + return backend->get_attitude_quaternion(att_quat); +} +// get mount's current attitude in euler angles in degrees. yaw angle is in body-frame +// returns true on success +bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg) +{ // re-use get_attitude_quaternion and convert to Euler angles Quaternion att_quat; - if (!backend->get_attitude_quaternion(att_quat)) { + if (!get_attitude_quaternion(instance, att_quat)) { return false; } diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 54762e915307f..d34f581636893 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -206,6 +206,11 @@ class AP_Mount bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc) const; #endif + // get attitude as a quaternion. returns true on success. + // att_quat will be an earth-frame quaternion rotated such that + // yaw is in body-frame. + bool get_attitude_quaternion(uint8_t instance, Quaternion& att_quat); + // get mount's current attitude in euler angles in degrees. yaw angle is in body-frame // returns true on success bool get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg); diff --git a/libraries/AP_Networking/AP_Networking_Config.h b/libraries/AP_Networking/AP_Networking_Config.h index a29aed082c40a..342257972f8d7 100644 --- a/libraries/AP_Networking/AP_Networking_Config.h +++ b/libraries/AP_Networking/AP_Networking_Config.h @@ -75,9 +75,9 @@ #define AP_NETWORKING_DEFAULT_DHCP_ENABLE AP_NETWORKING_DHCP_AVAILABLE #endif -// Default Static IP Address: 192.168.13.14 +// Default Static IP Address: 192.168.144.14 #ifndef AP_NETWORKING_DEFAULT_STATIC_IP_ADDR -#define AP_NETWORKING_DEFAULT_STATIC_IP_ADDR "192.168.13.14" +#define AP_NETWORKING_DEFAULT_STATIC_IP_ADDR "192.168.144.14" #endif // Default Netmask: 24 @@ -88,9 +88,9 @@ #endif -// Default Static IP Address: 192.168.13.1 +// Default Static IP Address: 192.168.144.1 #ifndef AP_NETWORKING_DEFAULT_STATIC_GW_ADDR -#define AP_NETWORKING_DEFAULT_STATIC_GW_ADDR "192.168.13.1" +#define AP_NETWORKING_DEFAULT_STATIC_GW_ADDR "192.168.144.1" #endif // Default MAC Address: C2:AF:51:03:CF:46 @@ -109,7 +109,7 @@ #if AP_NETWORKING_TESTS_ENABLED #ifndef AP_NETWORKING_TEST_IP -#define AP_NETWORKING_TEST_IP "192.168.13.2" +#define AP_NETWORKING_TEST_IP "192.168.144.2" #endif #endif diff --git a/libraries/AP_Scripting/applets/UniversalAutoLand.lua b/libraries/AP_Scripting/applets/UniversalAutoLand.lua new file mode 100644 index 0000000000000..282a644e5df05 --- /dev/null +++ b/libraries/AP_Scripting/applets/UniversalAutoLand.lua @@ -0,0 +1,178 @@ +--[[ Upon Arming , creates a four item mission consisting of: NAV_TAKEOFF, DO_LAND_START,Final Approach WP opposite bearing from HOME of heading used during takeoff, at TKOFF_ALT or SCR_USER3 above home, SCR_USER2 or 2X TKOFF_DIST, and a LAND waypoint at HOME and stops until next disarm/boot. SCR_USER1 is used to enable or disable it. +--]] +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +local PARAM_TABLE_KEY = 80 +local PARAM_TABLE_PREFIX = "AUTOLAND_" +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 4), 'could not add param table') + +-- add a parameter and bind it to a variable +function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return Parameter(PARAM_TABLE_PREFIX .. name) +end + +--[[ + // @Param: AUTOLAND_ENABLE + // @DisplayName: AUTOLAND ENABLE + // @Description: enable AUTOLAND script action + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local AULND_ENABLE = bind_add_param('ENABLE', 1, 1) +local enable = AULND_ENABLE:get() + +--[[ + // @Param: AUTOLAND_WP_ALT + // @DisplayName: Final approach waypoint alt + // @Description: Altitude of final approach waypoint created by script + // @Range: 1 100 + // @Units: m + // @User: Standard +--]] +local AULND_ALT = bind_add_param('WP_ALT', 2, 0) +local final_wp_alt = AULND_ALT:get() +--[[ + // @Param: AUTOLAND_WP_DIST + // @DisplayName: Final approach waypoint distance + // @Description: Distance from landng point (HOME) to final approach waypoint created by script in the opposite direction of initial takeoff + // @Range: 0 1000 + // @Units: m + // @User: Standard +--]] +local AULND_DIST = bind_add_param('WP_DIST', 3, 0) +local final_wp_dist = AULND_DIST:get() + +FRAME_GLOBAL = 3 +NAV_WAYPOINT = 16 +NAV_TAKEOFF = 22 +NAV_LAND = 21 +DO_LAND_START = 189 + +TAKEOFF_PITCH = 15 + +local function wrap_360(angle) + local res = math.fmod(angle, 360.0) + if res < 0 then + res = res + 360.0 + end + return res +end + +local function wrap_180(angle) + local res = wrap_360(angle) + if res > 180 then + res = res - 360 + end + return res +end + +function create_final_approach_WP(i,bearing,dist,alt) --index,degs,m,m + local item = mavlink_mission_item_int_t() + local loc = ahrs:get_home() + loc:offset_bearing(bearing,dist) ---degs and meters + + item:seq(i) + item:frame(FRAME_GLOBAL) + item:command(NAV_WAYPOINT) + item:param1(0) + item:param2(0) + item:param3(0) + item:param4(0) + item:x(loc:lat()) + item:y(loc:lng()) + item:z(alt) + return item +end + +function create_takeoff_WP(alt) + local item = mavlink_mission_item_int_t() + local loc = ahrs:get_home() + + item:seq(1) + item:frame(FRAME_GLOBAL) + item:command(NAV_TAKEOFF) + item:param1(TAKEOFF_PITCH) + item:param2(0) + item:param3(0) + item:param4(0) + item:x(loc:lat()) + item:y(loc:lng()) + item:z(alt) + return item +end + +function create_land_WP() + local item = mavlink_mission_item_int_t() + local loc = ahrs:get_home() + + item:seq(4) + item:frame(FRAME_GLOBAL) + item:command(NAV_LAND) + item:param1(15) + item:param2(0) + item:param3(0) + item:param4(0) + item:x(loc:lat()) + item:y(loc:lng()) + item:z(0) + return item +end + +function create_do_land_start_WP() + local item = mavlink_mission_item_int_t() + + item:seq(2) + item:frame(FRAME_GLOBAL) + item:command(DO_LAND_START) + item:param1(0) + item:param2(0) + item:param3(0) + item:param4(0) + item:x(0) + item:y(0) + item:z(0) + return item +end + +function update() + if not arming:is_armed() then --if disarmed, wait until armed + mission_loaded = false + return update,1000 + end + + if not mission_loaded then --if first time after arm and enabled is valid then create mission + local home = ahrs:get_home() + local location = ahrs:get_location() + local speed = gps:ground_speed(0) + local alt = baro:get_altitude() + if location and home and speed > (0.5 * param:get("AIRSPEED_MIN")) then + local yaw = gps:ground_course(0) + mission:set_item(3,create_final_approach_WP(3,wrap_180(yaw+180),final_wp_dist,final_wp_alt)) + mission:set_item(4,create_land_WP()) + mission_loaded = true + gcs:send_text(MAV_SEVERITY.NOTICE, string.format("Captured initial takeoff direction = %.1f at %.1f m and %.1f m/s",yaw, alt, speed)) + end + end + return update, 200 +end + +gcs:send_text(MAV_SEVERITY.INFO,"Loaded UniversalAutoLand.lua") +if enable == 1 then + if final_wp_dist == 0 or final_wp_alt ==0 then + gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("Must set Final Waypoint alt and dist values!")) + return + end + mission:clear() + local item = mavlink_mission_item_int_t() + item:command(NAV_WAYPOINT) + mission:set_item(0,item) + mission:set_item(1,create_takeoff_WP(param:get("TKOFF_ALT"))) + mission:set_item(2,create_do_land_start_WP()) + gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("Set Final Waypoint alt and dist values!")) + return update, 1000 +else + gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("Script disabled by AUTOLAND_ENABLE")) +return +end + diff --git a/libraries/AP_Scripting/applets/UniversalAutoLand.md b/libraries/AP_Scripting/applets/UniversalAutoLand.md new file mode 100644 index 0000000000000..b2b2b568905d9 --- /dev/null +++ b/libraries/AP_Scripting/applets/UniversalAutoLand.md @@ -0,0 +1,9 @@ +This script is intended to allow easy, unpre-planned operation at any location with the protection of a do-land-start autoland sequence for failsafes that accounts for takeoff direction (ie wind direction). Final approach objects must be considered before you launch. + +If enabled by AUTOLAND_ENABLE =1, setups up an autotakeoff waypoint as first waypoint and upon Arming , adds mission items consisting of: DO_LAND_START,Final Approach WP opposite bearing from HOME of heading used during takeoff, to AUTOLAND_WP_ALT above home, and at AUTOLAND_WP_DIST distancee, and a LAND waypoint at HOME and stops until next disarm/boot. Warnings are given if the AUTOLAND parameters are not set. + +In use you just arm and switch to AUTO, and then switch to other flight modes after takeoff is completed to fly around.....relatively assured that a failsafe (assuming defaults for Failsafe) will result in an autolanding in the correct direction. You can also just switch back to AUTO or RTL to do an autoland. + +Caveats: be sure you have tested and setup autoland and AUTOLAND parameters. Setting AUTOLAND_WP_ALT and _WP_DIST for a good glide path on a final approach is required (be aware of possible obstructions when using). Values of 400 meters distance and 55 meters altitude work well for typcial 1m wingspan/1 kg foam planes. RTL_AUTOLAND must be set and a value = 2 is recommended also. + +Be aware of obstacles that might come into play on a final approach. Be aware that occasionally final approach waypoints may not be exactly in line with the takeoff if the GPS signal quality is compromised during takeoff. diff --git a/libraries/Filter/AP_Filter_config.h b/libraries/Filter/AP_Filter_config.h index d94dd9df1b720..d89e7a0a620e5 100644 --- a/libraries/Filter/AP_Filter_config.h +++ b/libraries/Filter/AP_Filter_config.h @@ -2,14 +2,16 @@ #include -#ifndef AP_FILTER_NUM_FILTERS -#if BOARD_FLASH_SIZE > 1024 -#define AP_FILTER_NUM_FILTERS 8 -#else -#define AP_FILTER_NUM_FILTERS 0 -#endif -#endif - #ifndef AP_FILTER_ENABLED -#define AP_FILTER_ENABLED AP_FILTER_NUM_FILTERS > 0 +#define AP_FILTER_ENABLED BOARD_FLASH_SIZE > 1024 #endif + +#if AP_FILTER_ENABLED + #ifndef AP_FILTER_NUM_FILTERS + #if BOARD_FLASH_SIZE > 1024 + #define AP_FILTER_NUM_FILTERS 8 + #else + #define AP_FILTER_NUM_FILTERS 4 + #endif // BOARD_FLASH_SIZE + #endif // AP_FILTER_NUM_FILTERS +#endif // AP_FILTER_ENABLED diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index c0469abc7cf20..138e5ae875534 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -839,6 +839,11 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel) sitl->models.slung_payload_sim.update(get_position_relhome(), velocity_ef, accel_earth, wind_ef); #endif + // update tether +#if AP_SIM_TETHER_ENABLED + sitl->models.tether_sim.update(location); +#endif + // allow for changes in physics step adjust_frame_time(constrain_float(sitl->loop_rate_hz, rate_hz-1, rate_hz+1)); } @@ -1273,18 +1278,26 @@ void Aircraft::add_twist_forces(Vector3f &rot_accel) } } -#if AP_SIM_SLUNGPAYLOAD_ENABLED -// add body-frame force due to slung payload -void Aircraft::add_slungpayload_forces(Vector3f &body_accel) +// add body-frame force due to slung payload and tether +void Aircraft::add_external_forces(Vector3f &body_accel) { - Vector3f forces_ef; - sitl->models.slung_payload_sim.get_forces_on_vehicle(forces_ef); + Vector3f total_force; +#if AP_SIM_SLUNGPAYLOAD_ENABLED + Vector3f forces_ef_slung; + sitl->models.slung_payload_sim.get_forces_on_vehicle(forces_ef_slung); + total_force += forces_ef_slung; +#endif + +#if AP_SIM_TETHER_ENABLED + Vector3f forces_ef_tether; + sitl->models.tether_sim.get_forces_on_vehicle(forces_ef_tether); + total_force += forces_ef_tether; +#endif // convert ef forces to body-frame accelerations (acceleration = force / mass) - const Vector3f accel_bf = dcm.transposed() * forces_ef / mass; - body_accel += accel_bf; + const Vector3f accel_bf_tether = dcm.transposed() * total_force / mass; + body_accel += accel_bf_tether; } -#endif /* get position relative to home diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index e84a078ac8faf..a6d7b273c09b6 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -322,10 +322,8 @@ class Aircraft { void add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel); void add_twist_forces(Vector3f &rot_accel); -#if AP_SIM_SLUNGPAYLOAD_ENABLED - // add body-frame force due to slung payload - void add_slungpayload_forces(Vector3f &body_accel); -#endif + // add body-frame force due to payload + void add_external_forces(Vector3f &body_accel); // get local thermal updraft float get_local_updraft(const Vector3d ¤tPos); diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 4e019dde9b4de..273c2fc06dad7 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -49,12 +49,10 @@ void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot add_shove_forces(rot_accel, body_accel); add_twist_forces(rot_accel); -#if AP_SIM_SLUNGPAYLOAD_ENABLED - // add forces from slung payload - add_slungpayload_forces(body_accel); -#endif + // add forces from slung payload or tether payload + add_external_forces(body_accel); } - + /* update the multicopter simulation by one time step */ diff --git a/libraries/SITL/SIM_Tether.cpp b/libraries/SITL/SIM_Tether.cpp new file mode 100644 index 0000000000000..005347b4c25ce --- /dev/null +++ b/libraries/SITL/SIM_Tether.cpp @@ -0,0 +1,309 @@ +/* + 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 a static tether attached to the vehicle and ground +*/ + +#include "SIM_config.h" + +#if AP_SIM_TETHER_ENABLED + +#include "SIM_Tether.h" +#include "SITL.h" +#include +#include "SIM_Aircraft.h" +#include +#include +#include + +using namespace SITL; + +// TetherSim parameters +const AP_Param::GroupInfo TetherSim::var_info[] = { + // @Param: ENABLE + // @DisplayName: Tether Simulation Enable/Disable + // @Description: Enable or disable the tether simulation + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO_FLAGS("ENABLE", 1, TetherSim, enable, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: DENSITY + // @DisplayName: Tether Wire Density + // @Description: Linear mass density of the tether wire + // @Range: 0 1 + // @User: Advanced + AP_GROUPINFO("DENSITY", 2, TetherSim, tether_linear_density, 0.0167), + + // @Param: LINELEN + // @DisplayName: Tether Maximum Line Length + // @Description: Maximum length of the tether line in meters + // @Units: m + // @Range: 0 100 + // @User: Advanced + AP_GROUPINFO("LINELEN", 3, TetherSim, max_line_length, 100.0), + + // @Param: SYSID + // @DisplayName: Tether Simulation MAVLink System ID + // @Description: MAVLink system ID for the tether simulation, used to distinguish it from other systems on the network + // @Range: 0 255 + // @User: Advanced + AP_GROUPINFO("SYSID", 4, TetherSim, sys_id, 2), + + // @Param: STUCK + // @DisplayName: Tether Stuck Enable/Disable + // @Description: Enable or disable a stuck tether simulation + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO("STUCK", 5, TetherSim, tether_stuck, 0), + + // @Param: SPGCNST + // @DisplayName: Tether Spring Constant + // @Description: Spring constant for the tether to simulate elastic forces when stretched beyond its maximum length + // @Range: 0 255 + // @User: Advanced + AP_GROUPINFO("SPGCNST", 6, TetherSim, tether_spring_constant, 100), + + AP_GROUPEND +}; + +// TetherSim handles interaction with main vehicle +TetherSim::TetherSim() +{ + AP_Param::setup_object_defaults(this, var_info); +} + + +void TetherSim::update(const Location& veh_pos) +{ + if (!enable) { + return; + } + + if (veh_pos.is_zero()) { + return; + } + + // initialise fixed tether ground location + const uint32_t now_us = AP_HAL::micros(); + if (!initialised) { + // capture EKF origin + auto *sitl = AP::sitl(); + const Location ekf_origin = sitl->state.home; + if (ekf_origin.lat == 0 && ekf_origin.lng == 0) { + return; + } + + // more initialisation + last_update_us = now_us; + initialised = true; + } + + // calculate dt and update tether forces + const float dt = (now_us - last_update_us)*1.0e-6; + last_update_us = now_us; + + update_tether_force(veh_pos, dt); + + // send tether location to GCS at 5hz + // TO-Do: add provision to make the tether movable + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - last_report_ms >= reporting_period_ms) { + last_report_ms = now_ms; + send_report(); + write_log(); + } +} + +// get earth-frame forces on the vehicle from the tether +// returns true on success and fills in forces_ef argument, false on failure +bool TetherSim::get_forces_on_vehicle(Vector3f& forces_ef) const +{ + if (!enable) { + return false; + } + + forces_ef = veh_forces_ef; + return true; +} + +// send a report to the vehicle control code over MAVLink +void TetherSim::send_report(void) +{ + if (!mavlink_connected && mav_socket.connect(target_address, target_port)) { + ::printf("Tether System connected to %s:%u\n", target_address, (unsigned)target_port); + mavlink_connected = true; + } + if (!mavlink_connected) { + return; + } + + // get current time + uint32_t now_ms = AP_HAL::millis(); + + // send heartbeat at 1hz + const uint8_t component_id = MAV_COMP_ID_USER11; + if (now_ms - last_heartbeat_ms >= 1000) { + last_heartbeat_ms = now_ms; + + const mavlink_heartbeat_t heartbeat{ + custom_mode: 0, + type : MAV_TYPE_GROUND_ROVER, + autopilot : MAV_AUTOPILOT_INVALID, + base_mode: 0, + system_status: 0, + mavlink_version: 0, + }; + + mavlink_message_t msg; + mavlink_msg_heartbeat_encode_status( + sys_id.get(), + component_id, + &mav_status, + &msg, + &heartbeat); + uint8_t buf[300]; + const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); + mav_socket.send(buf, len); + } + + // send a GLOBAL_POSITION_INT messages + { + Location tether_anchor_loc; + int32_t alt_amsl_cm, alt_rel_cm; + if (!get_tether_ground_location(tether_anchor_loc) || + !tether_anchor_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm) || + !tether_anchor_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_rel_cm)) { + return; + } + const mavlink_global_position_int_t global_position_int{ + time_boot_ms: now_ms, + lat: tether_anchor_loc.lat, + lon: tether_anchor_loc.lng, + alt: alt_amsl_cm * 10, // amsl alt in mm + relative_alt: alt_rel_cm * 10, // relative alt in mm + vx: 0, // velocity in cm/s + vy: 0, // velocity in cm/s + vz: 0, // velocity in cm/s + hdg: 0 // heading in centi-degrees + }; + mavlink_message_t msg; + mavlink_msg_global_position_int_encode_status(sys_id, component_id, &mav_status, &msg, &global_position_int); + uint8_t buf[300]; + const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); + if (len > 0) { + mav_socket.send(buf, len); + } + } +} + +void TetherSim::write_log() +{ +#if HAL_LOGGING_ENABLED + // write log of tether state + // @LoggerMessage: TETH + // @Description: Tether state + // @Field: TimeUS: Time since system startup + // @Field: Len: Tether length + // @Field: VFN: Force on vehicle in North direction + // @Field: VFE: Force on vehicle in East direction + // @Field: VFD: Force on vehicle in Down direction + AP::logger().WriteStreaming("TETH", + "TimeUS,Len,VFN,VFE,VFD", // labels + "s----", // units + "F----", // multipliers + "Qffff", // format + AP_HAL::micros64(), + (float)tether_length, + (double)veh_forces_ef.x, + (double)veh_forces_ef.y, + (double)veh_forces_ef.z); +#endif +} +// returns true on success and fills in tether_ground_loc argument, false on failure +bool TetherSim::get_tether_ground_location(Location& tether_ground_loc) const +{ + // get EKF origin + auto *sitl = AP::sitl(); + if (sitl == nullptr) { + return false; + } + const Location ekf_origin = sitl->state.home; + if (ekf_origin.lat == 0 && ekf_origin.lng == 0) { + return false; + } + + tether_ground_loc = ekf_origin; + return true; +} + +void TetherSim::update_tether_force(const Location& veh_pos, float dt) +{ + + Location tether_anchor_loc; + if (!get_tether_ground_location(tether_anchor_loc)) { + return; + } + + // Step 1: Calculate the vector from the tether anchor to the vehicle + Vector3f tether_vector = veh_pos.get_distance_NED(tether_anchor_loc); + tether_length = tether_vector.length(); + + // Step 2: Check if tether is taut (length exceeds maximum allowed length) or stuck + if (tether_length > max_line_length) { + + // Calculate the stretch beyond the maximum length + float stretch = MAX(tether_length - max_line_length, 0.0f); + + // Apply a spring-like penalty force proportional to the stretch + float penalty_force_magnitude = tether_spring_constant * stretch; + + // Direction of force is along the tether, pulling toward the anchor + veh_forces_ef = tether_vector.normalized() * penalty_force_magnitude; + + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Tether: Exceeded maximum length."); + + return; + } + + if (tether_stuck) { + + // Calculate the stretch beyond the maximum length + float stretch = MAX(tether_length - tether_not_stuck_length, 0.0f); + + // Apply a spring-like penalty force proportional to the stretch + float penalty_force_magnitude = tether_spring_constant * stretch; + + // Direction of force is directly along the tether, towards the tether anchor point + veh_forces_ef = tether_vector.normalized() * penalty_force_magnitude; + + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Tether: Stuck."); + + return; + } else { + tether_not_stuck_length = tether_length; + } + + // Step 3: Calculate the weight of the tether being lifted + // The weight is proportional to the current tether length + const float tether_weight_force = tether_linear_density * tether_length * GRAVITY_MSS; + + // Step 4: Calculate the tension force + Vector3f tension_force_NED = tether_vector.normalized() * tether_weight_force; + + // Step 5: Apply the force to the vehicle + veh_forces_ef = tension_force_NED; +} + +#endif diff --git a/libraries/SITL/SIM_Tether.h b/libraries/SITL/SIM_Tether.h new file mode 100644 index 0000000000000..4a7366e1089cd --- /dev/null +++ b/libraries/SITL/SIM_Tether.h @@ -0,0 +1,95 @@ +/* + 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 a tethered vehicle. + Models the forces exerted by the tether and reports them to the vehicle simulation. The dynamics are not accurate but provide a very rough approximation intended to test a "stuck tether". + */ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_TETHER_ENABLED + +#include +#include +#include +#include + +namespace SITL { + +// TetherSim simulates a tethered to a vehicle +class TetherSim { +public: + // constructor + TetherSim(); + + // Update the tether simulation state using the vehicle's earth-frame position + void update(const Location& veh_pos); + + // Retrieve earth-frame forces on the vehicle due to the tether + // Returns true on success and fills in the forces_ef argument; false on failure + bool get_forces_on_vehicle(Vector3f& forces_ef) const; + + // Parameter table for configuration + static const struct AP_Param::GroupInfo var_info[]; + +private: + // Parameters + AP_Int8 enable; // Enable or disable the tether simulation + AP_Float tether_linear_density; // Linear mass density of the tether in kg/m + AP_Float max_line_length; // Maximum allowed tether length in meters + AP_Int8 sys_id; // MAVLink system ID for GCS reporting + AP_Int8 tether_stuck; // Set to 1 to simulate a stuck tether + AP_Float tether_spring_constant; // Spring constant for modeling tether stretch + + // Send MAVLink messages to the GCS + void send_report(); + + // Write tether simulation state to onboard log + void write_log(); + + // Retrieve the location of the tether anchor point on the ground + // Returns true on success and fills in the tether_anchor_loc argument; false on failure + bool get_tether_ground_location(Location& tether_anchor_loc) const; + + // Update forces exerted by the tether based on the vehicle's position + // Takes the vehicle's position and the simulation time step (dt) as inputs + void update_tether_force(const Location& veh_pos, float dt); + + // Socket connection variables for MAVLink communication + const char *target_address = "127.0.0.1"; // Address for MAVLink socket communication + const uint16_t target_port = 5763; // Port for MAVLink socket communication + SocketAPM_native mav_socket { false }; // Socket for MAVLink communication + bool initialised; // True if the simulation class is initialized + uint32_t last_update_us; // Timestamp of the last update in microseconds + + // MAVLink reporting variables + const float reporting_period_ms = 100; // Reporting period in milliseconds + uint32_t last_report_ms; // Timestamp of the last MAVLink report sent to GCS + uint32_t last_heartbeat_ms; // Timestamp of the last MAVLink heartbeat sent to GCS + bool mavlink_connected; // True if MAVLink connection is established + mavlink_status_t mav_status; // Status of MAVLink communication + + // Tether simulation variables + Vector3f veh_forces_ef; // Earth-frame forces on the vehicle due to the tether + float tether_length = 0.0f; // Current tether length in meters + float tether_not_stuck_length = 0.0f; // Tether length when the tether is not stuck +}; + +} // namespace SITL + +#endif // AP_SIM_TETHER_ENABLED diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index d417577249278..12fca3b30789c 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -57,6 +57,10 @@ #define AP_SIM_SLUNGPAYLOAD_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif +#ifndef AP_SIM_TETHER_ENABLED +#define AP_SIM_TETHER_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + #ifndef AP_SIM_FLIGHTAXIS_ENABLED #define AP_SIM_FLIGHTAXIS_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index e329ed232a960..03ba5f1771718 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -1367,6 +1367,12 @@ const AP_Param::GroupInfo SIM::ModelParm::var_info[] = { AP_SUBGROUPPTR(flightaxis_ptr, "RFL_", 5, SIM::ModelParm, FlightAxis), #endif +#if AP_SIM_TETHER_ENABLED + // @Group: TETH_ + // @Path: ./SIM_Tether.cpp + AP_SUBGROUPINFO(tether_sim, "TETH_", 6, SIM::ModelParm, TetherSim), +#endif + AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 4edda5efd21e4..54d935043718a 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -28,6 +28,7 @@ #include "SIM_IntelligentEnergy24.h" #include "SIM_Ship.h" #include "SIM_SlungPayload.h" +#include "SIM_Tether.h" #include "SIM_GPS.h" #include "SIM_DroneCANDevice.h" #include "SIM_ADSB_Sagetech_MXS.h" @@ -338,6 +339,9 @@ class SIM { #if AP_SIM_SLUNGPAYLOAD_ENABLED SlungPayloadSim slung_payload_sim; #endif +#if AP_SIM_TETHER_ENABLED + TetherSim tether_sim; +#endif #if AP_SIM_FLIGHTAXIS_ENABLED FlightAxis *flightaxis_ptr; #endif