diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp index 79d1739ed21f0f..2d20396ecfc694 100644 --- a/AntennaTracker/Parameters.cpp +++ b/AntennaTracker/Parameters.cpp @@ -23,6 +23,7 @@ const AP_Param::Info Tracker::var_info[] = { // @DisplayName: Ground station MAVLink system ID // @Description: The identifier of the ground station in the MAVLink protocol. Don't change this unless you also modify the ground station to match. // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 7c09a42b07f7c4..b449b18e146fcc 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -46,6 +46,7 @@ const AP_Param::Info Copter::var_info[] = { // @DisplayName: My ground station number // @Description: Allows restricting radio overrides to only come from my ground station // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index 40acda0f05c592..749a830f4dce65 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -114,9 +114,9 @@ void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t di } else { // absolute angle _fixed_yaw_offset_cd = wrap_180_cd(angle_deg * 100.0 - _yaw_angle_cd); - if ( direction < 0 && is_positive(_fixed_yaw_offset_cd) ) { + if (direction < 0 && is_positive(_fixed_yaw_offset_cd)) { _fixed_yaw_offset_cd -= 36000.0; - } else if ( direction > 0 && is_negative(_fixed_yaw_offset_cd) ) { + } else if (direction > 0 && is_negative(_fixed_yaw_offset_cd)) { _fixed_yaw_offset_cd += 36000.0; } } diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index b79b008c2e6b64..99449a2991daea 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1850,8 +1850,6 @@ void ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd) // Do (Now) commands /********************************************************************************/ - - void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd) { if (cmd.content.speed.target_ms > 0) { diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 720eed4c273694..fbecd0ee40b6c4 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -23,6 +23,7 @@ const AP_Param::Info Plane::var_info[] = { // @DisplayName: Ground station MAVLink system ID // @Description: The identifier of the ground station in the MAVLink protocol. Don't change this unless you also modify the ground station to match. // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index ff124dc6ba3dd1..b6c3a2b44fc863 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -113,6 +113,9 @@ bool Mode::enter() plane.adsb.set_is_auto_mode(does_auto_navigation()); #endif + // set the nav controller stale AFTER _enter() so that we can check if we're currently in a loiter during the mode change + plane.nav_controller->set_data_is_stale(); + // reset steering integrator on mode change plane.steerController.reset_I(); diff --git a/ArduPlane/mode_LoiterAltQLand.cpp b/ArduPlane/mode_LoiterAltQLand.cpp index 0cc8b9457c11e7..8fbfd59735cd24 100644 --- a/ArduPlane/mode_LoiterAltQLand.cpp +++ b/ArduPlane/mode_LoiterAltQLand.cpp @@ -10,17 +10,13 @@ bool ModeLoiterAltQLand::_enter() return true; } + // If we were already in a loiter then use that waypoint. Else, use the current point + const bool already_in_a_loiter = plane.nav_controller->reached_loiter_target() && !plane.nav_controller->data_is_stale(); + const Location loiter_wp = already_in_a_loiter ? plane.next_WP_loc : plane.current_loc; + ModeLoiter::_enter(); -#if AP_TERRAIN_AVAILABLE - if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) { - plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN); - } else { - plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); - } -#else - plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); -#endif + handle_guided_request(loiter_wp); switch_qland(); diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 73ce3dbcb02f54..83ce1933c30289 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -47,6 +47,8 @@ const AP_Param::Info Sub::var_info[] = { // @Param: SYSID_MYGCS // @DisplayName: My ground station number // @Description: Allows restricting radio overrides to only come from my ground station + // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), diff --git a/Blimp/Parameters.cpp b/Blimp/Parameters.cpp index 26971ef2e65392..593e9f26687f74 100644 --- a/Blimp/Parameters.cpp +++ b/Blimp/Parameters.cpp @@ -41,6 +41,7 @@ const AP_Param::Info Blimp::var_info[] = { // @DisplayName: My ground station number // @Description: Allows restricting radio overrides to only come from my ground station // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 313cec0c21dc62..5d4a84fc9b08b6 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -42,6 +42,7 @@ const AP_Param::Info Rover::var_info[] = { // @DisplayName: MAVLink ground station ID // @Description: The identifier of the ground station in the MAVLink protocol. Don't change this unless you also modify the ground station to match. // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), diff --git a/Rover/RC_Channel.cpp b/Rover/RC_Channel.cpp index e799f38a70fd24..9a89b66d613764 100644 --- a/Rover/RC_Channel.cpp +++ b/Rover/RC_Channel.cpp @@ -33,6 +33,7 @@ void RC_Channel_Rover::init_aux_function(const aux_func_t ch_option, const AuxSw // the following functions do not need initialising: case AUX_FUNC::ACRO: case AUX_FUNC::AUTO: + case AUX_FUNC::CIRCLE: case AUX_FUNC::FOLLOW: case AUX_FUNC::GUIDED: case AUX_FUNC::HOLD: @@ -226,6 +227,10 @@ bool RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const AuxSwit do_aux_function_change_mode(rover.mode_simple, ch_flag); break; + case AUX_FUNC::CIRCLE: + do_aux_function_change_mode(rover.g2.mode_circle, ch_flag); + break; + // trigger sailboat tack case AUX_FUNC::SAILBOAT_TACK: // any switch movement interpreted as request to tack diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index 439f4a014b034e..5d2029b8201d25 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -82,21 +82,15 @@ void ModeAuto::update() switch (_submode) { case SubMode::WP: { - // check if we've reached the destination - if (!g2.wp_nav.reached_destination() || g2.wp_nav.is_fast_waypoint()) { - // update navigation controller + // boats loiter once the waypoint is reached + bool keep_navigating = true; + if (rover.is_boat() && g2.wp_nav.reached_destination() && !g2.wp_nav.is_fast_waypoint()) { + keep_navigating = !start_loiter(); + } + + // update navigation controller + if (keep_navigating) { navigate_to_waypoint(); - } else { - // we have reached the destination so stay here - if (rover.is_boat()) { - if (!start_loiter()) { - start_stop(); - } - } else { - start_stop(); - } - // update distance to destination - _distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination()); } break; } diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 376e7245ba0e0d..b0e0839d6f08c9 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -26,7 +26,6 @@ #include #include #include -#include #include #include #include @@ -77,6 +76,15 @@ #define HAL_PERIPH_CAN_MIRROR 0 #endif +#if defined(HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT) && !defined(HAL_DEBUG_BUILD) && !defined(HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_NON_DEBUG) +/* this checking for reboot can lose bytes on GPS modules and other + * serial devices. It is really only relevent on a debug build if you + * really want it for non-debug build then define + * HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_NON_DEBUG in hwdef.dat + */ +#undef HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT +#endif + #include "Parameters.h" #if CONFIG_HAL_BOARD == HAL_BOARD_SITL diff --git a/Tools/AP_Periph/networking.h b/Tools/AP_Periph/networking.h index d25dc5aa7aaa77..6b7f857fdcde8c 100644 --- a/Tools/AP_Periph/networking.h +++ b/Tools/AP_Periph/networking.h @@ -1,9 +1,11 @@ #pragma once -#include "AP_Periph.h" +#include #ifdef HAL_PERIPH_ENABLE_NETWORKING +#include + #ifndef HAL_PERIPH_NETWORK_NUM_PASSTHRU #define HAL_PERIPH_NETWORK_NUM_PASSTHRU 2 #endif diff --git a/Tools/AP_Periph/networking_passthru.cpp b/Tools/AP_Periph/networking_passthru.cpp index 18ed58bf68bac5..3d2f65a1cad7b7 100644 --- a/Tools/AP_Periph/networking_passthru.cpp +++ b/Tools/AP_Periph/networking_passthru.cpp @@ -13,7 +13,7 @@ along with this program. If not, see . */ -#include "networking.h" +#include "AP_Periph.h" #if defined(HAL_PERIPH_ENABLE_NETWORKING) && HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0 diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 776a1cd9223724..e18d3137065f3d 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -462,8 +462,8 @@ def configure_env(self, cfg, env): # We always want to use PRI format macros cfg.define('__STDC_FORMAT_MACROS', 1) - if cfg.options.disable_ekf2: - env.CXXFLAGS += ['-DHAL_NAVEKF2_AVAILABLE=0'] + if cfg.options.enable_ekf2: + env.CXXFLAGS += ['-DHAL_NAVEKF2_AVAILABLE=1'] if cfg.options.disable_ekf3: env.CXXFLAGS += ['-DHAL_NAVEKF3_AVAILABLE=0'] @@ -654,6 +654,12 @@ def configure_env(self, cfg, env): cfg.define('AP_NOTIFY_LP5562_BUS', 2) cfg.define('AP_NOTIFY_LP5562_ADDR', 0x30) + try: + env.CXXFLAGS.remove('-DHAL_NAVEKF2_AVAILABLE=0') + except ValueError: + pass + env.CXXFLAGS += ['-DHAL_NAVEKF2_AVAILABLE=1'] + if self.with_can: cfg.define('HAL_NUM_CAN_IFACES', 2) env.DEFINES.update(CANARD_MULTI_IFACE=1, @@ -793,6 +799,7 @@ def configure_env(self, cfg, env): # whitelist of compilers which we should build with -Werror gcc_whitelist = frozenset([ ('11','3','0'), + ('11','4','0'), ('12','1','0'), ]) @@ -871,13 +878,17 @@ def configure_env(self, cfg, env): AP_CAN_SLCAN_ENABLED = 0, HAL_PROXIMITY_ENABLED = 0, AP_SCRIPTING_ENABLED = 0, - HAL_NAVEKF2_AVAILABLE = 0, HAL_NAVEKF3_AVAILABLE = 0, HAL_PWM_COUNT = 32, HAL_WITH_ESC_TELEM = 1, AP_RTC_ENABLED = 0, ) + try: + env.CXXFLAGS.remove('-DHAL_NAVEKF2_AVAILABLE=1') + except ValueError: + pass + env.CXXFLAGS += ['-DHAL_NAVEKF2_AVAILABLE=0'] class esp32(Board): abstract = True @@ -1138,7 +1149,8 @@ def configure_env(self, cfg, env): ] env.INCLUDES += [ - cfg.srcnode.find_dir('libraries/AP_GyroFFT/CMSIS_5/include').abspath() + cfg.srcnode.find_dir('libraries/AP_GyroFFT/CMSIS_5/include').abspath(), + cfg.srcnode.find_dir('modules/ChibiOS/ext/lwip/src/include/compat/posix').abspath() ] # whitelist of compilers which we should build with -Werror @@ -1149,6 +1161,7 @@ def configure_env(self, cfg, env): ('9','3','1'), ('10','2','1'), ('11','3','0'), + ('11','4','0'), ]) if cfg.env.HAL_CANFD_SUPPORTED: diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index c7898e2b755353..f8afa254b4cec3 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -7678,6 +7678,12 @@ def RichenPower(self): raise NotAchievedException("Did not find expected GEN message") def IE24(self): + '''Test IntelligentEnergy 2.4kWh generator with V1 and V2 telemetry protocols''' + protocol_ver = (1, 2) + for ver in protocol_ver: + self.run_IE24(ver) + + def run_IE24(self, proto_ver): '''Test IntelligentEnergy 2.4kWh generator''' elec_battery_instance = 2 fuel_battery_instance = 1 @@ -7687,14 +7693,14 @@ def IE24(self): "GEN_TYPE": 2, "BATT%u_MONITOR" % (fuel_battery_instance + 1): 18, # fuel-based generator "BATT%u_MONITOR" % (elec_battery_instance + 1): 17, - "SIM_IE24_ENABLE": 1, + "SIM_IE24_ENABLE": proto_ver, "LOG_DISARMED": 1, }) self.customise_SITL_commandline(["--uartF=sim:ie24"]) - self.start_subtest("ensure that BATTERY_STATUS for electrical generator message looks right") - self.start_subsubtest("Checking original voltage (electrical)") + self.start_subtest("Protocol %i: ensure that BATTERY_STATUS for electrical generator message looks right" % proto_ver) + self.start_subsubtest("Protocol %i: Checking original voltage (electrical)" % proto_ver) # ArduPilot spits out essentially uninitialised battery # messages until we read things fromthe battery: self.delay_sim_time(30) @@ -7712,13 +7718,13 @@ def IE24(self): "battery_remaining": original_elec_m.battery_remaining - 1, }, instance=elec_battery_instance) - self.start_subtest("ensure that BATTERY_STATUS for fuel generator message looks right") - self.start_subsubtest("Checking original voltage (fuel)") + self.start_subtest("Protocol %i: ensure that BATTERY_STATUS for fuel generator message looks right" % proto_ver) + self.start_subsubtest("Protocol %i: Checking original voltage (fuel)" % proto_ver) # ArduPilot spits out essentially uninitialised battery # messages until we read things fromthe battery: if original_fuel_m.battery_remaining <= 90: raise NotAchievedException("Bad original percentage (want=>%f got %f" % (90, original_fuel_m.battery_remaining)) - self.start_subsubtest("Ensure percentage is counting down") + self.start_subsubtest("Protocol %i: Ensure percentage is counting down" % proto_ver) self.wait_message_field_values('BATTERY_STATUS', { "battery_remaining": original_fuel_m.battery_remaining - 1, }, instance=fuel_battery_instance) @@ -7728,7 +7734,7 @@ def IE24(self): self.disarm_vehicle() # Test for pre-arm check fail when state is not running - self.start_subtest("If you haven't taken off generator error should cause instant failsafe and disarm") + self.start_subtest("Protocol %i: Without takeoff generator error should cause failsafe and disarm" % proto_ver) self.set_parameter("SIM_IE24_STATE", 8) self.wait_statustext("Status not running", timeout=40) self.try_arm(result=False, @@ -7736,7 +7742,7 @@ def IE24(self): self.set_parameter("SIM_IE24_STATE", 2) # Explicitly set state to running # Test that error code does result in failsafe - self.start_subtest("If you haven't taken off generator error should cause instant failsafe and disarm") + self.start_subtest("Protocol %i: Without taken off generator error should cause failsafe and disarm" % proto_ver) self.change_mode("STABILIZE") self.set_parameter("DISARM_DELAY", 0) self.arm_vehicle() diff --git a/Tools/autotest/param_metadata/param_parse.py b/Tools/autotest/param_metadata/param_parse.py index 7f5f9c21202dee..fdb1465e29cc4d 100755 --- a/Tools/autotest/param_metadata/param_parse.py +++ b/Tools/autotest/param_metadata/param_parse.py @@ -109,11 +109,12 @@ def lua_applets(): libraries = [] -# AP_Vehicle also has parameters rooted at "", but isn't referenced -# from the vehicle in any way: -ap_vehicle_lib = Library("", reference="VEHICLE") # the "" is tacked onto the front of param name -setattr(ap_vehicle_lib, "Path", os.path.join('..', 'libraries', 'AP_Vehicle', 'AP_Vehicle.cpp')) -libraries.append(ap_vehicle_lib) +if args.vehicle != "AP_Periph": + # AP_Vehicle also has parameters rooted at "", but isn't referenced + # from the vehicle in any way: + ap_vehicle_lib = Library("", reference="VEHICLE") # the "" is tacked onto the front of param name + setattr(ap_vehicle_lib, "Path", os.path.join('..', 'libraries', 'AP_Vehicle', 'AP_Vehicle.cpp')) + libraries.append(ap_vehicle_lib) libraries.append(lua_applets()) diff --git a/Tools/autotest/sim_vehicle.py b/Tools/autotest/sim_vehicle.py index 89113e04108a34..065b5265c1c53f 100755 --- a/Tools/autotest/sim_vehicle.py +++ b/Tools/autotest/sim_vehicle.py @@ -378,8 +378,8 @@ def do_build(opts, frame_options): if opts.math_check_indexes: cmd_configure.append("--enable-math-check-indexes") - if opts.disable_ekf2: - cmd_configure.append("--disable-ekf2") + if opts.enable_ekf2: + cmd_configure.append("--enable-ekf2") if opts.disable_ekf3: cmd_configure.append("--disable-ekf3") @@ -1295,7 +1295,7 @@ def generate_frame_help(): group_sim.add_option("--fram-storage", action='store_true', help="use fram storage emulation") -group_sim.add_option("--disable-ekf2", +group_sim.add_option("--enable-ekf2", action='store_true', help="disable EKF2 in build") group_sim.add_option("--disable-ekf3", diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index c44aef485f5842..87c881efbda588 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -4268,27 +4268,94 @@ def TestLogDownloadMAVProxyNetwork(self, upload_logs=False): self.set_parameters({ "NET_ENABLED": 1, "NET_DHCP": 0, + "LOG_DARM_RATEMAX": 2, # make small logs + # UDP client "NET_P1_TYPE": 1, "NET_P1_PROTOCOL": 2, - "NET_P1_PORT": 15004, + "NET_P1_PORT": 16001, "NET_P1_IP0": 127, "NET_P1_IP1": 0, "NET_P1_IP2": 0, - "NET_P1_IP3": 1 + "NET_P1_IP3": 1, + # UDP server + "NET_P2_TYPE": 2, + "NET_P2_PROTOCOL": 2, + "NET_P2_PORT": 16002, + "NET_P2_IP0": 0, + "NET_P2_IP1": 0, + "NET_P2_IP2": 0, + "NET_P2_IP3": 0, + # TCP client + "NET_P3_TYPE": 3, + "NET_P3_PROTOCOL": 2, + "NET_P3_PORT": 16003, + "NET_P3_IP0": 127, + "NET_P3_IP1": 0, + "NET_P3_IP2": 0, + "NET_P3_IP3": 1, + # TCP server + "NET_P4_TYPE": 4, + "NET_P4_PROTOCOL": 2, + "NET_P4_PORT": 16004, + "NET_P4_IP0": 0, + "NET_P4_IP1": 0, + "NET_P4_IP2": 0, + "NET_P4_IP3": 0, }) self.reboot_sitl() - filename = "MAVProxy-downloaded-net-log.BIN" - mavproxy = self.start_mavproxy(master=':15004') - self.mavproxy_load_module(mavproxy, 'log') - mavproxy.send("log list\n") - mavproxy.expect("numLogs") - self.wait_heartbeat() - self.wait_heartbeat() - mavproxy.send("set shownoise 0\n") - mavproxy.send("log download latest %s\n" % filename) - mavproxy.expect("Finished downloading", timeout=120) - self.mavproxy_unload_module(mavproxy, 'log') - self.stop_mavproxy(mavproxy) + endpoints = [('UDPClient', ':16001') , + ('UDPServer', 'udpout:127.0.0.1:16002'), + ('TCPClient', 'tcpin:0.0.0.0:16003'), + ('TCPServer', 'tcp:127.0.0.1:16004')] + for name, e in endpoints: + self.progress("Downloading log with %s %s" % (name, e)) + filename = "MAVProxy-downloaded-net-log-%s.BIN" % name + + mavproxy = self.start_mavproxy(master=e) + self.mavproxy_load_module(mavproxy, 'log') + self.wait_heartbeat() + mavproxy.send("log list\n") + mavproxy.expect("numLogs") + mavproxy.send("log download latest %s\n" % filename) + mavproxy.expect("Finished downloading", timeout=120) + self.mavproxy_unload_module(mavproxy, 'log') + self.stop_mavproxy(mavproxy) + + self.set_parameters({ + # multicast UDP client + "NET_P1_TYPE": 1, + "NET_P1_PROTOCOL": 2, + "NET_P1_PORT": 14550, + "NET_P1_IP0": 239, + "NET_P1_IP1": 255, + "NET_P1_IP2": 145, + "NET_P1_IP3": 50, + # Broadcast UDP client + "NET_P2_TYPE": 1, + "NET_P2_PROTOCOL": 2, + "NET_P2_PORT": 16005, + "NET_P2_IP0": 255, + "NET_P2_IP1": 255, + "NET_P2_IP2": 255, + "NET_P2_IP3": 255, + }) + self.reboot_sitl() + endpoints = [('UDPMulticast', 'mcast:') , + ('UDPBroadcast', ':16005')] + for name, e in endpoints: + self.progress("Downloading log with %s %s" % (name, e)) + filename = "MAVProxy-downloaded-net-log-%s.BIN" % name + + mavproxy = self.start_mavproxy(master=e) + self.mavproxy_load_module(mavproxy, 'log') + self.wait_heartbeat() + mavproxy.send("log list\n") + mavproxy.expect("numLogs") + mavproxy.send("log download latest %s\n" % filename) + mavproxy.expect("Finished downloading", timeout=120) + self.mavproxy_unload_module(mavproxy, 'log') + self.stop_mavproxy(mavproxy) + self.context_pop() def TestLogDownloadMAVProxyCAN(self, upload_logs=False): diff --git a/Tools/bootloaders/Aocoda-RC-H743Dual_bl.bin b/Tools/bootloaders/Aocoda-RC-H743Dual_bl.bin new file mode 100755 index 00000000000000..d806e3b8f53b95 Binary files /dev/null and b/Tools/bootloaders/Aocoda-RC-H743Dual_bl.bin differ diff --git a/Tools/bootloaders/Aocoda-RC-H743Dual_bl.hex b/Tools/bootloaders/Aocoda-RC-H743Dual_bl.hex new file mode 100644 index 00000000000000..ecc103e918efce --- /dev/null +++ b/Tools/bootloaders/Aocoda-RC-H743Dual_bl.hex @@ -0,0 +1,1004 @@ +:020000040800F2 +:1000000000060020E1020008E3020008E302000805 +:10001000E3020008E3020008E3020008E30200082C +:10002000E3020008E3020008E3020008BD34000810 +:10003000E3020008E3020008E3020008E30200080C +:10004000E3020008E3020008E3020008E3020008FC +:10005000E3020008E3020008A5380008D1380008D0 +:10006000FD3800082939000855390008E302000866 +:10007000E3020008E3020008E3020008E3020008CC +:10008000E3020008E3020008E3020008E3020008BC +:10009000E3020008E3020008E302000881390008D7 +:1000A000E3020008E3020008E3020008E30200089C +:1000B000E3020008E3020008E3020008E30200088C +:1000C000E3020008E3020008E3020008E30200087C +:1000D000E3020008E3020008E3020008E30200086C +:1000E000E5390008E3020008E3020008E302000823 +:1000F000E3020008E3020008E3020008E30200084C +:10010000E3020008E3020008593A0008E30200088D +:10011000E3020008E3020008E3020008E30200082B +:10012000E3020008E3020008E3020008E30200081B +:10013000E3020008E3020008E3020008E30200080B +:10014000E3020008E3020008E3020008E3020008FB +:10015000E3020008E3020008E3020008E3020008EB +:10016000E3020008E3020008E3020008E3020008DB +:10017000E30200083D2E0008E3020008E302000845 +:10018000E3020008E3020008E3020008E3020008BB +:10019000E3020008E3020008E3020008E3020008AB +:1001A000E3020008E3020008E3020008E30200089B +:1001B000E3020008E3020008E3020008E30200088B +:1001C000E3020008E3020008E3020008E30200087B +:1001D000E3020008292E0008E3020008E3020008F9 +:1001E000E3020008E3020008E3020008E30200085B +:1001F000E3020008E3020008E3020008E30200084B +:10020000E3020008E3020008E3020008E30200083A +:10021000E3020008E3020008E3020008E30200082A +:10022000E3020008E3020008E3020008E30200081A +:10023000E3020008E3020008E3020008E30200080A +:10024000E3020008E3020008E3020008E3020008FA +:10025000E3020008E3020008E3020008E3020008EA +:10026000E3020008E3020008E3020008E3020008DA +:10027000E3020008E3020008E3020008E3020008CA +:10028000E3020008E3020008E3020008E3020008BA +:10029000E3020008E3020008E3020008E3020008AA +:1002A000E3020008E3020008E3020008E30200089A +:1002B000E3020008E3020008E3020008E30200088A +:1002C000E3020008E3020008E3020008E30200087A +:1002D000E3020008E3020008E3020008E30200086A +:1002E00002E000F000F8FEE772B6374880F30888B5 +:1002F000364880F3098836483649086040F20000E5 +:10030000CCF200004EF63471CEF200010860BFF36B +:100310004F8FBFF36F8F40F20000C0F2F0004EF637 +:100320008851CEF200010860BFF34F8FBFF36F8F8B +:100330004FF00000E1EE100A4EF63C71CEF20001E3 +:100340000860062080F31488BFF36F8F01F0E2FF8E +:1003500003F006F84FF055301F491B4A91423CBF4D +:1003600041F8040BFAE71D49184A91423CBF41F895 +:10037000040BFAE71A491B4A1B4B9A423EBF51F83D +:10038000040B42F8040BF8E700201849184A914280 +:100390003CBF41F8040BFAE701F0FAFF03F064F800 +:1003A000144C154DAC4203DA54F8041B8847F9E7A6 +:1003B00000F042F8114C124DAC4203DA54F8041B21 +:1003C0008847F9E701F0E2BF000600200022002084 +:1003D0000000000808ED00E00000002000060020FA +:1003E000383E0008002200205C220020602200200D +:1003F00034430020E0020008E0020008E0020008A8 +:10040000E00200082DE9F04F2DED108AC1F80CD064 +:10041000D0F80CD0BDEC108ABDE8F08F002383F338 +:1004200011882846A047002001F0FAFAFEE701F003 +:1004300089FA00DFFEE7000038B500F0DBFB01F0D1 +:1004400029FF054601F05CFF0446D0B90F4B9D42E1 +:1004500019D001339D4241F2883512BF0446002570 +:100460000124002001F020FF0CB100F077F800F02B +:1004700073FD00F025FC284600F01EF900F06EF830 +:10048000F9E70025EDE70546EBE700BF010007B0FF +:1004900008B500F0E7FBA0F120035842584108BD21 +:1004A00007B541F21203022101A8ADF8043000F0B3 +:1004B000F7FB03B05DF804FB38B5302383F31188F4 +:1004C000174803680BB101F077FB0023154A4FF47E +:1004D0007A71134801F066FB002383F31188124CF4 +:1004E000236813B12368013B2360636813B1636819 +:1004F000013B63600D4D2B7833B963687BB90220F3 +:1005000000F0A4FC322363602B78032B07D16368CF +:100510002BB9022000F09AFC4FF47A73636038BD67 +:1005200060220020B90400088023002078220020E7 +:10053000084B187003280CD8DFE800F00805020803 +:10054000022000F07BBC022000F06EBC024B0022B7 +:100550005A6070477822002080230020F8B5504B65 +:10056000504A1C461968013100F0998004339342C7 +:10057000F8D162684C4B9A4240F291804B4B9B6899 +:1005800003F1006303F500339A4280F08880002075 +:1005900000F0BCFB0220FFF7CBFF454B0021D3F856 +:1005A000E820C3F8E810D3F81021C3F81011D3F8ED +:1005B0001021D3F8EC20C3F8EC10D3F81421C3F8C1 +:1005C0001411D3F81421D3F8F020C3F8F010D3F8A5 +:1005D0001821C3F81811D3F81821D3F8802042F05D +:1005E0000062C3F88020D3F8802022F00062C3F8B4 +:1005F0008020D3F88020D3F8802042F00072C3F826 +:100600008020D3F8802022F00072C3F88020D3F835 +:10061000803072B64FF0E023C3F8084DD4E90004EF +:10062000BFF34F8FBFF36F8F224AC2F88410BFF31E +:100630004F8F536923F480335361BFF34F8FD2F848 +:10064000803043F6E076C3F3C905C3F34E335B0154 +:1006500003EA060C29464CEA81770139C2F8747224 +:10066000F9D2203B13F1200FF2D1BFF34F8FBFF32C +:100670006F8FBFF34F8FBFF36F8F536923F4003336 +:1006800053610023C2F85032BFF34F8FBFF36F8F17 +:10069000302383F31188854680F308882047F8BD0E +:1006A0000000020820000208FFFF010800220020CD +:1006B0000044025800ED00E02DE9F04F93B0B44B38 +:1006C0002022FF2100900AA89D6800F0EFFBB14AAC +:1006D0001378A3B90121B04811700360302383F36C +:1006E000118803680BB101F067FA0023AB4A4FF49D +:1006F0007A71A94801F056FA002383F31188009B10 +:1007000013B1A74B009A1A60A64A1378032B03D0A3 +:1007100000231370A24A53604FF0000A009CD34696 +:100720005646D146012000F089FB24B19C4B1B6842 +:10073000002B00F02682002000F094FA0390039B27 +:10074000002BF2DB012000F06FFB039B213B1F2BF2 +:10075000E8D801A252F823F0D907000801080008E0 +:100760009508000825070008250700082507000848 +:1007700027090008F70A0008110A0008730A000890 +:100780009B0A0008C10A000825070008D30A0008D0 +:1007900025070008450B0008790800082507000810 +:1007A000890B0008E50700087908000825070008FC +:1007B000730A000825070008250700082507000818 +:1007C0002507000825070008250700082507000859 +:1007D00025070008950800080220FFF759FE0028A9 +:1007E00040F0F981009B022105A8BAF1000F08BF73 +:1007F0001C4641F21233ADF8143000F051FA91E783 +:100800004FF47A7000F02EFA071EEBDB0220FFF7A0 +:100810003FFE0028E6D0013F052F00F2DE81DFE831 +:1008200007F0030A0D1013360523042105A80593CC +:1008300000F036FA17E004215548F9E704215A4838 +:10084000F6E704215948F3E74FF01C08404608F149 +:10085000040800F05DFA0421059005A800F020FAD4 +:10086000B8F12C0FF2D101204FF0000900FA07F780 +:1008700047EA0B0B5FFA8BFB00F064FB26B10BF031 +:100880000B030B2B08BF0024FFF70AFE4AE70421E5 +:100890004748CDE7002EA5D00BF00B030B2BA1D1C1 +:1008A0000220FFF7F5FD074600289BD00120002617 +:1008B00000F02CFA0220FFF73BFE1FFA86F84046B4 +:1008C00000F034FA0446B0B1039940460136A1F174 +:1008D00040025142514100F039FA0028EDD1BA46A8 +:1008E000044641F21213022105A83E46ADF8143029 +:1008F00000F0D6F916E725460120FFF719FE244B34 +:100900009B68AB4207D9284600F002FA013040F05C +:1009100067810435F3E70025224BBA463E461D7039 +:100920001F4B5D60A8E7002E3FF45CAF0BF00B039C +:100930000B2B7FF457AF0220FFF7FAFD322000F0B7 +:1009400091F9B0F10008FFF64DAF18F003077FF4FE +:1009500049AF0F4A08EB0503926893423FF642AF56 +:10096000B8F5807F3FF73EAF124BB845019323DDCA +:100970004FF47A7000F076F90390039A002AFFF69C +:1009800031AF039A0137019B03F8012BEDE700BF5C +:10099000002200207C23002060220020B9040008EF +:1009A000802300207822002004220020082200203A +:1009B0000C2200207C220020C820FFF769FD07469A +:1009C00000283FF40FAF1F2D11D8C5F120020AAB4C +:1009D00025F0030084494245184428BF424601924D +:1009E00000F03EFA019AFF217F4800F05FFA4FEADB +:1009F000A803C8F387027C492846019300F05EFAF9 +:100A0000064600283FF46DAF019B05EB830533E7F5 +:100A10000220FFF73DFD00283FF4E4AE00F0B4F9FA +:100A200000283FF4DFAE0027B846704B9B68BB42FE +:100A300018D91F2F11D80A9B01330ED027F00303BA +:100A400012AA134453F8203C05934046042205A9FA +:100A5000043700F0ADFA8046E7E7384600F058F971 +:100A60000590F2E7CDF81480042105A800F018F9EC +:100A700002E70023642104A8049300F007F900288A +:100A80007FF4B0AE0220FFF703FD00283FF4AAAECA +:100A9000049800F06FF90590E6E70023642104A8AC +:100AA000049300F0F3F800287FF49CAE0220FFF7D7 +:100AB000EFFC00283FF496AE049800F05DF9EAE7F9 +:100AC0000220FFF7E5FC00283FF48CAE00F06CF943 +:100AD000E1E70220FFF7DCFC00283FF483AE05A924 +:100AE000142000F067F907460421049004A800F0E0 +:100AF000D7F83946B9E7322000F0B4F8071EFFF600 +:100B000071AEBB077FF46EAE384A07EB09039268FB +:100B100093423FF667AE0220FFF7BAFC00283FF48D +:100B200061AE27F003074F44B9453FF4A5AE4846F0 +:100B300009F1040900F0ECF80421059005A800F083 +:100B4000AFF8F1E74FF47A70FFF7A2FC00283FF40A +:100B500049AE00F019F9002844D00A9B01330BD0AC +:100B600008220AA9002000F0A9F900283AD0202282 +:100B7000FF210AA800F09AF9FFF792FC1C4800F048 +:100B800055FF13B0BDE8F08F002E3FF42BAE0BF0F5 +:100B90000B030B2B7FF426AE0023642105A80593DD +:100BA00000F074F8074600287FF41CAE0220FFF71F +:100BB0006FFC804600283FF415AEFFF771FC41F250 +:100BC000883000F033FF059800F0F0F946463C46C7 +:100BD00000F0B8F9A6E506464EE64FF0000901E63A +:100BE000BA467EE637467CE67C22002000220020C2 +:100BF000A0860100704700000F4B70B51B780C46B3 +:100C00000133DBB2012B11D80C4D4FF47A732968F4 +:100C1000A2FB033222460E6A01462846B0478442B0 +:100C200004D1074B002201201A7070BD4FF4FA70F6 +:100C300000F0FCFE0020F8E710220020282600200B +:100C4000B423002070B504464FF47A76412C254633 +:100C500028BF412506FB05F000F0E8FE641BF5D136 +:100C600070BD0000002307B5024601210DF1070009 +:100C70008DF80730FFF7C0FF20B19DF8070003B0E3 +:100C80005DF804FB4FF0FF30F9E700000A4604214D +:100C900008B5FFF7B1FF80F00100C0B2404208BDC7 +:100CA00030B4054C0A46014623682046DD69034BF3 +:100CB000AC4630BC604700BF28260020A08601005B +:100CC00070B5104C0025104E01F0CCF92080306832 +:100CD000238883420CD800252088013801F0BEF912 +:100CE00023880544013BB5F5802F2380F4D370BDE4 +:100CF00001F0B4F9336805440133B5F5003F3360C2 +:100D0000E5D3E8E7B62300208823002001F078BA75 +:100D100000F1006000F500300068704700F10060ED +:100D2000920000F5003001F0F9B90000054B1A6897 +:100D3000054B1B889B1A834202D9104401F08EB9DF +:100D40000020704788230020B623002038B50446D1 +:100D5000074D29B128682044BDE8384001F096B914 +:100D60002868204401F080F90028F3D038BD00BF86 +:100D7000882300200020704700F1FF5000F58F10FD +:100D8000D0F8000870470000064991F8243033B1CC +:100D900000230822086A81F82430FFF7BFBF012032 +:100DA000704700BF8C230020014B1868704700BFBC +:100DB0000010005C194B01380322084470B51D680F +:100DC000174BC5F30B042D0C1E88A6420BD15C6893 +:100DD0000A46013C824213460FD214F9016F4EB10C +:100DE00002F8016BF6E7013A03F10803ECD1814206 +:100DF0000B4602D22C2203F8012B0424094A168840 +:100E0000AE4204D1984284BF967803F8016B013C4E +:100E100002F10402F3D1581A70BD00BF0010005C4B +:100E200014220020503B0008022803D1024B4FF44B +:100E300000229A61704700BF00100258022802D1B8 +:100E4000014B08229A61704700100258022804D111 +:100E5000024A536983F00803536170470010025837 +:100E6000002310B5934203D0CC5CC4540133F9E79E +:100E700010BD0000013810B510F9013F3BB191F9E8 +:100E800000409C4203D11AB10131013AF4E71AB192 +:100E900091F90020981A10BD1046FCE7034602465F +:100EA000D01A12F9011B0029FAD1704702440346F7 +:100EB000934202D003F8011BFAE770472DE9F8438B +:100EC0001F4D14460746884695F8242052BBDFF88C +:100ED00070909CB395F824302BB92022FF2148460E +:100EE0002F62FFF7E3FF95F824004146C0F10802A6 +:100EF00005EB8000A24228BF2246D6B29200FFF73F +:100F0000AFFF95F82430A41B17441E449044E4B26C +:100F1000F6B2082E85F82460DBD1FFF735FF0028F4 +:100F2000D7D108E02B6A03EB82038342CFD0FFF7CF +:100F30002BFF0028CBD10020BDE8F8830120FBE780 +:100F40008C230020024B1A78024B1A70704700BFA6 +:100F5000B42300201022002010B5114C114800F0DD +:100F6000B9F821460F4800F0E1F824684FF47A7090 +:100F7000D4F89020D2F8043843F00203C2F80438C1 +:100F8000FFF760FE0849204600F0DEF9D4F8902013 +:100F9000D2F8043823F00203C2F8043810BD00BFB1 +:100FA0000C3C000828260020143C00087047000074 +:100FB00030B50A44084D91420DD011F8013B58401C +:100FC000082340F30004013B2C4013F0FF0384EAA4 +:100FD0005000F6D1EFE730BD2083B8ED02684368DA +:100FE0001143016003B118477047000013B5406B0F +:100FF00000F58054D4F8A4381A681178042914D163 +:10100000017C022911D11979012312898B401342E5 +:101010000BD101A94C3002F059F8D4F8A44802468B +:10102000019B2179206800F0DFF902B010BD0000BB +:10103000143001F0DBBF00004FF0FF33143001F03B +:10104000D5BF00004C3002F0ADB800004FF0FF33C8 +:101050004C3002F0A7B80000143001F0A9BF000026 +:101060004FF0FF31143001F0A3BF00004C3002F00C +:1010700079B800004FF0FF324C3002F073B8000036 +:101080000020704710B500F58054D4F8A4381A68D1 +:101090001178042917D1017C022914D1597901232F +:1010A00052898B4013420ED1143001F03BFF0246AF +:1010B00048B1D4F8A4484FF4407361792068BDE882 +:1010C000104000F07FB910BD406BFFF7DBBF0000A0 +:1010D000704700007FB5124B0125042604460360CB +:1010E0000023057400F1840243602946C0E90233FD +:1010F0000C4B0290143001934FF44073009601F0B2 +:10110000EDFE094B04F69442294604F14C0002948A +:10111000CDE900634FF4407301F0B4FF04B070BD3B +:10112000603B0008C9100008ED0F00080A68302372 +:1011300083F311880B790B3342F823004B79133377 +:1011400042F823008B7913B10B3342F8230000F5EA +:101150008053C3F8A41802230374002080F311887D +:101160007047000038B5037F044613B190F854303F +:10117000ABB90125201D0221FFF730FF04F1140057 +:101180006FF00101257700F079FC04F14C0084F840 +:1011900054506FF00101BDE8384000F06FBC38BD1D +:1011A00010B5012104460430FFF718FF0023237710 +:1011B00084F8543010BD000038B5044600251430C2 +:1011C00001F0A4FE04F14C00257701F073FF201D0F +:1011D00084F854500121FFF701FF2046BDE8384054 +:1011E000FFF750BF90F8803003F06003202B06D14A +:1011F00090F881200023212A03D81F2A06D8002036 +:101200007047222AFBD1C0E91D3303E0034A42673D +:1012100007228267C3670120704700BF2C2200208D +:1012200037B500F58055D5F8A4381A681178042927 +:101230001AD1017C022917D11979012312898B4017 +:10124000134211D100F14C04204601F0F3FF58B1D4 +:1012500001A9204601F03AFFD5F8A4480246019BB7 +:101260002179206800F0C0F803B030BD01F10B0314 +:10127000F0B550F8236085B004460D46FEB130232A +:1012800083F3118804EB8507301D0821FFF7A6FEC4 +:10129000FB6806F14C005B691B681BB1019001F013 +:1012A00023FF019803A901F011FF024648B1039BF7 +:1012B0002946204600F098F8002383F3118805B0F2 +:1012C000F0BDFB685A691268002AF5D01B8A013B01 +:1012D0001340F1D104F18002EAE70000133138B580 +:1012E00050F82140ECB1302383F3118804F580538A +:1012F000D3F8A4281368527903EB8203DB689B6957 +:101300005D6845B104216018FFF768FE294604F1C5 +:10131000140001F011FE2046FFF7B4FE002383F312 +:10132000118838BD7047000001F0C8B80123402281 +:10133000002110B5044600F8303BFFF7B7FD00234D +:10134000C4E9013310BD000010B53023044683F317 +:1013500011882422416000210C30FFF7A7FD2046B0 +:1013600001F0CEF802230020237080F3118810BD15 +:1013700070B500EB8103054650690E461446DA60ED +:1013800018B110220021FFF791FDA06918B11022B9 +:101390000021FFF78BFD31462846BDE8704001F083 +:1013A000B5B9000083682022002103F0011310B5B5 +:1013B000044683601030FFF779FD2046BDE81040F9 +:1013C00001F030BAF0B4012500EB810447898D406B +:1013D000E4683D43A469458123600023A260636003 +:1013E000F0BC01F04DBA0000F0B4012500EB81041F +:1013F00007898D40E4683D436469058123600023CB +:10140000A2606360F0BC01F0C3BA000070B50223B3 +:1014100000250446242203702946C0F888500C3069 +:1014200040F8045CFFF742FD204684F8705001F05C +:1014300001F963681B6823B129462046BDE8704066 +:10144000184770BD0378052B10B504460AD080F804 +:101450008C300523037043681B680BB10421984747 +:101460000023A36010BD00000178052906D190F883 +:101470008C20436802701B6803B118477047000056 +:1014800070B590F87030044613B1002380F87030C6 +:1014900004F18002204601F0E9F963689B68B3B962 +:1014A00094F8803013F0600535D00021204601F01B +:1014B000DBFC0021204601F0CBFC63681B6813B104 +:1014C000062120469847062384F8703070BD2046D8 +:1014D00098470028E4D0B4F88630A26F9A4288BFBB +:1014E000A36794F98030A56F002B4FF0300380F292 +:1014F0000381002D00F0F280092284F8702083F32C +:10150000118800212046D4E91D23FFF76DFF002339 +:1015100083F31188DAE794F8812003F07F0343EA2C +:10152000022340F20232934200F0C58021D8B3F585 +:10153000807F48D00DD8012B3FD0022B00F0938044 +:10154000002BB2D104F1880262670222A267E3672E +:10155000C1E7B3F5817F00F09B80B3F5407FA4D154 +:1015600094F88230012BA0D1B4F8883043F0020304 +:1015700032E0B3F5006F4DD017D8B3F5A06F31D07E +:10158000A3F5C063012B90D86368204694F88220AD +:101590005E6894F88310B4F88430B047002884D093 +:1015A000436863670368A3671AE0B3F5106F36D02A +:1015B00040F6024293427FF478AF5C4B63670223AC +:1015C000A3670023C3E794F88230012B7FF46DAF4B +:1015D000B4F8883023F00203A4F88830C4E91D551C +:1015E000E56778E7B4F88030B3F5A06F0ED194F8D2 +:1015F0008230204684F88A3001F07AF863681B68EC +:1016000013B1012120469847032323700023C4E926 +:101610001D339CE704F18B0363670123C3E7237841 +:10162000042B10D1302383F311882046FFF7BAFE34 +:1016300085F311880321636884F88B5021701B683F +:101640000BB12046984794F88230002BDED084F806 +:101650008B300423237063681B68002BD6D00221D3 +:1016600020469847D2E794F8843020461D0603F0C0 +:101670000F010AD501F0ECF8012804D002287FF40C +:1016800014AF2B4B9AE72B4B98E701F0D3F8F3E715 +:1016900094F88230002B7FF408AF94F8843013F074 +:1016A0000F01B3D01A06204602D501F0F5FBADE7D5 +:1016B00001F0E6FBAAE794F88230002B7FF4F5AE48 +:1016C00094F8843013F00F01A0D01B06204602D5F9 +:1016D00001F0CAFB9AE701F0BBFB97E7142284F8FC +:1016E000702083F311882B462A4629462046FFF7AF +:1016F00069FE85F31188E9E65DB1152284F8702052 +:1017000083F3118800212046D4E91D23FFF75AFEF8 +:10171000FDE60B2284F8702083F311882B462A46BD +:1017200029462046FFF760FEE3E700BF903B000834 +:10173000883B00088C3B000838B590F870300446B0 +:10174000002B3ED0063BDAB20F2A34D80F2B32D80A +:10175000DFE803F0373131082232313131313131B4 +:1017600031313737856FB0F886309D4214D2C36867 +:101770001B8AB5FBF3F203FB12556DB9302383F3DB +:1017800011882B462A462946FFF72EFE85F311883D +:101790000A2384F870300EE0142384F8703030236C +:1017A00083F31188002320461A461946FFF70AFEE4 +:1017B000002383F3118838BDC36F03B1984700231A +:1017C000E7E70021204601F04FFB0021204601F011 +:1017D0003FFB63681B6813B1062120469847062328 +:1017E000D7E7000010B590F870300446142B29D0CC +:1017F00017D8062B05D001D81BB110BD093B022B11 +:10180000FBD80021204601F02FFB0021204601F0EB +:101810001FFB63681B6813B1062120469847062307 +:1018200019E0152BE9D10B2380F87030302383F3B6 +:10183000118800231A461946FFF7D6FD002383F3CB +:101840001188DAE7C3689B695B68002BD5D1C36F49 +:1018500003B19847002384F87030CEE700238268F4 +:10186000037503691B6899689142FBD25A6803604B +:1018700042601060586070470023826803750369F6 +:101880001B6899689142FBD85A68036042601060F7 +:101890005860704708B50846302383F311880B7DE4 +:1018A000032B05D0042B0DD02BB983F3118808BD71 +:1018B0008B6900221A604FF0FF338361FFF7CEFF80 +:1018C0000023F2E7D1E9003213605A60F3E7000029 +:1018D000FFF7C4BF054BD9680875186802685360E4 +:1018E0001A600122D8600275FEF78CBDB823002073 +:1018F0000C4B30B5DD684B1C87B004460FD02B462F +:10190000094A684600F04EF92046FFF7E3FF009BC6 +:1019100013B1684600F050F9A86907B030BDFFF771 +:10192000D9FFF9E7B823002095180008044B1A687E +:10193000DB6890689B68984294BF00200120704744 +:10194000B8230020084B10B51C68D8682268536083 +:101950001A600122DC602275FFF78EFF01462046E7 +:10196000BDE81040FEF74EBDB823002038B5074C47 +:1019700001230025064907482370656001F098FCA3 +:101980000223237085F3118838BD00BF2026002074 +:10199000983B0008B823002000F044B9034A51687E +:1019A00053685B1A9842FBD8704700BF001000E0F4 +:1019B0008B600223086108468B8270478368A3F11D +:1019C000840243F8142C026943F8442C426943F81A +:1019D000402C094A43F8242CC268A3F1200043F8A4 +:1019E000182C022203F80C2C002203F80B2C034ABB +:1019F00043F8102C704700BF1D040008B8230020D6 +:101A000008B5FFF7DBFFBDE80840FFF761BF000046 +:101A1000024BDB6898610F20FFF75CBFB823002002 +:101A2000302383F31188FFF7F3BF000008B50146A8 +:101A3000302383F311880820FFF75AFF002383F334 +:101A4000118808BD064BDB6839B1426818605A60DE +:101A5000136043600420FFF74BBF4FF0FF30704727 +:101A6000B82300200368984206D01A6802605060CC +:101A700018469961FFF72CBF7047000038B504463F +:101A80000D462068844200D138BD036823605C6045 +:101A90008561FFF71DFFF4E7036810B59C68A2425B +:101AA0000CD85C688A600B604C6021605960996852 +:101AB0008A1A9A604FF0FF33836010BD121B1B68B7 +:101AC000ECE700000A2938BF0A2170B504460D462C +:101AD0000A26601901F0E4FB01F0CCFB041BA542CF +:101AE00003D8751C04462E46F3E70A2E04D90120BC +:101AF000BDE8704001F01CBC70BD0000F8B5144B8F +:101B00000D460A2A4FF00A07D96103F110018260DD +:101B100038BF0A22416019691446016048601861A3 +:101B2000A81801F0ADFB01F0A5FB431B0646A3423C +:101B300006D37C1C28192746354601F0B1FBF2E795 +:101B40000A2F04D90120BDE8F84001F0F1BBF8BD2F +:101B5000B8230020F8B506460D4601F08BFB0F4A6E +:101B6000134653F8107F9F4206D12A46014630465D +:101B7000BDE8F840FFF7C2BFD169BB68441A2C1911 +:101B800028BF2C46A34202D92946FFF79BFF2246D5 +:101B900031460348BDE8F840FFF77EBFB823002078 +:101BA000C8230020C0E90323002310B45DF8044BD0 +:101BB0004361FFF7CFBF000010B5194C236998426D +:101BC0000DD08168D0E9003213605A609A680A44E7 +:101BD0009A60002303604FF0FF33A36110BD0268D9 +:101BE000234643F8102F53600022026022699A4274 +:101BF00003D1BDE8104001F04DBB936881680B44F0 +:101C0000936001F037FB2269E1699268441AA242AD +:101C1000E4D91144BDE81040091AFFF753BF00BFD3 +:101C2000B82300202DE9F047DFF8BC8008F1100749 +:101C30002C4ED8F8105001F01DFBD8F81C40AA68B3 +:101C4000031B9A423ED814444FF00009D5E90032F4 +:101C5000C8F81C4013605A60C5F80090D8F81030DE +:101C6000B34201D101F016FB89F31188D5E90331A4 +:101C700028469847302383F311886B69002BD8D00E +:101C800001F0F8FA6A69A0EB040982464A450DD2D0 +:101C9000022001F04DFB0022D8F81030B34208D1E9 +:101CA00051462846BDE8F047FFF728BF121A2244E4 +:101CB000F2E712EB09092946384638BF4A46FFF7D2 +:101CC000EBFEB5E7D8F81030B34208D01444C8F89A +:101CD0001C00211AA960BDE8F047FFF7F3BEBDE87C +:101CE000F08700BFC8230020B823002000207047E1 +:101CF000FEE70000704700004FF0FF307047000023 +:101D000002290CD0032904D00129074818BF00205C +:101D10007047032A05D8054800EBC2007047044805 +:101D200070470020704700BF703C00083C22002034 +:101D3000243C000870B59AB005460846144601A92F +:101D400000F0C2F801A8FFF7A9F8431C0022C6B2B0 +:101D50005B001046C5E9003423700323023404F805 +:101D6000013C01ABD1B202348E4201D81AB070BD31 +:101D700013F8011B013204F8010C04F8021CF1E70E +:101D800008B5302383F311880348FFF749FA00238D +:101D900083F3118808BD00BF2826002090F880300A +:101DA00003F01F02012A07D190F881200B2A03D1EA +:101DB0000023C0E91D3315E003F06003202B08D198 +:101DC000B0F884302BB990F88120212A03D81F2A3B +:101DD00004D8FFF707BA222AEBD0FAE7034A426792 +:101DE00007228267C3670120704700BF33220020AB +:101DF00007B5052917D8DFE801F0191603191920CE +:101E0000302383F31188104A01210190FFF7B0FAC3 +:101E1000019802210D4AFFF7ABFA0D48FFF7CCF904 +:101E2000002383F3118803B05DF804FB302383F3B0 +:101E300011880748FFF796F9F2E7302383F31188FA +:101E40000348FFF7ADF9EBE7C43B0008E83B0008A7 +:101E50002826002038B50C4D0C4C2A460C4904F1BC +:101E60000800FFF767FF05F1CA0204F110000949F5 +:101E7000FFF760FF05F5CA7204F118000649BDE8D6 +:101E80003840FFF757BF00BF003F00203C22002032 +:101E9000A43B0008AE3B0008B93B000870B50446FF +:101EA00008460D46FEF7FAFFC6B220460134037815 +:101EB0000BB9184670BD32462946FEF7DBFF0028F5 +:101EC000F3D10120F6E700002DE9F04705460C4666 +:101ED000FEF7E4FF2B49C6B22846FFF7DFFF08B143 +:101EE0000B36F6B228492846FFF7D8FF08B110365E +:101EF000F6B2632E0BD8DFF88C80DFF88C90234F7E +:101F0000DFF894A02E7846B92670BDE8F087294600 +:101F10002046BDE8F04701F0FDBD252E2ED1072259 +:101F200041462846FEF7A6FF70B9194B224603F139 +:101F3000100153F8040B8B4242F8040BF9D11B88B3 +:101F4000073512341380DDE7082249462846FEF79C +:101F500091FF98B9A21C0F4B197802320909C95D8B +:101F600002F8041C13F8011B01F00F015345C95D71 +:101F700002F8031CF0D118340835C3E7013504F822 +:101F8000016BBFE7903C0008B93B0008AB3C000880 +:101F9000983C000800E8F11F0CE8F11FBFF34F8FD9 +:101FA000044B1A695107FCD1D3F810215207F8D11C +:101FB000704700BF0020005208B50D4B1B78ABB92D +:101FC000FFF7ECFF0B4BDA68D10704D50A4A5A60D9 +:101FD00002F188325A60D3F80C21D20706D5064A9E +:101FE000C3F8042102F18832C3F8042108BD00BF00 +:101FF0005E410020002000522301674508B5114BC7 +:102000001B78F3B9104B1A69510703D5DA6842F00F +:102010004002DA60D3F81021520705D5D3F80C211D +:1020200042F04002C3F80C21FFF7B8FF064BDA6814 +:1020300042F00102DA60D3F80C2142F00102C3F849 +:102040000C2108BD5E410020002000520F289ABFDD +:1020500000F5806040040020704700004FF400301D +:1020600070470000102070470F2808B50BD8FFF705 +:10207000EDFF00F500330268013204D104308342E1 +:10208000F9D1012008BD0020FCE700000F2838B579 +:1020900005463FD8FFF782FF1F4CFFF78DFF4FF03B +:1020A000FF3307286361C4F814311DD82361FFF79B +:1020B00075FF030243F02403E360E36843F0800309 +:1020C000E36023695A07FCD42846FFF767FFFFF750 +:1020D000BDFF4FF4003100F0F5F82846FFF78EFF02 +:1020E000BDE83840FFF7C0BFC4F81031FFF756FF16 +:1020F000A0F108031B0243F02403C4F80C31D4F808 +:102100000C3143F08003C4F80C31D4F810315B0774 +:10211000FBD4D9E7002038BD002000522DE9F84F4C +:1021200005460C46104645EA0203DE0602D00020B2 +:10213000BDE8F88F20F01F00DFF8BCB0DFF8BCA0CE +:10214000FFF73AFF04EB0008444503D10120FFF7F5 +:1021500055FFEDE720222946204601F0CBFC10B9BF +:1021600020352034F0E72B4605F120021F68791C4A +:10217000DDD104339A42F9D105F178431B481C4E56 +:10218000B3F5801F1B4B38BF184603F1F80332BF6D +:10219000D946D1461E46FFF701FF0760A5EB040CA8 +:1021A000336804F11C0143F002033360231FD9F8A4 +:1021B000007017F00507FAD153F8042F8B424CF842 +:1021C0000320F4D1BFF34F8FFFF7E8FE4FF0FF334A +:1021D0002022214603602846336823F0020333603F +:1021E00001F088FC0028BBD03846B0E7142100522B +:1021F0000C200052142000521020005210210052D6 +:1022000010B5084C237828B11BB9FFF7D5FE012380 +:10221000237010BD002BFCD02070BDE81040FFF7EC +:10222000EDBE00BF5E4100200244074BD2B210B5A4 +:10223000904200D110BD441C00B253F8200041F878 +:10224000040BE0B2F4E700BF504000580E4B30B52D +:102250001C6F240405D41C6F1C671C6F44F40044DD +:102260001C670A4C02442368D2B243F48073236093 +:10227000074B904200D130BD441C51F8045B00B2C2 +:1022800043F82050E0B2F4E70044025800480258F6 +:102290005040005807B5012201A90020FFF7C4FFF4 +:1022A000019803B05DF804FB13B50446FFF7F2FF95 +:1022B000A04205D0012201A900200194FFF7C6FF2A +:1022C00002B010BD0144BFF34F8F064B884204D3C8 +:1022D000BFF34F8FBFF36F8F7047C3F85C0220309E +:1022E000F4E700BF00ED00E0034B1A681AB9034A97 +:1022F000D2F8D0241A607047604100200040025894 +:1023000008B5FFF7F1FF024B1868C0F3806008BD05 +:102310006041002070B5BFF34F8FBFF36F8F1A4A33 +:102320000021C2F85012BFF34F8FBFF36F8F536974 +:1023300043F400335361BFF34F8FBFF36F8FC2F885 +:102340008410BFF34F8FD2F8803043F6E074C3F3AC +:10235000C900C3F34E335B0103EA0406014646EAB3 +:1023600081750139C2F86052F9D2203B13F1200F78 +:10237000F2D1BFF34F8F536943F480335361BFF3FE +:102380004F8FBFF36F8F70BD00ED00E0FEE70000E0 +:10239000214B2248224A70B5904237D3214BC11EAF +:1023A000DA1C121A22F003028B4238BF00220021ED +:1023B000FEF77CFD1C4A0023C2F88430BFF34F8F28 +:1023C000D2F8803043F6E074C3F3C900C3F34E3350 +:1023D0005B0103EA0406014646EA81750139C2F849 +:1023E0006C52F9D2203B13F1200FF2D1BFF34F8F83 +:1023F000BFF36F8FBFF34F8FBFF36F8F0023C2F810 +:102400005032BFF34F8FBFF36F8F70BD53F8041B73 +:1024100040F8041BC0E700BF943E0008344300208E +:10242000344300203443002000ED00E0074BD3F894 +:10243000D81021EA0001C3F8D810D3F8002122EA0D +:102440000002C3F80021D3F800317047004402585D +:1024500070B5D0E9244300224FF0FF359E6804EBAD +:1024600042135101D3F80009002805DAD3F8000916 +:1024700040F08040C3F80009D3F8000B002805DACB +:10248000D3F8000B40F08040C3F8000B0132631812 +:102490009642C3F80859C3F8085BE0D24FF0011325 +:1024A000C4F81C3870BD0000890141F020010161B1 +:1024B00003699B06FCD41220FFF770BA10B50A4CD2 +:1024C0002046FEF733FF094BC4F89030084BC4F8A0 +:1024D0009430084C2046FEF729FF074BC4F8903093 +:1024E000064BC4F8943010BD644100200000084041 +:1024F000E03C00080042002000000440EC3C0008E2 +:1025000070B503780546012B5CD1434BD0F8904061 +:10251000984258D1414B0E216520D3F8D82042F083 +:102520000062C3F8D820D3F8002142F00062C3F85B +:102530000021D3F80021D3F8802042F00062C3F8D4 +:102540008020D3F8802022F00062C3F88020D3F8E6 +:10255000803000F0ADFC324BE360324BC4F8003801 +:102560000023D5F89060C4F8003EC02323604FF4E8 +:102570000413A3633369002BFCDA01230C203361BD +:10258000FFF70CFA3369DB07FCD41220FFF706FAD9 +:102590003369002BFCDA00262846A660FFF758FFB7 +:1025A0006B68C4F81068DB68C4F81468C4F81C6869 +:1025B00083BB1D4BA3614FF0FF336361A36843F0FE +:1025C0000103A36070BD194B9842C9D1134B4FF062 +:1025D0008060D3F8D82042F00072C3F8D820D3F836 +:1025E000002142F00072C3F80021D3F80021D3F893 +:1025F000802042F00072C3F88020D3F8802022F0BF +:102600000072C3F88020D3F88030FFF70FFF0E214F +:102610004D209EE7064BCDE7644100200044025860 +:102620004014004003002002003C30C00042002063 +:10263000083C30C0F8B5D0F89040054600214FF076 +:1026400000662046FFF730FFD5F8941000234FF0C6 +:1026500001128F684FF0FF30C4F83438C4F81C28DA +:1026600004EB431201339F42C2F80069C2F8006BC9 +:10267000C2F80809C2F8080BF2D20B68D5F890200E +:10268000C5F89830636210231361166916F01006BE +:10269000FBD11220FFF782F9D4F8003823F4FE634F +:1026A000C4F80038A36943F4402343F01003A36146 +:1026B0000923C4F81038C4F814380B4BEB604FF002 +:1026C000C043C4F8103B094BC4F8003BC4F8106980 +:1026D000C4F80039D5F8983003F1100243F48013A0 +:1026E000C5F89820A362F8BDBC3C000840800010EB +:1026F000D0F8902090F88A10D2F8003823F4FE63C6 +:1027000043EA0113C2F80038704700002DE9F8438E +:1027100000EB8103D0F890500C468046DA680FFA3F +:1027200081F94801166806F00306731E022B05EBBB +:1027300041134FF0000194BFB604384EC3F8101B8C +:102740004FF0010104F1100398BF06F1805601FA21 +:1027500003F3916998BF06F5004600293AD0578ADD +:1027600004F15801374349016F50D5F81C180B4349 +:102770000021C5F81C382B180127C3F81019A740F1 +:102780005369611E9BB3138A928B9B08012A88BFF1 +:102790005343D8F89820981842EA034301F14002C5 +:1027A0002146C8F89800284605EB82025360FFF7DF +:1027B0007BFE08EB8900C3681B8A43EA84534834D4 +:1027C0001E4364012E51D5F81C381F43C5F81C78F0 +:1027D000BDE8F88305EB4917D7F8001B21F4004149 +:1027E000C7F8001BD5F81C1821EA0303C0E704F161 +:1027F0003F030B4A2846214605EB83035A60FFF747 +:1028000053FE05EB4910D0F8003923F40043C0F81B +:102810000039D5F81C3823EA0707D7E700800010F5 +:1028200000040002D0F894201268C0F89820FFF746 +:102830000FBE00005831D0F8903049015B5813F4B6 +:10284000004004D013F4001F0CBF02200120704789 +:102850004831D0F8903049015B5813F4004004D05F +:1028600013F4001F0CBF02200120704700EB810110 +:10287000CB68196A0B6813604B685360704700009F +:1028800000EB810330B5DD68AA691368D36019B91C +:10289000402B84BF402313606B8A1468D0F89020CB +:1028A0001C4402EB4110013C09B2B4FBF3F4634356 +:1028B000033323F0030343EAC44343F0C043C0F8A7 +:1028C000103B2B6803F00303012B0ED1D2F808381C +:1028D00002EB411013F4807FD0F8003B14BF43F0AB +:1028E000805343F00053C0F8003B02EB4112D2F892 +:1028F000003B43F00443C2F8003B30BD2DE9F041FA +:10290000D0F8906005460C4606EB4113D3F8087BDF +:102910003A07C3F8087B08D5D6F814381B0704D546 +:1029200000EB8103DB685B689847FA071FD5D6F890 +:102930001438DB071BD505EB8403D968CCB98B6948 +:10294000488A5A68B2FBF0F600FB16228AB918686A +:10295000DA6890420DD2121AC3E90024302383F3BF +:10296000118821462846FFF78BFF84F31188BDE8C4 +:10297000F081012303FA04F26B8923EA02036B81DD +:10298000CB68002BF3D021462846BDE8F04118471C +:1029900000EB81034A0170B5DD68D0F890306C69B6 +:1029A0002668E66056BB1A444FF40020C2F81009AE +:1029B0002A6802F00302012A0AB20ED1D3F80808ED +:1029C00003EB421410F4807FD4F8000914BF40F0E8 +:1029D000805040F00050C4F8000903EB4212D2F8D6 +:1029E000000940F00440C2F800090122D3F834087D +:1029F00002FA01F10143C3F8341870BD19B9402E31 +:102A000084BF4020206020681A442E8A8419013C2B +:102A1000B4FBF6F440EAC44040F00050C6E70000C2 +:102A20002DE9F843D0F8906005460C464F0106EBBF +:102A30004113D3F8088918F0010FC3F808891CD096 +:102A4000D6F81038DB0718D500EB8103D3F80CC09B +:102A5000DCF81430D3F800E0DA68964530D2A2EB07 +:102A60000E024FF000091A60C3F80490302383F37C +:102A70001188FFF78DFF89F3118818F0800F1DD0A2 +:102A8000D6F834380126A640334217D005EB84032C +:102A90000134D5F89050D3F80CC0E4B22F44DCF8E0 +:102AA000142005EB0434D2F800E05168714514D3CA +:102AB000D5F8343823EA0606C5F83468BDE8F8834B +:102AC000012303FA01F2038923EA02030381DCF8FC +:102AD0000830002BD1D09847CFE7AEEB0103BCF80C +:102AE0001000834228BF0346D7F8180980B2B3EB21 +:102AF000800FE3D89068A0F1040959F8048FC4F856 +:102B00000080A0EB09089844B8F1040FF5D81844E8 +:102B10000B4490605360C8E72DE9F84FD0F890500F +:102B200004466E69AB691E4016F480586E6103D08E +:102B3000BDE8F84FFEF76ABC002E12DAD5F8003E69 +:102B40009B0705D0D5F8003E23F00303C5F8003EEF +:102B5000D5F80438204623F00103C5F80438FEF701 +:102B600083FC370505D52046FFF772FC2046FEF7AB +:102B700069FCB0040CD5D5F8083813F0060FEB68E3 +:102B800023F470530CBF43F4105343F4A053EB6091 +:102B900031071BD56368DB681BB9AB6923F00803F9 +:102BA000AB612378052B0CD1D5F8003E9A0705D0F0 +:102BB000D5F8003E23F00303C5F8003E2046FEF79B +:102BC00053FC6368DB680BB120469847F30200F1C1 +:102BD000BA80B70226D5D4F8909000274FF0010AAA +:102BE00009EB4712D2F8003B03F44023B3F5802FE2 +:102BF00011D1D2F8003B002B0DDA62890AFA07F3F3 +:102C000022EA0303638104EB8703DB68DB6813B10B +:102C10003946204698470137D4F89430FFB29B6874 +:102C20009F42DDD9F00619D5D4F89000026AC2F3AC +:102C30000A1702F00F0302F4F012B2F5802F00F031 +:102C4000CA80B2F5402F09D104EB8303002200F5BE +:102C50008050DB681B6A974240F0B0803003D5F8A3 +:102C6000185835D5E90303D500212046FFF746FE65 +:102C7000AA0303D501212046FFF740FE6B0303D5CD +:102C800002212046FFF73AFE2F0303D503212046F9 +:102C9000FFF734FEE80203D504212046FFF72EFE9D +:102CA000A90203D505212046FFF728FE6A0203D5B5 +:102CB00006212046FFF722FE2B0203D507212046DE +:102CC000FFF71CFEEF0103D508212046FFF716FE93 +:102CD000700340F1A780E90703D500212046FFF7E4 +:102CE0009FFEAA0703D501212046FFF799FE6B0737 +:102CF00003D502212046FFF793FE2F0703D50321BA +:102D00002046FFF78DFEEE0603D504212046FFF78F +:102D100087FEA80603D505212046FFF781FE690638 +:102D200003D506212046FFF77BFE2A0603D507219F +:102D30002046FFF775FEEB0574D520460821BDE857 +:102D4000F84FFFF76DBED4F890904FF0000B4FF0A6 +:102D5000010AD4F894305FFA8BF79B689F423FF6E4 +:102D600038AF09EB4713D3F8002902F44022B2F53B +:102D7000802F20D1D3F80029002A1CDAD3F80029AB +:102D800042F09042C3F80029D3F80029002AFBDB67 +:102D90003946D4F89000FFF787FB22890AFA07F337 +:102DA00022EA0303238104EB8703DB689B6813B1EA +:102DB0003946204698470BF1010BCAE7910701D12C +:102DC000D0F80080072A02F101029CBF03F8018BB2 +:102DD0004FEA18283FE704EB830300F58050DA68D8 +:102DE000D2F818C0DCF80820DCE9001CA1EB0C0CC0 +:102DF00000218F4208D1DB689B699A683A449A6047 +:102E00005A683A445A6029E711F0030F01D1D0F80B +:102E100000808C4501F1010184BF02F8018B4FEA6B +:102E20001828E6E7BDE8F88F08B50348FFF774FEF9 +:102E3000BDE8084000F07ABB6441002008B50348B3 +:102E4000FFF76AFEBDE8084000F070BB00420020BA +:102E5000D0F8903003EB4111D1F8003B43F400135C +:102E6000C1F8003B70470000D0F8903003EB4111EF +:102E7000D1F8003943F40013C1F80039704700005D +:102E8000D0F8903003EB4111D1F8003B23F400134C +:102E9000C1F8003B70470000D0F8903003EB4111BF +:102EA000D1F8003923F40013C1F80039704700004D +:102EB000090100F16043012203F56143C9B283F8BF +:102EC000001300F01F039A4043099B0003F1604385 +:102ED00003F56143C3F880211A60704730B50433AD +:102EE000039C0172002104FB0325C160C0E9065365 +:102EF000049B0363059BC0E90000C0E90422C0E90C +:102F00000842C0E90A11436330BD00000022416A53 +:102F1000C260C0E90411C0E90A226FF00101FEF7A6 +:102F2000ADBD0000D0E90432934201D1C2680AB9B4 +:102F3000181D704700207047036919600021C2689E +:102F40000132C260C269134482699342036124BFA3 +:102F5000436A0361FEF786BD38B504460D46E36853 +:102F60003BB162690020131D1268A3621344E3623F +:102F700007E0237A33B929462046FEF763FD00288F +:102F8000EDDA38BD6FF00100FBE70000C368C269ED +:102F9000013BC3604369134482699342436124BF88 +:102FA000436A436100238362036B03B11847704790 +:102FB00070B53023044683F31188866A3EB9FFF763 +:102FC000CBFF054618B186F31188284670BDA36A69 +:102FD000E26A13F8015B9342A36202D32046FFF733 +:102FE000D5FF002383F31188EFE700002DE9F84FA8 +:102FF00004460E46174698464FF0300989F311886B +:103000000025AA46D4F828B0BBF1000F09D14146EB +:103010002046FFF7A1FF20B18BF311882846BDE8B9 +:10302000F88FD4E90A12A7EB050B521A934528BF73 +:103030009346BBF1400F1BD9334601F1400251F8D2 +:10304000040B914243F8040BF9D1A36A4036403592 +:103050004033A362D4E90A239A4202D32046FFF701 +:1030600095FF8AF31188BD42D8D289F31188C9E748 +:1030700030465A46FDF7F4FEA36A5E445D445B4465 +:10308000A362E7E710B5029C0433017203FB04213D +:10309000C460C0E906130023C0E90A33039B03633D +:1030A000049BC0E90000C0E90422C0E90842436370 +:1030B00010BD0000026A6FF00101C260426AC0E9FF +:1030C00004220022C0E90A22FEF7D8BCD0E904237A +:1030D0009A4201D1C26822B9184650F8043B0B60ED +:1030E000704700231846FAE7C3680021C26901331C +:1030F000C3604369134482699342436124BF436AB6 +:103100004361FEF7AFBC000038B504460D46E368E6 +:103110003BB1236900201A1DA262E2691344E362F5 +:1031200007E0237A33B929462046FEF78BFC0028B6 +:10313000EDDA38BD6FF00100FBE7000003691960AC +:10314000C268013AC260C269134482699342036152 +:1031500024BF436A036100238362036B03B11847F2 +:103160007047000070B530230D460446114683F3C6 +:103170001188866A2EB9FFF7C7FF10B186F3118850 +:1031800070BDA36A1D70A36AE26A01339342A36211 +:1031900004D3E16920460439FFF7D0FF002080F313 +:1031A0001188EDE72DE9F84F04460D469046994603 +:1031B0004FF0300A8AF311880026B346A76A4FB948 +:1031C00049462046FFF7A0FF20B187F3118830461B +:1031D000BDE8F88FD4E90A073A1AA8EB0607974228 +:1031E00028BF1746402F1BD905F1400355F8042B83 +:1031F0009D4240F8042BF9D1A36A40364033A362C4 +:10320000D4E90A239A4204D3E16920460439FFF73E +:1032100095FF8BF311884645D9D28AF31188CDE703 +:1032200029463A46FDF71CFEA36A3D443E443B4412 +:10323000A362E5E7D0E904239A4217D1C3689BB1A2 +:10324000836A8BB1043B9B1A0ED01360C368013BA9 +:10325000C360C3691A4483699A42026124BF436A06 +:103260000361002383620123184670470023FBE7B4 +:1032700000F086B9014B586A704700BF000C00404F +:10328000034B002258631A610222DA60704700BFC4 +:10329000000C0040014B0022DA607047000C004037 +:1032A000014B5863704700BF000C0040FEE7000070 +:1032B00070B51B4B0025044686B058600E468562EB +:1032C000016300F00BF904F11003A560E562C4E9A5 +:1032D00004334FF0FF33C4E90044C4E90635FFF777 +:1032E000C9FF2B46024604F134012046C4E90823F5 +:1032F00080230C4A2565FEF75BFB01230A4AE06048 +:1033000000920375684672680192B268CDE90223A3 +:10331000064BCDE90435FEF773FB06B070BD00BF68 +:1033200020260020F83C0008FD3C0008AD320008D3 +:10333000024AD36A1843D062704700BFB823002006 +:103340004B6843608B688360CB68C3600B694361E3 +:103350004B6903628B6943620B680360704700002E +:1033600008B53C4B40F2FF713B48D3F888200A4334 +:10337000C3F88820D3F8882022F4FF6222F00702E5 +:10338000C3F88820D3F88820D3F8E0200A43C3F894 +:10339000E020D3F808210A43C3F808212F4AD3F8C4 +:1033A00008311146FFF7CCFF00F5806002F11C01E7 +:1033B000FFF7C6FF00F5806002F13801FFF7C0FF9C +:1033C00000F5806002F15401FFF7BAFF00F580605C +:1033D00002F17001FFF7B4FF00F5806002F18C018B +:1033E000FFF7AEFF00F5806002F1A801FFF7A8FF2C +:1033F00000F5806002F1C401FFF7A2FF00F58060D4 +:1034000002F1E001FFF79CFF00F5806002F1FC0192 +:10341000FFF796FF02F58C7100F58060FFF790FFD3 +:1034200000F076F90E4BD3F8902242F00102C3F877 +:103430009022D3F8942242F00102C3F8942205228C +:10344000C3F898204FF06052C3F89C20054AC3F897 +:10345000A02008BD0044025800000258043D0008A6 +:1034600000ED00E01F00080308B500F025FBFEF7A3 +:103470007DFA0F4BD3F8DC2042F04002C3F8DC2089 +:10348000D3F8042122F04002C3F80421D3F8043118 +:10349000084B1A6842F008021A601A6842F00402E7 +:1034A0001A60FEF721FFBDE80840FEF7D3BC00BF5D +:1034B000004402580018024870470000EFF30983E7 +:1034C000054968334A6B22F001024A6383F3098895 +:1034D000002383F31188704700EF00E0302080F371 +:1034E000118862B60D4B0E4AD96821F4E0610904D7 +:1034F000090C0A430B49DA60D3F8FC2042F08072D1 +:10350000C3F8FC20084AC2F8B01F116841F001015D +:1035100011602022DA7783F82200704700ED00E086 +:103520000003FA0555CEACC5001000E0302310B5FD +:1035300083F311880E4B5B6813F4006314D0F1EE33 +:10354000103AEFF309844FF08073683CE361094B54 +:10355000DB6B236684F30988FEF7E8F910B1064BAC +:10356000A36110BD054BFBE783F31188F9E700BFAA +:1035700000ED00E000EF00E02F0400083204000836 +:10358000114BD3F8E82042F00802C3F8E820D3F842 +:10359000102142F00802C3F810210C4AD3F8103170 +:1035A000D36B43F00803D363C722094B9A624FF0F1 +:1035B000FF32DA6200229A615A63DA605A600122AD +:1035C0005A611A60704700BF004402580010005C46 +:1035D000000C0040094A08B51169D3680B40D9B204 +:1035E0009B076FEA0101116107D5302383F311882E +:1035F000FEF7D2F9002383F3118808BD000C0040C8 +:10360000064BD3F8DC200243C3F8DC20D3F80421B6 +:103610001043C3F80401D3F8043170470044025842 +:103620003A4B4FF0FF31D3F8802062F00042C3F8EC +:103630008020D3F8802002F00042C3F88020D3F825 +:103640008020D3F88420C3F88410D3F8842000228B +:10365000C3F88420D3F88400D86F40F0FF4040F4D2 +:10366000FF0040F4DF4040F07F00D867D86F20F0C3 +:10367000FF4020F4FF0020F4DF4020F07F00D867F7 +:10368000D86FD3F888006FEA40506FEA5050C3F803 +:103690008800D3F88800C0F30A00C3F88800D3F884 +:1036A0008800D3F89000C3F89010D3F89000C3F8C6 +:1036B0009020D3F89000D3F89400C3F89410D3F876 +:1036C0009400C3F89420D3F89400D3F89800C3F87A +:1036D0009810D3F89800C3F89820D3F89800D3F83E +:1036E0008C00C3F88C10D3F88C00C3F88C20D3F86E +:1036F0008C00D3F89C00C3F89C10D3F89C10C3F83E +:103700009C20D3F89C3000F0AFB900BF00440258B1 +:1037100008B50122534BC3F80821534BD3F8F420CA +:1037200042F00202C3F8F420D3F81C2142F0020256 +:10373000C3F81C210222D3F81C314C4BDA605A68C2 +:103740009104FCD54A4A1A6001229A60494ADA601B +:1037500000221A614FF440429A61444B9A699204E4 +:10376000FCD51A6842F480721A603F4B1A6F12F44B +:10377000407F04D04FF480321A6700221A671A681B +:1037800042F001021A60384B1A685007FCD500223B +:103790001A611A6912F03802FBD1012119604FF049 +:1037A000804159605A67344ADA62344A1A611A68A9 +:1037B00042F480321A602C4B1A689103FCD51A68C7 +:1037C00042F480521A601A689204FCD52C4A2D49A2 +:1037D0009A6200225A63196301F57C01DA6301F2EF +:1037E000E71199635A64284A1A64284ADA621A6807 +:1037F00042F0A8521A601C4B1A6802F02852B2F12B +:10380000285FF9D148229A614FF48862DA61402238 +:103810001A621F4ADA641F4A1A651F4A5A651F4A0C +:103820009A6532231E4A1360136803F00F03022BBC +:10383000FAD10D4A136943F003031361136903F0CE +:103840003803182BFAD14FF00050FFF7D9FE4FF094 +:103850008040FFF7D5FE4FF00040BDE80840FFF77D +:10386000CFBE00BF008000510044025800480258FB +:1038700000C000F0020000010000FF010088900875 +:103880001210200063020901470E0508DD0BBF017D +:1038900020000020000001100910E00000010110CC +:1038A000002000524FF0B04208B5D2F8883003F043 +:1038B0000103C2F8883023B1044A13680BB1506881 +:1038C0009847BDE80840FFF731BE00BFB442002072 +:1038D0004FF0B04208B5D2F8883003F00203C2F8C6 +:1038E000883023B1044A93680BB1D0689847BDE88B +:1038F0000840FFF71BBE00BFB44200204FF0B042AB +:1039000008B5D2F8883003F00403C2F8883023B138 +:10391000044A13690BB150699847BDE80840FFF7A6 +:1039200005BE00BFB44200204FF0B04208B5D2F847 +:10393000883003F00803C2F8883023B1044A936941 +:103940000BB1D0699847BDE80840FFF7EFBD00BF55 +:10395000B44200204FF0B04208B5D2F8883003F0EE +:103960001003C2F8883023B1044A136A0BB1506ABD +:103970009847BDE80840FFF7D9BD00BFB44200201A +:103980004FF0B04310B5D3F8884004F47872C3F810 +:103990008820A30604D5124A936A0BB1D06A9847CF +:1039A000600604D50E4A136B0BB1506B9847210685 +:1039B00004D50B4A936B0BB1D06B9847E20504D545 +:1039C000074A136C0BB1506C9847A30504D5044A01 +:1039D000936C0BB1D06C9847BDE81040FFF7A6BDC3 +:1039E000B44200204FF0B04310B5D3F8884004F43F +:1039F0007C42C3F88820620504D5164A136D0BB1CA +:103A0000506D9847230504D5124A936D0BB1D06DC4 +:103A10009847E00404D50F4A136E0BB1506E9847D7 +:103A2000A10404D50B4A936E0BB1D06E9847620483 +:103A300004D5084A136F0BB1506F9847230404D57F +:103A4000044A936F0BB1D06F9847BDE81040FFF761 +:103A50006DBD00BFB442002008B5FFF7BBFDBDE857 +:103A60000840FFF763BD0000062108B50846FFF7D0 +:103A70001FFA06210720FFF71BFA06210820FFF78F +:103A800017FA06210920FFF713FA06210A20FFF78B +:103A90000FFA06211720FFF70BFA06212820FFF75F +:103AA00007FA09217A20FFF703FA07213220BDE83F +:103AB0000840FFF7FDB9000008B5FFF7B1FD00F0C1 +:103AC0000BF8FDF731FCFDF703FBFFF7F5FCBDE854 +:103AD0000840FFF7CDBB00000023054A194601331B +:103AE000102BC2E9001102F10802F8D1704700BFA3 +:103AF000B442002010B501390244904201D10020A7 +:103B000005E0037811F8014FA34201D0181B10BD46 +:103B10000130F2E7034611F8012B03F8012B002ACC +:103B2000F9D1704753544D333248373F3F3F00532C +:103B3000544D3332483733782F3732780053544D51 +:103B40003332483734332F3735332F373530000091 +:103B500001105A000310590001205800032056009C +:103B6000000000004D100008391000087510000812 +:103B7000611000086D100008591000084510000879 +:103B80003110000881100008000000000100000052 +:103B90000000000063300000943B00081024002067 +:103BA000202600204172647550696C6F74002542B4 +:103BB0004F415244252D424C002553455249414C1A +:103BC0002500000002000000000000006D12000847 +:103BD000DD12000840004000D03E0020E03E002002 +:103BE00002000000000000000300000000000000D0 +:103BF000251300080000000010000000F03E002027 +:103C000000000000010000000000000064410020EE +:103C100001010200F11D0008011D00089D1D0008A2 +:103C2000811D0008430000002C3C000809024300ED +:103C3000020100C032090400000102020100052453 +:103C400000100105240100010424020205240600DD +:103C500001070582030800FF09040100020A0000B1 +:103C60000007050102400000070581024000000036 +:103C700012000000783C0008120110010200004010 +:103C80000912415700020102030100000403090464 +:103C900025424F4152442500416F636F64612D52AC +:103CA000432D483734334475616C0030313233343E +:103CB000353637383941424344454600000000005C +:103CC0008114000839170008E5170008400040007B +:103CD0009C4200209C42002001000000AC420020D9 +:103CE000800000004001000008000000000100000A +:103CF00000100000080000006D61696E0069646CCE +:103D0000650000000000802A00000000AAAAAAAAFC +:103D100000000024FFFF00000000000000A00A00D7 +:103D20000000000100000000AAAAAAAA00000001E9 +:103D3000FFFF000000000000000000000000004045 +:103D400000000000AAAAAAAA00000040FFFF00008D +:103D50000000000000000000400000000000000023 +:103D6000AAAAAAAA40000000FFFF0000000000006D +:103D7000000000004000400000000000AAAAAAAA1B +:103D800000004000F7FF00000000000000000000FD +:103D90000000000000000000AAAAAAAA000000007B +:103DA000FFFF000000000000000000000000000015 +:103DB00000000000AAAAAAAA00000000FFFF00005D +:103DC00000000000000000000000000000000000F3 +:103DD000AAAAAAAA00000000FFFF0000000000003D +:103DE000000000000000000000000000AAAAAAAA2B +:103DF00000000000FFFF00000000000000000000C5 +:103E00000000000000000000AAAAAAAA000000000A +:103E1000FFFF0000000000000000000000000000A4 +:103E200000000000AAAAAAAA00000000FFFF0000EC +:103E300000000000000000005A1400000000000014 +:103E400000001A0000000000FF0000000000000059 +:103E5000243B0008830400002F3B000850040000AE +:103E60003D3B00080096000000000800960000009E +:103E700000080000040000008C3C00080000000066 +:103E80000000000000000000000000000000000032 +:043E9000000000002E +:00000001FF diff --git a/Tools/scripts/uploader.py b/Tools/scripts/uploader.py index ee13ed62571091..a4872f2844ff70 100755 --- a/Tools/scripts/uploader.py +++ b/Tools/scripts/uploader.py @@ -1007,7 +1007,7 @@ def ports_to_try(args): if "linux" in _platform or "darwin" in _platform or "cygwin" in _platform: import glob for pattern in patterns: - portlist += glob.glob(pattern) + portlist += sorted(glob.glob(pattern)) else: portlist = patterns diff --git a/libraries/APM_Control/AR_PosControl.cpp b/libraries/APM_Control/AR_PosControl.cpp index 9cf414831acf7e..4107082558054b 100644 --- a/libraries/APM_Control/AR_PosControl.cpp +++ b/libraries/APM_Control/AR_PosControl.cpp @@ -56,7 +56,7 @@ const AP_Param::GroupInfo AR_PosControl::var_info[] = { // @Param: _VEL_I // @DisplayName: Velocity (horizontal) I gain // @Description: Velocity (horizontal) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration - // @Range: 0.02 1.00 + // @Range: 0.00 1.00 // @Increment: 0.01 // @User: Advanced @@ -141,9 +141,11 @@ void AR_PosControl::update(float dt) } // calculation velocity error + bool stopping = false; if (_vel_desired_valid) { // add target velocity to desired velocity from position error _vel_target += _vel_desired; + stopping = _vel_desired.is_zero(); } // limit velocity to maximum speed @@ -192,19 +194,21 @@ void AR_PosControl::update(float dt) const Vector2f vel_target_FR = AP::ahrs().earth_to_body2D(_vel_target); // desired speed is normally the forward component (only) of the target velocity - // but we do not let it fall below the minimum turn speed unless the vehicle is slowing down - const float abs_des_speed_min = MIN(_vel_target.length(), turn_speed_min); - float des_speed; - if (_reversed != backing_up) { - // if reversed or backing up desired speed will be negative - des_speed = MIN(-abs_des_speed_min, vel_target_FR.x); - } else { - des_speed = MAX(abs_des_speed_min, vel_target_FR.x); + float des_speed = vel_target_FR.x; + if (!stopping) { + // do not let target speed fall below the minimum turn speed unless the vehicle is slowing down + const float abs_des_speed_min = MIN(_vel_target.length(), turn_speed_min); + if (_reversed != backing_up) { + // if reversed or backing up desired speed will be negative + des_speed = MIN(-abs_des_speed_min, vel_target_FR.x); + } else { + des_speed = MAX(abs_des_speed_min, vel_target_FR.x); + } } _desired_speed = _atc.get_desired_speed_accel_limited(des_speed, dt); // calculate turn rate from desired lateral acceleration - _desired_lat_accel = accel_target_FR.y; + _desired_lat_accel = stopping ? 0 : accel_target_FR.y; _desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(_desired_lat_accel, _desired_speed); } diff --git a/libraries/AP_AHRS/AP_AHRS_config.h b/libraries/AP_AHRS/AP_AHRS_config.h index 0be9f0cdd828bd..559d0403b0566e 100644 --- a/libraries/AP_AHRS/AP_AHRS_config.h +++ b/libraries/AP_AHRS/AP_AHRS_config.h @@ -16,8 +16,8 @@ #endif #ifndef HAL_NAVEKF2_AVAILABLE -// only default to EK2 enabled on boards with over 1M flash -#define HAL_NAVEKF2_AVAILABLE (BOARD_FLASH_SIZE>1024) +// EKF2 slated compiled out by default in 4.5, slated to be removed. +#define HAL_NAVEKF2_AVAILABLE 0 #endif #ifndef HAL_NAVEKF3_AVAILABLE diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp index 281831f0d18390..123287c89a92ce 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp @@ -137,7 +137,8 @@ void AP_BattMonitor_DroneCAN::update_interim_state(const float voltage, const fl const uint32_t tnow = AP_HAL::micros(); - if (!_has_battery_info_aux || _mppt.is_detected) { + if (!_has_battery_info_aux || + !use_CAN_SoC()) { const uint32_t dt_us = tnow - _interim_state.last_time_micros; // update total current drawn since startup diff --git a/libraries/AP_CANManager/AP_CANDriver.cpp b/libraries/AP_CANManager/AP_CANDriver.cpp index 1f279c12a690de..0ec7ebbceb2a00 100644 --- a/libraries/AP_CANManager/AP_CANDriver.cpp +++ b/libraries/AP_CANManager/AP_CANDriver.cpp @@ -51,6 +51,14 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = { AP_SUBGROUPPTR(_piccolocan, "PC_", 5, AP_CANManager::CANDriver_Params, AP_PiccoloCAN), #endif + // @Param: PROTOCOL2 + // @DisplayName: Secondary protocol with 11 bit CAN addressing + // @Description: Secondary protocol with 11 bit CAN addressing + // @Values: 0:Disabled,7:USD1,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar_NRA24 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("PROTOCOL2", 6, AP_CANManager::CANDriver_Params, _driver_type_11bit, float(AP_CAN::Protocol::None)), + AP_GROUPEND }; #endif diff --git a/libraries/AP_CANManager/AP_CANDriver.h b/libraries/AP_CANManager/AP_CANDriver.h index a950535019f16c..f93e96cec0e7a8 100644 --- a/libraries/AP_CANManager/AP_CANDriver.h +++ b/libraries/AP_CANManager/AP_CANDriver.h @@ -21,6 +21,8 @@ #include class AP_CANManager; +class CANSensor; + class AP_CANDriver { public: @@ -34,4 +36,9 @@ class AP_CANDriver // link protocol drivers with interfaces by adding reference to CANIface virtual bool add_interface(AP_HAL::CANIface* can_iface) = 0; + // add an 11 bit auxillary driver + virtual bool add_11bit_driver(CANSensor *sensor) { return false; } + + // handler for outgoing frames for auxillary drivers + virtual bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) { return false; } }; diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp index 78be64a97c66c9..20918ede682a67 100644 --- a/libraries/AP_CANManager/AP_CANManager.cpp +++ b/libraries/AP_CANManager/AP_CANManager.cpp @@ -119,6 +119,9 @@ void AP_CANManager::init() { WITH_SEMAPHORE(_sem); + // we need to mutate the HAL to install new CAN interfaces + AP_HAL::HAL& hal_mutable = AP_HAL::get_HAL_mutable(); + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (AP::sitl() == nullptr) { AP_HAL::panic("CANManager: SITL not initialised!"); @@ -146,17 +149,17 @@ void AP_CANManager::init() } drv_num--; - if (hal.can[i] == nullptr) { + if (hal_mutable.can[i] == nullptr) { // So if this interface is not allocated allocate it here, // also pass the index of the CANBus - const_cast (hal).can[i] = new HAL_CANIface(i); + hal_mutable.can[i] = new HAL_CANIface(i); } // Initialise the interface we just allocated - if (hal.can[i] == nullptr) { + if (hal_mutable.can[i] == nullptr) { continue; } - AP_HAL::CANIface* iface = hal.can[i]; + AP_HAL::CANIface* iface = hal_mutable.can[i]; // Find the driver type that we need to allocate and register this interface with drv_type[drv_num] = (AP_CAN::Protocol) _drv_param[drv_num]._driver_type.get(); @@ -166,13 +169,13 @@ void AP_CANManager::init() #if AP_CAN_SLCAN_ENABLED if (_slcan_interface.init_passthrough(i)) { // we have slcan bridge setup pass that on as can iface - can_initialised = hal.can[i]->init(_interfaces[i]._bitrate, _interfaces[i]._fdbitrate*1000000, AP_HAL::CANIface::NormalMode); + can_initialised = hal_mutable.can[i]->init(_interfaces[i]._bitrate, _interfaces[i]._fdbitrate*1000000, AP_HAL::CANIface::NormalMode); iface = &_slcan_interface; } else { #else if (true) { #endif - can_initialised = hal.can[i]->init(_interfaces[i]._bitrate, _interfaces[i]._fdbitrate*1000000, AP_HAL::CANIface::NormalMode); + can_initialised = hal_mutable.can[i]->init(_interfaces[i]._bitrate, _interfaces[i]._fdbitrate*1000000, AP_HAL::CANIface::NormalMode); } if (!can_initialised) { @@ -245,8 +248,8 @@ void AP_CANManager::init() bool enable_filter = false; for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { if (_interfaces[i]._driver_number == (drv_num+1) && - hal.can[i] != nullptr && - hal.can[i]->get_operating_mode() == AP_HAL::CANIface::FilteredMode) { + hal_mutable.can[i] != nullptr && + hal_mutable.can[i]->get_operating_mode() == AP_HAL::CANIface::FilteredMode) { // Don't worry we don't enable Filters for Normal Ifaces under the driver // this is just to ensure we enable them for the ones we already decided on enable_filter = true; @@ -277,6 +280,7 @@ void AP_CANManager::init() } } #endif + /* register a new CAN driver */ @@ -284,6 +288,9 @@ bool AP_CANManager::register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver { WITH_SEMAPHORE(_sem); + // we need to mutate the HAL to install new CAN interfaces + AP_HAL::HAL& hal_mutable = AP_HAL::get_HAL_mutable(); + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { uint8_t drv_num = _interfaces[i]._driver_number; if (drv_num == 0 || drv_num > HAL_MAX_CAN_PROTOCOL_DRIVERS) { @@ -302,17 +309,17 @@ bool AP_CANManager::register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver continue; } - if (hal.can[i] == nullptr) { + if (hal_mutable.can[i] == nullptr) { // if this interface is not allocated allocate it here, // also pass the index of the CANBus - const_cast (hal).can[i] = new HAL_CANIface(i); + hal_mutable.can[i] = new HAL_CANIface(i); } // Initialise the interface we just allocated - if (hal.can[i] == nullptr) { + if (hal_mutable.can[i] == nullptr) { continue; } - AP_HAL::CANIface* iface = hal.can[i]; + AP_HAL::CANIface* iface = hal_mutable.can[i]; _drivers[drv_num] = driver; _drivers[drv_num]->add_interface(iface); @@ -328,6 +335,32 @@ bool AP_CANManager::register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver return false; } +// register a new auxillary sensor driver for 11 bit address frames +bool AP_CANManager::register_11bit_driver(AP_CAN::Protocol dtype, CANSensor *sensor, uint8_t &driver_index) +{ + WITH_SEMAPHORE(_sem); + + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { + uint8_t drv_num = _interfaces[i]._driver_number; + if (drv_num == 0 || drv_num > HAL_MAX_CAN_PROTOCOL_DRIVERS) { + continue; + } + // from 1 based to 0 based + drv_num--; + + if (dtype != (AP_CAN::Protocol)_drv_param[drv_num]._driver_type_11bit.get()) { + continue; + } + if (_drivers[drv_num] != nullptr && + _drivers[drv_num]->add_11bit_driver(sensor)) { + driver_index = drv_num; + return true; + } + } + return false; + +} + // Method used by CAN related library methods to report status and debug info // The result of this method can be accessed via ftp get @SYS/can_log.txt void AP_CANManager::log_text(AP_CANManager::LogLevel loglevel, const char *tag, const char *fmt, ...) diff --git a/libraries/AP_CANManager/AP_CANManager.h b/libraries/AP_CANManager/AP_CANManager.h index 85d4aa31ed31f2..5c352ae5b39231 100644 --- a/libraries/AP_CANManager/AP_CANManager.h +++ b/libraries/AP_CANManager/AP_CANManager.h @@ -34,6 +34,8 @@ #include "AP_CAN.h" +class CANSensor; + class AP_CANManager { public: @@ -63,6 +65,9 @@ class AP_CANManager // register a new driver bool register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver); + // register a new auxillary sensor driver for 11 bit address frames + bool register_11bit_driver(AP_CAN::Protocol dtype, CANSensor *sensor, uint8_t &driver_index); + // returns number of active CAN Drivers uint8_t get_num_drivers(void) const { @@ -141,6 +146,7 @@ class AP_CANManager private: AP_Int8 _driver_type; + AP_Int8 _driver_type_11bit; AP_CANDriver* _uavcan; AP_CANDriver* _piccolocan; }; diff --git a/libraries/AP_CANManager/AP_CANSensor.cpp b/libraries/AP_CANManager/AP_CANSensor.cpp index 881eb39ce93c8f..025dfd8036e2f8 100644 --- a/libraries/AP_CANManager/AP_CANSensor.cpp +++ b/libraries/AP_CANManager/AP_CANSensor.cpp @@ -40,7 +40,13 @@ void CANSensor::register_driver(AP_CAN::Protocol dtype) { #if HAL_CANMANAGER_ENABLED if (!AP::can().register_driver(dtype, this)) { - debug_can(AP_CANManager::LOG_ERROR, "Failed to register CANSensor %s", _driver_name); + if (AP::can().register_11bit_driver(dtype, this, _driver_index)) { + is_aux_11bit_driver = true; + _can_driver = AP::can().get_driver(_driver_index); + _initialized = true; + } else { + debug_can(AP_CANManager::LOG_ERROR, "Failed to register CANSensor %s", _driver_name); + } } else { debug_can(AP_CANManager::LOG_INFO, "%s: constructed", _driver_name); } @@ -135,6 +141,10 @@ bool CANSensor::write_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_ return false; } + if (is_aux_11bit_driver && _can_driver != nullptr) { + return _can_driver->write_aux_frame(out_frame, timeout_us); + } + bool read_select = false; bool write_select = true; bool ret = _can_iface->select(read_select, write_select, &out_frame, AP_HAL::micros64() + timeout_us); diff --git a/libraries/AP_CANManager/AP_CANSensor.h b/libraries/AP_CANManager/AP_CANSensor.h index a1374821e40e4e..e340b71facdd09 100644 --- a/libraries/AP_CANManager/AP_CANSensor.h +++ b/libraries/AP_CANManager/AP_CANSensor.h @@ -72,6 +72,10 @@ class CANSensor : public AP_CANDriver { const uint16_t _stack_size; bool _initialized; uint8_t _driver_index; + + // this is true when we are setup as an auxillary driver using CAN_Dn_PROTOCOL2 + bool is_aux_11bit_driver; + AP_CANDriver *_can_driver; HAL_EventHandle _event_handle; AP_HAL::CANIface* _can_iface; diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.cpp b/libraries/AP_DroneCAN/AP_Canard_iface.cpp index 60980e510f8f9a..86bedca40e680d 100644 --- a/libraries/AP_DroneCAN/AP_Canard_iface.cpp +++ b/libraries/AP_DroneCAN/AP_Canard_iface.cpp @@ -9,6 +9,7 @@ extern const AP_HAL::HAL& hal; #define LOG_TAG "DroneCANIface" #include +#include #define DEBUG_PKTS 0 @@ -346,6 +347,15 @@ void CanardInterface::processRx() { if (ifaces[i]->receive(rxmsg, timestamp, flags) <= 0) { break; } + + if (!rxmsg.isExtended()) { + // 11 bit frame, see if we have a handler + if (aux_11bit_driver != nullptr) { + aux_11bit_driver->handle_frame(rxmsg); + } + continue; + } + rx_frame.data_len = AP_HAL::CANFrame::dlcToDataLength(rxmsg.dlc); memcpy(rx_frame.data, rxmsg.data, rx_frame.data_len); #if HAL_CANFD_SUPPORTED @@ -434,4 +444,29 @@ bool CanardInterface::add_interface(AP_HAL::CANIface *can_iface) num_ifaces++; return true; } + +// add an 11 bit auxillary driver +bool CanardInterface::add_11bit_driver(CANSensor *sensor) +{ + if (aux_11bit_driver != nullptr) { + // only allow one + return false; + } + aux_11bit_driver = sensor; + return true; +} + +// handler for outgoing frames for auxillary drivers +bool CanardInterface::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) +{ + bool ret = false; + for (uint8_t iface = 0; iface < num_ifaces; iface++) { + if (ifaces[iface] == NULL) { + continue; + } + ret |= ifaces[iface]->send(out_frame, timeout_us, 0) > 0; + } + return ret; +} + #endif // #if HAL_ENABLE_DRONECAN_DRIVERS diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.h b/libraries/AP_DroneCAN/AP_Canard_iface.h index aa7533e6913b3f..ce9b75ee923d79 100644 --- a/libraries/AP_DroneCAN/AP_Canard_iface.h +++ b/libraries/AP_DroneCAN/AP_Canard_iface.h @@ -5,6 +5,8 @@ #include class AP_DroneCAN; +class CANSensor; + class CanardInterface : public Canard::Interface { friend class AP_DroneCAN; public: @@ -48,6 +50,12 @@ class CanardInterface : public Canard::Interface { bool add_interface(AP_HAL::CANIface *can_drv); + // add an auxillary driver for 11 bit frames + bool add_11bit_driver(CANSensor *sensor); + + // handler for outgoing frames for auxillary drivers + bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us); + #if AP_TEST_DRONECAN_DRIVERS static CanardInterface& get_test_iface() { return test_iface; } static void processTestRx(); @@ -70,5 +78,8 @@ class CanardInterface : public Canard::Interface { HAL_Semaphore _sem_rx; CanardTxTransfer tx_transfer; dronecan_protocol_Stats protocol_stats; + + // auxillary 11 bit CANSensor + CANSensor *aux_11bit_driver; }; -#endif // HAL_ENABLE_DRONECAN_DRIVERS \ No newline at end of file +#endif // HAL_ENABLE_DRONECAN_DRIVERS diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 55d148bf1f90c0..9f34e7f93b10ef 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -1751,4 +1751,20 @@ void AP_DroneCAN::logging(void) #endif // HAL_LOGGING_ENABLED } +// add an 11 bit auxillary driver +bool AP_DroneCAN::add_11bit_driver(CANSensor *sensor) +{ + return canard_iface.add_11bit_driver(sensor); +} + +// handler for outgoing frames for auxillary drivers +bool AP_DroneCAN::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) +{ + if (out_frame.isExtended()) { + // don't allow extended frames to be sent by auxillary driver + return false; + } + return canard_iface.write_aux_frame(out_frame, timeout_us); +} + #endif // HAL_NUM_CAN_IFACES diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index f502e05a39f276..61f0528bc95ff5 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -69,6 +69,7 @@ // fwd-declare callback classes class AP_DroneCAN_DNA_Server; +class CANSensor; class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { friend class AP_DroneCAN_DNA_Server; @@ -85,6 +86,12 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { void init(uint8_t driver_index, bool enable_filters) override; bool add_interface(AP_HAL::CANIface* can_iface) override; + // add an 11 bit auxillary driver + bool add_11bit_driver(CANSensor *sensor) override; + + // handler for outgoing frames for auxillary drivers + bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) override; + uint8_t get_driver_index() const { return _driver_index; } // define string with length structure diff --git a/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp b/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp index 97a7850d41efdd..5745e94b761d04 100644 --- a/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp +++ b/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp @@ -117,15 +117,18 @@ static void cb_GetNodeInfoRequest(const CanardRxTransfer &transfer, const uavcan void DroneCAN_sniffer::init(void) { - const_cast (hal).can[driver_index] = new HAL_CANIface(driver_index); + // we need to mutate the HAL to install new CAN interfaces + AP_HAL::HAL& hal_mutable = AP_HAL::get_HAL_mutable(); + + hal_mutable.can[driver_index] = new HAL_CANIface(driver_index); - if (hal.can[driver_index] == nullptr) { + if (hal_mutable.can[driver_index] == nullptr) { AP_HAL::panic("Couldn't allocate CANManager, something is very wrong"); } - hal.can[driver_index]->init(1000000, AP_HAL::CANIface::NormalMode); + hal_mutable.can[driver_index]->init(1000000, AP_HAL::CANIface::NormalMode); - if (!hal.can[driver_index]->is_initialized()) { + if (!hal_mutable.can[driver_index]->is_initialized()) { debug_dronecan("Can not initialised\n"); return; } @@ -135,7 +138,7 @@ void DroneCAN_sniffer::init(void) return; } - if (!_uavcan_iface_mgr->add_interface(hal.can[driver_index])) { + if (!_uavcan_iface_mgr->add_interface(hal_mutable.can[driver_index])) { debug_dronecan("Failed to add iface"); return; } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h index b35d099c2ff1de..07436b7ae43888 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h @@ -32,16 +32,22 @@ class AP_ExternalAHRS_backend { // Get model/type name virtual const char* get_name() const = 0; - // accessors for AP_AHRS + // Accessors for AP_AHRS + + // If not healthy, none of the other API's can be trusted. + // Example: Serial cable is severed. virtual bool healthy(void) const = 0; + // The communication interface is up and the device has sent valid data. virtual bool initialised(void) const = 0; virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const = 0; virtual void get_filter_status(nav_filter_status &status) const {} virtual void send_status_report(class GCS_MAVLINK &link) const {} - // check for new data + // Check for new data. + // This is used when there's not a separate thread for EAHRS. + // This can also copy interim state protected by locking. virtual void update() = 0; - + protected: AP_ExternalAHRS::state_t &state; uint16_t get_rate(void) const; diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 0b984bf1df709e..b947175b0bf620 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -129,18 +129,16 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: _TYPE // @DisplayName: 1st GPS type // @Description: GPS type of 1st GPS - // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover,24:UnicoreNMEA,25:UnicoreMovingBaselineNMEA + // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover,24:UnicoreNMEA,25:UnicoreMovingBaselineNMEA,26:SBF-DualAntenna // @RebootRequired: True // @User: Advanced AP_GROUPINFO("_TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT), #if GPS_MAX_RECEIVERS > 1 // @Param: _TYPE2 + // @CopyFieldsFrom: GPS_TYPE // @DisplayName: 2nd GPS type // @Description: GPS type of 2nd GPS - // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover,24:UnicoreNMEA,25:UnicoreMovingBaselineNMEA - // @RebootRequired: True - // @User: Advanced AP_GROUPINFO("_TYPE2", 1, AP_GPS, _type[1], 0), #endif @@ -645,6 +643,7 @@ void AP_GPS::send_blob_start(uint8_t instance) switch (_type[instance]) { #if AP_GPS_SBF_ENABLED case GPS_TYPE_SBF: + case GPS_TYPE_SBF_DUAL_ANTENNA: #endif //AP_GPS_SBF_ENABLED #if AP_GPS_GSOF_ENABLED case GPS_TYPE_GSOF: @@ -806,6 +805,7 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance) #if AP_GPS_SBF_ENABLED // by default the sbf/trimble gps outputs no data on its port, until configured. case GPS_TYPE_SBF: + case GPS_TYPE_SBF_DUAL_ANTENNA: return new AP_GPS_SBF(*this, state[instance], _port[instance]); #endif //AP_GPS_SBF_ENABLED #if AP_GPS_NOVA_ENABLED diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 4b184dcde0a827..eedc1a0c5a43a4 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -131,6 +131,7 @@ class AP_GPS GPS_TYPE_UAVCAN_RTK_ROVER = 23, GPS_TYPE_UNICORE_NMEA = 24, GPS_TYPE_UNICORE_MOVINGBASE_NMEA = 25, + GPS_TYPE_SBF_DUAL_ANTENNA = 26, #if HAL_SIM_GPS_ENABLED GPS_TYPE_SITL = 100, #endif diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp index 9de37cc4b71f48..42616a6af88459 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.cpp +++ b/libraries/AP_GPS/AP_GPS_GSOF.cpp @@ -22,7 +22,8 @@ // param set GPS_TYPE 11 // GSOF // param set SERIAL3_PROTOCOL 5 // GPS // - +// Pure SITL usage +// param set SIM_GPS_TYPE 11 // GSOF #define ALLOW_DOUBLE_MATH_FUNCTIONS #include "AP_GPS.h" diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 3b1d8d70095c96..569d5b6910ec4f 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -68,7 +68,10 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state, // if we ever parse RTK observations it will always be of type NED, so set it once state.rtk_baseline_coords_type = RTK_BASELINE_COORDINATE_SYSTEM_NED; - if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw)) { + + // yaw available when option bit set or using dual antenna + if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw) || + (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA)) { state.gps_yaw_configured = true; } } @@ -92,9 +95,9 @@ AP_GPS_SBF::read(void) ret |= parse(temp); } + const uint32_t now = AP_HAL::millis(); if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE) { if (config_step != Config_State::Complete) { - uint32_t now = AP_HAL::millis(); if (now > _init_blob_time) { if (now > _config_last_ack_time + 2000) { const size_t port_enable_len = strlen(_port_enable); @@ -116,9 +119,20 @@ AP_GPS_SBF::read(void) } break; case Config_State::SSO: - if (asprintf(&config_string, "sso,Stream%d,COM%d,PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic+BaseVectorGeod,msec100\n", + const char *extra_config; + switch (get_type()) { + case AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA: + extra_config = "+AttCovEuler+AuxAntPositions"; + break; + case AP_GPS::GPS_Type::GPS_TYPE_SBF: + default: + extra_config = ""; + break; + } + if (asprintf(&config_string, "sso,Stream%d,COM%d,PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic+BaseVectorGeod%s,msec100\n", (int)GPS_SBF_STREAM_NUMBER, - (int)gps._com_port[state.instance]) == -1) { + (int)gps._com_port[state.instance], + extra_config) == -1) { config_string = nullptr; } break; @@ -145,6 +159,17 @@ AP_GPS_SBF::read(void) break; } break; + case Config_State::SGA: + { + const char *targetGA = "none"; + if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) { + targetGA = "MultiAntenna"; + } + if (asprintf(&config_string, "sga, %s\n", targetGA)) { + config_string = nullptr; + } + break; + } case Config_State::Complete: // should never reach here, why search for a config if we have fully configured already INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); @@ -168,7 +193,6 @@ AP_GPS_SBF::read(void) } else if (_has_been_armed && (RxState & SBF_DISK_MOUNTED)) { // since init is done at this point and unmounting should be rate limited, // take over the _init_blob_time variable - uint32_t now = AP_HAL::millis(); if (now > _init_blob_time) { unmount_disk(); _init_blob_time = now + 1000; @@ -177,6 +201,12 @@ AP_GPS_SBF::read(void) } } + // yaw timeout after 300 milliseconds + if ((now - state.gps_yaw_time_ms) > 300) { + state.have_gps_yaw = false; + state.have_gps_yaw_accuracy = false; + } + return ret; } @@ -345,9 +375,12 @@ AP_GPS_SBF::parse(uint8_t temp) _init_blob_index++; if ((gps._sbas_mode == AP_GPS::SBAS_Mode::Disabled) ||_init_blob_index >= ARRAY_SIZE(sbas_on_blob)) { - config_step = Config_State::Complete; + config_step = Config_State::SGA; } break; + case Config_State::SGA: + config_step = Config_State::Complete; + break; case Config_State::Complete: // should never reach here, this implies that we validated a config string when we hadn't sent any INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); @@ -500,6 +533,51 @@ AP_GPS_SBF::process_message(void) } break; } + case AttEulerCov: + { + // yaw accuracy is taken from this message even though we actually calculate the yaw ourself (see AuxAntPositions below) + // this is OK based on the assumption that the calculation methods are similar and that inaccuracy arises from the sensor readings + if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) { + const msg5939 &temp = sbf_msg.data.msg5939u; + + check_new_itow(temp.TOW, sbf_msg.length); + + constexpr double floatDNU = -2e-10f; + constexpr uint8_t errorBits = 0x8F; // Bits 0-1 are aux 1 baseline + // Bits 2-3 are aux 2 baseline + // Bit 7 is attitude not requested +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" // suppress -Wfloat-equal as it's false positive when testing for DNU values + if (((temp.Error & errorBits) == 0) + && (temp.Cov_HeadHead != floatDNU)) { +#pragma GCC diagnostic pop + state.gps_yaw_accuracy = sqrtf(temp.Cov_HeadHead); + state.have_gps_yaw_accuracy = true; + } else { + state.gps_yaw_accuracy = false; + } + } + break; + } + case AuxAntPositions: + { +#if GPS_MOVING_BASELINE + if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) { + // calculate yaw using reported antenna positions in earth-frame + // note that this calculation does not correct for the vehicle's roll and pitch meaning it is inaccurate at very high lean angles + const msg5942 &temp = sbf_msg.data.msg5942u; + check_new_itow(temp.TOW, sbf_msg.length); + if (temp.N > 0 && temp.ant1.Error == 0 && temp.ant1.AmbiguityType == 0) { + // valid RTK integer fix + const float rel_heading_deg = degrees(atan2f(temp.ant1.DeltaEast, temp.ant1.DeltaNorth)); + calculate_moving_base_yaw(rel_heading_deg, + Vector3f(temp.ant1.DeltaNorth, temp.ant1.DeltaEast, temp.ant1.DeltaUp).length(), + -temp.ant1.DeltaUp); + } + } +#endif + break; + } case BaseVectorGeod: { #pragma GCC diagnostic push @@ -542,7 +620,7 @@ AP_GPS_SBF::process_message(void) } #endif // GPS_MOVING_BASELINE - } else { + } else if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw)) { state.rtk_baseline_y_mm = 0; state.rtk_baseline_x_mm = 0; state.rtk_baseline_z_mm = 0; diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index 2d8d47baafc500..e8773cf42cdefc 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -75,6 +75,7 @@ class AP_GPS_SBF : public AP_GPS_Backend SSO, Blob, SBAS, + SGA, Complete }; Config_State config_step; @@ -111,7 +112,9 @@ class AP_GPS_SBF : public AP_GPS_Backend PVTGeodetic = 4007, ReceiverStatus = 4014, BaseVectorGeod = 4028, - VelCovGeodetic = 5908 + VelCovGeodetic = 5908, + AttEulerCov = 5939, + AuxAntPositions = 5942, }; struct PACKED msg4007 // PVTGeodetic @@ -219,12 +222,51 @@ class AP_GPS_SBF : public AP_GPS_Backend float Cov_VuDt; }; + struct PACKED msg5939 // AttEulerCoV + { + uint32_t TOW; // receiver time stamp, 0.001s + uint16_t WNc; // receiver time stamp, 1 week + uint8_t Reserved; // unused + uint8_t Error; // error code. bit 0-1:antenna 1, bit 2-3:antenna2, bit 7: when att not requested + // 00b:no error, 01b:not enough meausurements, 10b:antennas are on one line, 11b:inconsistent with manual anntena pos info + float Cov_HeadHead; // heading estimate variance + float Cov_PitchPitch; // pitch estimate variance + float Cov_RollRoll; // roll estimate variance + float Cov_HeadPitch; // covariance between Euler angle estimates. Always set to Do-No-Use values + float Cov_HeadRoll; + float Cov_PitchRoll; + }; + + struct PACKED AuxAntPositionSubBlock { + uint8_t NrSV; // total number of satellites tracked by the antenna + uint8_t Error; // aux antenna position error code + uint8_t AmbiguityType; // aux antenna positions obtained with 0: fixed ambiguities, 1: float ambiguities + uint8_t AuxAntID; // aux antenna ID: 1 for the first auxiliary antenna, 2 for the second, etc. + double DeltaEast; // position in East direction (relative to main antenna) + double DeltaNorth; // position in North direction (relative to main antenna) + double DeltaUp; // position in Up direction (relative to main antenna) + double EastVel; // velocity in East direction (relative to main antenna) + double NorthVel; // velocity in North direction (relative to main antenna) + double UpVel; // velocity in Up direction (relative to main antenna) + }; + + struct PACKED msg5942 // AuxAntPositions + { + uint32_t TOW; + uint16_t WNc; + uint8_t N; // number of AuxAntPosition sub-blocks in this AuxAntPositions block + uint8_t SBLength; // length of one sub-block in bytes + AuxAntPositionSubBlock ant1; // first aux antennas position + }; + union PACKED msgbuffer { msg4007 msg4007u; msg4001 msg4001u; msg4014 msg4014u; msg4028 msg4028u; msg5908 msg5908u; + msg5939 msg5939u; + msg5942 msg5942u; uint8_t bytes[256]; }; diff --git a/libraries/AP_Generator/AP_Generator_IE_2400.cpp b/libraries/AP_Generator/AP_Generator_IE_2400.cpp index d297261d5ff4fa..84d6a497bfe35a 100644 --- a/libraries/AP_Generator/AP_Generator_IE_2400.cpp +++ b/libraries/AP_Generator/AP_Generator_IE_2400.cpp @@ -18,6 +18,7 @@ #if AP_GENERATOR_IE_2400_ENABLED #include +#include extern const AP_HAL::HAL& hal; @@ -32,21 +33,86 @@ void AP_Generator_IE_2400::init() _frontend._has_fuel_remaining = true; } -// Update fuel cell, expected to be called at 20hz +// Assigns the unit specific measurements once a valid sentence is obtained void AP_Generator_IE_2400::assign_measurements(const uint32_t now) { - // Successfully decoded a new valid sentence + + if (_type == PacketType::V2_INFO) { + // Got info packet + if (_had_info) { + // Not expecting the version to change + return; + } + _had_info = true; + + // Info tells us the protocol version, so lock on straight away + if (_version == ProtocolVersion::DETECTING) { + if (strcmp(_info.Protocol_version, "4") == 0) { + _version = ProtocolVersion::V2; + } else { + // Got a valid info packet, but don't know this protocol version + // Give up + _version = ProtocolVersion::UNKNOWN; + } + } + + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IE Fuel cell detected, PCM: %s, Ver: %s, SN: %s", _info.PCM_number, _info.Software_version, _info.Serial_number); + + return; + } + + // Try and lock onto version + if (_version == ProtocolVersion::DETECTING) { + ProtocolVersion new_version = ProtocolVersion::DETECTING; + switch (_type) { + case PacketType::NONE: + // Should not get a valid packet of type none + _last_version_packet_count = 0; + return; + + case PacketType::LEGACY_DATA: + new_version = ProtocolVersion::LEGACY; + break; + + case PacketType::V2_DATA: + case PacketType::V2_INFO: + new_version = ProtocolVersion::V2; + break; + } + + if (_last_version == new_version) { + _last_version_packet_count++; + } else { + _last_version_packet_count = 0; + } + _last_version = new_version; + + // If received 20 valid packets for a single protocol version then lock on + if (_last_version_packet_count > 20) { + _version = new_version; + gcs().send_text(MAV_SEVERITY_INFO, "Generator: IE using %s protocol", (_version == ProtocolVersion::V2) ? "V2" : "legacy" ); + + } else { + // Don't record any data during version detection + return; + } + } + + if (_type == PacketType::V2_DATA) { + memcpy(&_valid_V2, &_parsed_V2, sizeof(_valid_V2)); + } + // Update internal fuel cell state _pwr_out = _parsed.pwr_out; _spm_pwr = _parsed.spm_pwr; + _battery_pwr = _parsed.battery_pwr; _state = (State)_parsed.state; + _v2_state = (V2_State)_parsed.state; _err_code = _parsed.err_code; + _sub_err_code = _parsed.sub_err_code; - // Scale tank pressure linearly to a value between 0 and 1 - // Min = 5 bar, max = 300 bar, PRESS_GRAD = 1/295. - const float PRESS_GRAD = 0.003389830508f; - _fuel_remaining = constrain_float((_parsed.tank_bar-5)*PRESS_GRAD,0,1); + _fuel_remaining = _fuel_rem; // Update battery voltage _voltage = _parsed.battery_volt; @@ -55,7 +121,7 @@ void AP_Generator_IE_2400::assign_measurements(const uint32_t now) battery is charging. This is aligned with normal AP behaviour. This is the opposite of IE's convention hence *-1 */ if (_parsed.battery_volt > 0) { - _current = -1 * _parsed.battery_pwr / _parsed.battery_volt; + _current = -1 * _battery_pwr / _parsed.battery_volt; } else { _current = 0; } @@ -73,13 +139,44 @@ void AP_Generator_IE_2400::decode_latest_term() _term[_term_offset] = 0; _term_offset = 0; _term_number++; + _type = PacketType::NONE; + + if (_start_char == '<') { + decode_data_packet(); + + } else if (_start_char == '[') { + decode_info_packet(); + + } else { + _sentence_valid = false; + + } +} + +void AP_Generator_IE_2400::decode_data_packet() +{ + // Try and decode both protocol versions until locked on + if ((_version == ProtocolVersion::LEGACY) || (_version == ProtocolVersion::DETECTING)) { + decode_legacy_data(); + } + if ((_version == ProtocolVersion::V2) || (_version == ProtocolVersion::DETECTING)) { + decode_v2_data(); + } +} +void AP_Generator_IE_2400::decode_legacy_data() +{ switch (_term_number) { - case 1: + case 1: { // Float _parsed.tank_bar = strtof(_term, NULL); - break; + // Scale tank pressure linearly to a value between 0 and 1 + // Min = 5 bar, max = 300 bar, PRESS_GRAD = 1/295. + const float PRESS_GRAD = 0.003389830508f; + _fuel_rem = constrain_float((_parsed.tank_bar-5)*PRESS_GRAD,0,1); + break; + } case 2: // Float _parsed.battery_volt = strtof(_term, NULL); @@ -110,7 +207,80 @@ void AP_Generator_IE_2400::decode_latest_term() _parsed.err_code = strtoul(_term, nullptr, 10); // Sentence only declared valid when we have the expected number of terms _sentence_valid = true; + _type = PacketType::LEGACY_DATA; + break; + + default: + // We have received more terms than, something has gone wrong with telemetry data, mark invalid sentence + _sentence_valid = false; + break; + } +} + +void AP_Generator_IE_2400::decode_v2_data() +{ + switch (_term_number) { + case 1: + _fuel_rem = strtof(_term, NULL) * 0.01; + break; + + case 2: + _parsed_V2.inlet_press = strtof(_term, NULL); + break; + + case 3: + _parsed.battery_volt = strtof(_term, NULL); + break; + + case 4: + _parsed.pwr_out = strtol(_term, nullptr, 10); + break; + + case 5: + _parsed.spm_pwr = strtoul(_term, nullptr, 10); + break; + + case 6: + _parsed_V2.unit_fault = strtoul(_term, nullptr, 10); + break; + + case 7: + _parsed.battery_pwr = strtol(_term, nullptr, 10); + break; + + case 8: + _parsed.state = strtoul(_term, nullptr, 10); + break; + + case 9: + _parsed.err_code = strtoul(_term, nullptr, 10); + break; + + case 10: + _parsed.sub_err_code = strtoul(_term, nullptr, 10); + break; + + case 11: + strncpy(_parsed_V2.info_str, _term, ARRAY_SIZE(_parsed_V2.info_str)); + break; + + case 12: { + // The inverted checksum is sent, un-invert it + uint8_t checksum = ~strtoul(_term, nullptr, 10); + + // Sent checksum only included characters up to the checksum term + // Add on the checksum terms to match our running total + for (uint8_t i = 0; i < ARRAY_SIZE(_term); i++) { + if (_term[i] == 0) { + break; + } + checksum += _term[i]; + } + + _sentence_valid = checksum == _checksum; + _type = PacketType::V2_DATA; break; + } default: // We have received more terms than, something has gone wrong with telemetry data, mark invalid sentence @@ -119,6 +289,57 @@ void AP_Generator_IE_2400::decode_latest_term() } } + +void AP_Generator_IE_2400::decode_info_packet() +{ + + switch (_term_number) { + case 1: + // PCM software number + strncpy(_info.PCM_number, _term, ARRAY_SIZE(_info.PCM_number)); + break; + + case 2: + // Software version + strncpy(_info.Software_version, _term, ARRAY_SIZE(_info.Software_version)); + break; + + case 3: + // protocol version + strncpy(_info.Protocol_version, _term, ARRAY_SIZE(_info.Protocol_version)); + break; + + case 4: + // Hardware serial number + strncpy(_info.Serial_number, _term, ARRAY_SIZE(_info.Serial_number)); + break; + + case 5: { + // The inverted checksum is sent, un-invert it + uint8_t checksum = ~strtoul(_term, nullptr, 10); + + // Sent checksum only included characters up to the checksum term + // Add on the checksum terms to match our running total + for (uint8_t i = 0; i < ARRAY_SIZE(_term); i++) { + if (_term[i] == 0) { + break; + } + checksum += _term[i]; + } + + _sentence_valid = checksum == _checksum; + _type = PacketType::V2_INFO; + break; + } + + default: + // We have received more terms than, something has gone wrong with telemetry data, mark invalid sentence + _sentence_valid = false; + break; + } + +} + // Check for failsafes AP_BattMonitor::Failsafe AP_Generator_IE_2400::update_failsafes() const { @@ -138,6 +359,12 @@ AP_BattMonitor::Failsafe AP_Generator_IE_2400::update_failsafes() const // Check for error codes that are deemed critical bool AP_Generator_IE_2400::is_critical_error(const uint32_t err_in) const { + // V2 protocol + if (_version == ProtocolVersion::V2) { + return err_in >= 30; + } + + // V1 protocol switch ((ErrorCode)err_in) { // Error codes that lead to critical action battery monitor failsafe case ErrorCode::BATTERY_CRITICAL: @@ -154,6 +381,12 @@ bool AP_Generator_IE_2400::is_critical_error(const uint32_t err_in) const // Check for error codes that are deemed severe and would be cause to trigger a battery monitor low failsafe action bool AP_Generator_IE_2400::is_low_error(const uint32_t err_in) const { + // V2 protocol + if (_version == ProtocolVersion::V2) { + return (err_in > 20) && (err_in < 30); + } + + // V1 protocol switch ((ErrorCode)err_in) { // Error codes that lead to critical action battery monitor failsafe case ErrorCode::START_DENIED: @@ -161,7 +394,6 @@ bool AP_Generator_IE_2400::is_low_error(const uint32_t err_in) const case ErrorCode::BATTERY_LOW: case ErrorCode::PRESSURE_LOW: case ErrorCode::SPM_LOST: - case ErrorCode::REDUCED_POWER: return true; default: @@ -178,10 +410,47 @@ bool AP_Generator_IE_2400::check_for_err_code(char* msg_txt, uint8_t msg_len) co return false; } + if (_version == ProtocolVersion::V2) { + hal.util->snprintf(msg_txt, msg_len, "Fuel cell err %u.%u: %s", (unsigned)_err_code, (unsigned)_sub_err_code, _valid_V2.info_str); + return true; + } + hal.util->snprintf(msg_txt, msg_len, "Fuel cell err code <%u>", (unsigned)_err_code); return true; } +bool AP_Generator_IE_2400::check_for_warning_code(char* msg_txt, uint8_t msg_len) const +{ + if (_err_code == 0) { + // No error nothing to do. + return false; + } + if (is_critical_error(_err_code) || is_low_error(_err_code)) { + // Critical or low error are already reported + return false; + } + + switch (_version) { + case ProtocolVersion::DETECTING: + case ProtocolVersion::UNKNOWN: + break; + + case ProtocolVersion::LEGACY: + if ((ErrorCode)_err_code == ErrorCode::REDUCED_POWER) { + hal.util->snprintf(msg_txt, msg_len, "Fuel cell reduced power <%u>", (unsigned)_err_code); + return true; + } + break; + + case ProtocolVersion::V2: + hal.util->snprintf(msg_txt, msg_len, "Fuel cell warning %u.%u: %s", (unsigned)_err_code, (unsigned)_sub_err_code, _valid_V2.info_str); + return true; + } + + hal.util->snprintf(msg_txt, msg_len, "Fuel cell warning code <%u>", (unsigned)_err_code); + return true; +} + #if HAL_LOGGING_ENABLED // log generator status to the onboard log void AP_Generator_IE_2400::log_write() @@ -191,19 +460,106 @@ void AP_Generator_IE_2400::log_write() return; } - AP::logger().WriteStreaming( - "IE24", - "TimeUS,FUEL,SPMPWR,POUT,ERR", - "s%WW-", - "F2---", - "Qfiii", - AP_HAL::micros64(), - _fuel_remaining, - _spm_pwr, - _pwr_out, - _err_code - ); + switch (_version) { + case ProtocolVersion::DETECTING: + case ProtocolVersion::UNKNOWN: + return; + + case ProtocolVersion::LEGACY: + AP::logger().WriteStreaming( + "IE24", + "TimeUS,FUEL,SPMPWR,POUT,ERR", + "s%WW-", + "F2---", + "Qfiii", + AP_HAL::micros64(), + _fuel_remaining, + _spm_pwr, + _pwr_out, + _err_code + ); + break; + + case ProtocolVersion::V2: + AP::logger().WriteStreaming( + "IEFC", + "TimeUS,Tank,Inlet,BattV,OutPwr,SPMPwr,FNo,BPwr,State,F1,F2", + "s%-vWW-W---", + "F----------", + "QfffhHBhBII", + AP_HAL::micros64(), + _fuel_remaining, + _valid_V2.inlet_press, + _voltage, + _pwr_out, + _spm_pwr, + _valid_V2.unit_fault, + _battery_pwr, + uint8_t(_v2_state), + _err_code, + _sub_err_code + ); + break; + } + } #endif // HAL_LOGGING_ENABLED +// Return true is fuel cell is in running state suitable for arming +bool AP_Generator_IE_2400::is_running() const +{ + switch (_version) { + case ProtocolVersion::DETECTING: + case ProtocolVersion::UNKNOWN: + return false; + + case ProtocolVersion::LEGACY: + // Can use the base class method + return AP_Generator_IE_FuelCell::is_running(); + + case ProtocolVersion::V2: + return _v2_state == V2_State::Running; + } + + return false; +} + +// Lookup table for running state. State code is the same for all IE units. +const AP_Generator_IE_2400::Lookup_State_V2 AP_Generator_IE_2400::lookup_state_V2[] = { + { V2_State::FCPM_Off, "FCPM Off"}, + { V2_State::Starting, "Starting"}, + { V2_State::Running, "Running"}, + { V2_State::Stopping, "Stopping"}, + { V2_State::Go_to_Sleep, "Sleep"}, +}; + +// Print msg to user updating on state change +void AP_Generator_IE_2400::update_state_msg() +{ + switch (_version) { + case ProtocolVersion::DETECTING: + case ProtocolVersion::UNKNOWN: + break; + + case ProtocolVersion::LEGACY: + // Can use the base class method + AP_Generator_IE_FuelCell::update_state_msg(); + break; + + case ProtocolVersion::V2: { + // If fuel cell state has changed send gcs message + if (_v2_state != _last_v2_state) { + for (const struct Lookup_State_V2 entry : lookup_state_V2) { + if (_v2_state == entry.option) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Generator: %s", entry.msg_txt); + break; + } + } + _last_v2_state = _v2_state; + } + break; + } + } +} + #endif // AP_GENERATOR_IE_2400_ENABLED diff --git a/libraries/AP_Generator/AP_Generator_IE_2400.h b/libraries/AP_Generator/AP_Generator_IE_2400.h index 65acfa8ef95028..c98238f53f7c53 100644 --- a/libraries/AP_Generator/AP_Generator_IE_2400.h +++ b/libraries/AP_Generator/AP_Generator_IE_2400.h @@ -23,9 +23,20 @@ class AP_Generator_IE_2400 : public AP_Generator_IE_FuelCell // Process characters received and extract terms for IE 2.4kW void decode_latest_term(void) override; + // Decode a data packet + void decode_data_packet(); + void decode_legacy_data(); + void decode_v2_data(); + + // Decode a info packet + void decode_info_packet(); + // Check if we have received an error code and populate message with error code bool check_for_err_code(char* msg_txt, uint8_t msg_len) const override; + // Check if we have received an warning code and populate message with warning code + bool check_for_warning_code(char* msg_txt, uint8_t msg_len) const override; + // Check for error codes that are deemed critical bool is_critical_error(const uint32_t err_in) const; @@ -36,6 +47,12 @@ class AP_Generator_IE_2400 : public AP_Generator_IE_FuelCell void log_write(void) override; #endif + // Return true is fuel cell is in running state suitable for arming + bool is_running() const override; + + // Print msg to user updating on state change + void update_state_msg() override; + // IE 2.4kW failsafes enum class ErrorCode { MINOR_INTERNAL = 1, // Minor internal error is to be ignored @@ -53,6 +70,59 @@ class AP_Generator_IE_2400 : public AP_Generator_IE_FuelCell // These measurements are only available on this unit int16_t _pwr_out; // Output power (Watts) uint16_t _spm_pwr; // Stack Power Module (SPM) power draw (Watts) + float _fuel_rem; // fuel remaining 0 to 1 + int16_t _battery_pwr; // Battery charging power + + // Extra data in the V2 packet + struct V2_data { + float inlet_press; + uint8_t unit_fault; // Unit number with issue + char info_str[33]; + }; + V2_data _parsed_V2; + V2_data _valid_V2; + + // Info packet + struct { + char PCM_number[TERM_BUFFER]; + char Software_version[TERM_BUFFER]; + char Protocol_version[TERM_BUFFER]; + char Serial_number[TERM_BUFFER]; + } _info; + bool _had_info; + + enum class ProtocolVersion { + DETECTING = 0, + LEGACY = 1, + V2 = 2, + UNKNOWN = 3, + } _version; + + ProtocolVersion _last_version; + uint8_t _last_version_packet_count; + + enum class PacketType { + NONE = 0, + LEGACY_DATA = 1, + V2_DATA = 2, + V2_INFO = 3, + } _type; + + enum class V2_State { + FCPM_Off = 0, + Starting = 1, + Running = 2, + Stopping = 3, + Go_to_Sleep = 4, + } _v2_state; + V2_State _last_v2_state; + + // State enum to string lookup + struct Lookup_State_V2 { + V2_State option; + const char *msg_txt; + }; + static const Lookup_State_V2 lookup_state_V2[]; }; #endif // AP_GENERATOR_IE_2400_ENABLED diff --git a/libraries/AP_Generator/AP_Generator_IE_650_800.cpp b/libraries/AP_Generator/AP_Generator_IE_650_800.cpp index 8c5b770e1511f1..c0e9aae3a2a95b 100644 --- a/libraries/AP_Generator/AP_Generator_IE_650_800.cpp +++ b/libraries/AP_Generator/AP_Generator_IE_650_800.cpp @@ -59,6 +59,11 @@ void AP_Generator_IE_650_800::decode_latest_term() _term_offset = 0; _term_number++; + if (_start_char != '<') { + _sentence_valid = false; + return; + } + switch (_term_number) { case 1: _parsed.tank_pct = strtof(_term, NULL); diff --git a/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp b/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp index b66d59b8ddcd49..1385888435f76a 100644 --- a/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp +++ b/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp @@ -77,12 +77,14 @@ void AP_Generator_IE_FuelCell::update() bool AP_Generator_IE_FuelCell::decode(char c) { // Start of a string - if (c == '<') { + if ((c == '<') || (c == '[')) { + _start_char = c; _sentence_valid = false; _data_valid = true; _term_number = 0; _term_offset = 0; _in_string = true; + _checksum = c; return false; } if (!_in_string) { @@ -90,7 +92,8 @@ bool AP_Generator_IE_FuelCell::decode(char c) } // End of a string - if (c == '>') { + const char end_char = (_start_char == '[') ? ']' : '>'; + if (c == end_char) { decode_latest_term(); _in_string = false; @@ -100,11 +103,13 @@ bool AP_Generator_IE_FuelCell::decode(char c) // End of a term in the string if (c == ',') { decode_latest_term(); + _checksum += c; return false; } // Otherwise add the char to the current term _term[_term_offset++] = c; + _checksum += c; // We have overrun the expected sentence if (_term_offset >TERM_BUFFER) { @@ -124,7 +129,7 @@ bool AP_Generator_IE_FuelCell::pre_arm_check(char *failmsg, uint8_t failmsg_len) } // Refuse arming if not in running state - if (_state != State::RUNNING) { + if (!is_running()) { strncpy(failmsg, "Status not running", failmsg_len); return false; } @@ -160,36 +165,53 @@ void AP_Generator_IE_FuelCell::check_status(const uint32_t now) } // If fuel cell state has changed send gcs message - if (_state != _last_state) { - for (const struct Lookup_State entry : lookup_state) { - if (_state == entry.option) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Generator: %s", entry.msg_txt); - break; - } - } - _last_state = _state; - } + update_state_msg(); // Check error codes - char msg_txt[32]; - if (check_for_err_code_if_changed(msg_txt, sizeof(msg_txt))) { - GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "%s", msg_txt); - } + check_for_err_code_if_changed(); } // Check error codes and populate message with error code -bool AP_Generator_IE_FuelCell::check_for_err_code_if_changed(char* msg_txt, uint8_t msg_len) +void AP_Generator_IE_FuelCell::check_for_err_code_if_changed() { // Only check if there has been a change in error code - if (_err_code == _last_err_code) { - return false; + if ((_err_code == _last_err_code) && (_sub_err_code == _last_sub_err_code)) { + return; } - if (check_for_err_code(msg_txt, msg_len)) { - _last_err_code = _err_code; - return true; + char msg_txt[64]; + if (check_for_err_code(msg_txt, sizeof(msg_txt)) || check_for_warning_code(msg_txt, sizeof(msg_txt))) { + GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "%s", msg_txt); + + } else if ((_err_code == 0) && (_sub_err_code == 0)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Fuel cell error cleared"); + } - return false; + _last_err_code = _err_code; + _last_sub_err_code = _sub_err_code; + } + +// Return true is fuel cell is in running state suitable for arming +bool AP_Generator_IE_FuelCell::is_running() const +{ + return _state == State::RUNNING; +} + +// Print msg to user updating on state change +void AP_Generator_IE_FuelCell::update_state_msg() +{ + // If fuel cell state has changed send gcs message + if (_state != _last_state) { + for (const struct Lookup_State entry : lookup_state) { + if (_state == entry.option) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Generator: %s", entry.msg_txt); + break; + } + } + _last_state = _state; + } +} + #endif // AP_GENERATOR_IE_ENABLED diff --git a/libraries/AP_Generator/AP_Generator_IE_FuelCell.h b/libraries/AP_Generator/AP_Generator_IE_FuelCell.h index 84395f1125c917..8eac8952493019 100644 --- a/libraries/AP_Generator/AP_Generator_IE_FuelCell.h +++ b/libraries/AP_Generator/AP_Generator_IE_FuelCell.h @@ -49,6 +49,8 @@ class AP_Generator_IE_FuelCell : public AP_Generator_Backend uint32_t _err_code; // The error code from fuel cell uint32_t _last_err_code; // The previous error code from fuel cell + uint32_t _sub_err_code; // The sub error code from fuel cell + uint32_t _last_sub_err_code; // The previous sub error code from fuel cell State _state; // The PSU state State _last_state; // The previous PSU state uint32_t _last_time_ms; // Time we got a reading @@ -66,19 +68,22 @@ class AP_Generator_IE_FuelCell : public AP_Generator_Backend int16_t battery_pwr; uint8_t state; uint32_t err_code; + uint32_t sub_err_code; } _parsed; // Constants - static const uint8_t TERM_BUFFER = 12; // Max length of term we expect + static const uint8_t TERM_BUFFER = 33; // Max length of term we expect static const uint16_t HEALTHY_TIMEOUT_MS = 5000; // Time for driver to be marked un-healthy // Decoding vars + char _start_char; // inital sentence character giving sentence type char _term[TERM_BUFFER]; // Term buffer bool _sentence_valid; // Is current sentence valid bool _data_valid; // Is data within expected limits uint8_t _term_number; // Term index within the current sentence uint8_t _term_offset; // Offset within the _term buffer where the next character should be placed bool _in_string; // True if we should be decoding + uint8_t _checksum; // Basic checksum used by V2 protocol // Assigns the unit specific measurements once a valid sentence is obtained virtual void assign_measurements(const uint32_t now) = 0; @@ -100,8 +105,17 @@ class AP_Generator_IE_FuelCell : public AP_Generator_Backend // Check error codes and populate message with error code virtual bool check_for_err_code(char* msg_txt, uint8_t msg_len) const = 0; + // Check if we have received an warning code and populate message with warning code + virtual bool check_for_warning_code(char* msg_txt, uint8_t msg_len) const { return false; } + // Only check the error code if it has changed since we last checked - bool check_for_err_code_if_changed(char* msg_txt, uint8_t msg_len); + void check_for_err_code_if_changed(); + + // Return true is fuel cell is in running state suitable for arming + virtual bool is_running() const; + + // Print msg to user updating on state change + virtual void update_state_msg(); }; #endif // AP_GENERATOR_IE_ENABLED diff --git a/libraries/AP_HAL/AP_HAL_Namespace.h b/libraries/AP_HAL/AP_HAL_Namespace.h index b3e84314ed9ef0..aca8e02293763a 100644 --- a/libraries/AP_HAL/AP_HAL_Namespace.h +++ b/libraries/AP_HAL/AP_HAL_Namespace.h @@ -67,6 +67,7 @@ namespace AP_HAL { class SIMState; - // Must be implemented by the concrete HALs. + // Must be implemented by the concrete HALs and return the same reference. const HAL& get_HAL(); + HAL& get_HAL_mutable(); } diff --git a/libraries/AP_HAL/utility/Socket.cpp b/libraries/AP_HAL/utility/Socket.cpp index 0052778928aa46..a5beacca66d054 100644 --- a/libraries/AP_HAL/utility/Socket.cpp +++ b/libraries/AP_HAL/utility/Socket.cpp @@ -81,6 +81,7 @@ bool SocketAPM::connect(const char *address, uint16_t port) { struct sockaddr_in sockaddr; int ret; + int one = 1; make_sockaddr(address, port, sockaddr); if (datagram && is_multicast_address(sockaddr)) { @@ -92,7 +93,6 @@ bool SocketAPM::connect(const char *address, uint16_t port) return false; } struct sockaddr_in sockaddr_mc = sockaddr; - int one = 1; struct ip_mreq mreq {}; #ifdef FD_CLOEXEC CALL_PREFIX(fcntl)(fd_in, F_SETFD, FD_CLOEXEC); @@ -109,7 +109,7 @@ bool SocketAPM::connect(const char *address, uint16_t port) ret = CALL_PREFIX(bind)(fd_in, (struct sockaddr *)&sockaddr_mc, sizeof(sockaddr)); if (ret == -1) { - goto fail_mc; + goto fail_multi; } mreq.imr_multiaddr.s_addr = sockaddr.sin_addr.s_addr; @@ -117,10 +117,14 @@ bool SocketAPM::connect(const char *address, uint16_t port) ret = CALL_PREFIX(setsockopt)(fd_in, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)); if (ret == -1) { - goto fail_mc; + goto fail_multi; } - } else if (datagram && sockaddr.sin_addr.s_addr == INADDR_BROADCAST) { + } + + if (datagram && sockaddr.sin_addr.s_addr == INADDR_BROADCAST) { + // setup for bi-directional UDP broadcast set_broadcast(); + reuseaddress(); } ret = CALL_PREFIX(connect)(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); @@ -128,9 +132,27 @@ bool SocketAPM::connect(const char *address, uint16_t port) return false; } connected = true; + + if (datagram && sockaddr.sin_addr.s_addr == INADDR_BROADCAST) { + // for bi-directional UDP broadcast we need 2 sockets + struct sockaddr_in send_addr; + socklen_t send_len = sizeof(send_addr); + ret = CALL_PREFIX(getsockname)(fd, (struct sockaddr *)&send_addr, &send_len); + fd_in = CALL_PREFIX(socket)(AF_INET, SOCK_DGRAM, 0); + if (fd_in == -1) { + goto fail_multi; + } + CALL_PREFIX(setsockopt)(fd_in, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); + // 2nd socket needs to be bound to wildcard + send_addr.sin_addr.s_addr = INADDR_ANY; + ret = CALL_PREFIX(bind)(fd_in, (struct sockaddr *)&send_addr, sizeof(send_addr)); + if (ret == -1) { + goto fail_multi; + } + } return true; -fail_mc: +fail_multi: CALL_PREFIX(close)(fd_in); fd_in = -1; return false; @@ -247,6 +269,7 @@ ssize_t SocketAPM::sendto(const void *buf, size_t size, const char *address, uin ssize_t SocketAPM::recv(void *buf, size_t size, uint32_t timeout_ms) { if (!pollin(timeout_ms)) { + errno = EWOULDBLOCK; return -1; } socklen_t len = sizeof(in_addr); @@ -284,6 +307,19 @@ void SocketAPM::last_recv_address(const char *&ip_addr, uint16_t &port) const port = ntohs(in_addr.sin_port); } +/* + return the IP address and port of the last received packet, using caller supplied buffer + */ +const char *SocketAPM::last_recv_address(char *ip_addr_buf, uint8_t buflen, uint16_t &port) const +{ + const char *ret = inet_ntop(AF_INET, (void*)&in_addr.sin_addr, ip_addr_buf, buflen); + if (ret == nullptr) { + return nullptr; + } + port = ntohs(in_addr.sin_port); + return ret; +} + void SocketAPM::set_broadcast(void) const { int one = 1; @@ -350,13 +386,11 @@ SocketAPM *SocketAPM::accept(uint32_t timeout_ms) return nullptr; } - int newfd = CALL_PREFIX(accept)(fd, nullptr, nullptr); + socklen_t len = sizeof(in_addr); + int newfd = CALL_PREFIX(accept)(fd, (sockaddr *)&in_addr, &len); if (newfd == -1) { return nullptr; } - // turn off nagle for lower latency - int one = 1; - CALL_PREFIX(setsockopt)(newfd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); return new SocketAPM(false, newfd); } diff --git a/libraries/AP_HAL/utility/Socket.h b/libraries/AP_HAL/utility/Socket.h index 76ac0974c508a7..70e4a1cdea1584 100644 --- a/libraries/AP_HAL/utility/Socket.h +++ b/libraries/AP_HAL/utility/Socket.h @@ -57,6 +57,9 @@ class SocketAPM { // return the IP address and port of the last received packet void last_recv_address(const char *&ip_addr, uint16_t &port) const; + // return the IP address and port of the last received packet, using caller supplied buffer + const char *last_recv_address(char *ip_addr_buf, uint8_t buflen, uint16_t &port) const; + // return true if there is pending data for input bool pollin(uint32_t timeout_ms); diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index 2d261f7242084c..eaefd34324c69c 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -85,7 +85,7 @@ #error "Unsupported MCU for FDCAN" #endif -extern AP_HAL::HAL& hal; +extern const AP_HAL::HAL& hal; #define STR(x) #x #define XSTR(x) STR(x) @@ -568,7 +568,7 @@ bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const Oper if (can_ifaces[self_index_] == nullptr) { can_ifaces[self_index_] = this; #if !defined(HAL_BOOTLOADER_BUILD) - hal.can[self_index_] = this; + AP_HAL::get_HAL_mutable().can[self_index_] = this; #endif } diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp index 677f484804943e..97cd8acf2ad1be 100644 --- a/libraries/AP_HAL_ChibiOS/CanIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp @@ -84,7 +84,7 @@ #endif -extern AP_HAL::HAL& hal; +extern const AP_HAL::HAL& hal; using namespace ChibiOS; @@ -846,7 +846,7 @@ bool CANIface::init(const uint32_t bitrate, const CANIface::OperatingMode mode) if (can_ifaces[self_index_] == nullptr) { can_ifaces[self_index_] = this; #if !defined(HAL_BOOTLOADER_BUILD) - hal.can[self_index_] = this; + AP_HAL::get_HAL_mutable().can[self_index_] = this; #endif } diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index 6454f108e85ee3..46b352e5649f50 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp @@ -344,8 +344,13 @@ void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const main_loop(); } +static HAL_ChibiOS hal_chibios; + const AP_HAL::HAL& AP_HAL::get_HAL() { - static const HAL_ChibiOS hal_chibios; + return hal_chibios; +} + +AP_HAL::HAL& AP_HAL::get_HAL_mutable() { return hal_chibios; } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_Wiring_Diagram.jpg b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_Wiring_Diagram.jpg new file mode 100755 index 00000000000000..550efc89ac4af9 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_Wiring_Diagram.jpg differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_bottom.jpg b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_bottom.jpg new file mode 100755 index 00000000000000..e55abd46bec8c7 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_bottom.jpg differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_top.jpg b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_top.jpg new file mode 100755 index 00000000000000..673b031aa291c2 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_top.jpg differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/README.md b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/README.md new file mode 100755 index 00000000000000..3597f1ecccb866 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/README.md @@ -0,0 +1,113 @@ +# Aocoda-RC-H743Dual Flight Controller + +The Aocoda-RC-H743Dual is a flight controller produced by [Aocoda-RC](https://www.aocoda-rc.com/). + +## Features + +- MCU：STM32H743VIH6 +- Gyro：MPU6000/BIM270x2 +- Baro：DPS310/MS56XX/BMP280 +- Blackbox：128MB +- PWM output：10CH +- Servo：2CH +- UART：8CH +- Power Supply：3-6SLipo +- BEC Output：5V/2.5A, 9V/3A +- USB Connector: Type-C +- Weight：8.8g +- Size：37mm x 37mm +- Mounting Hole：30.5mm x 30.5mm + +## Pinout + + +![Aocoda-RC-H743Dual Top](Aocoda-RC-H743Dual_top.jpg "Aocoda-RC-H743Dual Top") +![Aocoda-RC-H743Dual Bottom](Aocoda-RC-H743Dual_bottom.jpg "Aocoda-RC-H743Dual Bottom") +![Aocoda-RC-H743Dual Wiring](Aocoda-RC-H743Dual_Wiring_Diagram.jpg "Aocoda-RC-H743Dual Wiring") + + +## 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 (primary mavlink, usually USB) + - SERIAL1 -> UART1 (RC input) + - SERIAL2 -> UART2 (GPS) + - SERIAL3 -> UART3 (VTX) + - SERIAL4 -> UART4 + - SERIAL5 -> not available + - SERIAL6 -> UART6 (ESC Telemetry) + - SERIAL7 -> UART7 + - SERIAL8 -> UART8 + +## RC Input + +RC input is configured on SERIAL1 (USART1), which is available on the Rx1, Tx1. PPM receivers are *not* supported as this input does not have a timer resource available. + +*Note* It is recommend to use CRSF/ELRS. + +With recommended option: + +- Set SERIAL1_PROTOCOL #include "AP_Networking.h" const AP_Param::GroupInfo AP_Networking_IPV4::var_info[] = { @@ -68,9 +69,11 @@ void AP_Networking_IPV4::set_default_uint32(uint32_t v) } } -const char* AP_Networking_IPV4::get_str() const +const char* AP_Networking_IPV4::get_str() { - return AP_Networking::convert_ip_to_str(get_uint32()); + const auto ip = ntohl(get_uint32()); + inet_ntop(AF_INET, &ip, strbuf, sizeof(strbuf)); + return strbuf; } #endif // AP_NETWORKING_ENABLED diff --git a/libraries/AP_Networking/AP_Networking_address.h b/libraries/AP_Networking/AP_Networking_address.h index a4a3b6f8b29a8f..f9852948676ec8 100644 --- a/libraries/AP_Networking/AP_Networking_address.h +++ b/libraries/AP_Networking/AP_Networking_address.h @@ -18,12 +18,15 @@ class AP_Networking_IPV4 void set_uint32(uint32_t addr); // return address as a null-terminated string - const char* get_str() const; + const char* get_str(); // set default address from a uint32 void set_default_uint32(uint32_t addr); static const struct AP_Param::GroupInfo var_info[]; + +private: + char strbuf[16]; }; /* diff --git a/libraries/AP_Networking/AP_Networking_port.cpp b/libraries/AP_Networking/AP_Networking_port.cpp index e70224cf4b4b7b..3cb6a2f3d17dba 100644 --- a/libraries/AP_Networking/AP_Networking_port.cpp +++ b/libraries/AP_Networking/AP_Networking_port.cpp @@ -11,6 +11,8 @@ #include #include #include +#include +#include extern const AP_HAL::HAL& hal; @@ -22,11 +24,15 @@ extern const AP_HAL::HAL& hal; #define AP_NETWORKING_PORT_MIN_RXSIZE 2048 #endif +#ifndef AP_NETWORKING_PORT_STACK_SIZE +#define AP_NETWORKING_PORT_STACK_SIZE 1024 +#endif + const AP_Param::GroupInfo AP_Networking::Port::var_info[] = { // @Param: TYPE // @DisplayName: Port type - // @Description: Port type - // @Values: 0:Disabled, 1:UDP client + // @Description: Port type for network serial port. For the two client types a valid destination IP address must be set. For the two server types either 0.0.0.0 or a local address can be used. + // @Values: 0:Disabled, 1:UDP client, 2:TCP client, 3:TCP server // @RebootRequired: True // @User: Advanced AP_GROUPINFO_FLAGS("TYPE", 1, AP_Networking::Port, type, 0, AP_PARAM_FLAG_ENABLE), @@ -66,45 +72,121 @@ void AP_Networking::ports_init(void) case NetworkPortType::NONE: break; case NetworkPortType::UDP_CLIENT: - p.udp_client_init(AP_NETWORKING_PORT_MIN_RXSIZE, AP_NETWORKING_PORT_MIN_TXSIZE); + p.udp_client_init(); + break; + case NetworkPortType::UDP_SERVER: + p.udp_server_init(); + break; + case NetworkPortType::TCP_SERVER: + p.tcp_server_init(); + break; + case NetworkPortType::TCP_CLIENT: + p.tcp_client_init(); break; } - if (p.sock != nullptr) { + if (p.sock != nullptr || p.listen_sock != nullptr) { AP::serialmanager().register_port(&p); } } } /* - initialise a UDP client + wrapper for thread_create for port functions */ -void AP_Networking::Port::udp_client_init(const uint32_t size_rx, const uint32_t size_tx) +void AP_Networking::Port::thread_create(AP_HAL::MemberProc proc) { - WITH_SEMAPHORE(sem); - if (!init_buffers(size_rx, size_tx)) { + const uint8_t idx = state.idx - AP_SERIALMANAGER_NET_PORT_1; + hal.util->snprintf(thread_name, sizeof(thread_name), "NET_P%u", unsigned(idx)); + + if (!init_buffers(AP_NETWORKING_PORT_MIN_RXSIZE, AP_NETWORKING_PORT_MIN_TXSIZE)) { + AP_BoardConfig::allocation_error("Failed to allocate %s buffers", thread_name); return; } - if (sock != nullptr) { + + if (!hal.scheduler->thread_create(proc, thread_name, AP_NETWORKING_PORT_STACK_SIZE, AP_HAL::Scheduler::PRIORITY_UART, 0)) { + AP_BoardConfig::allocation_error("Failed to allocate %s client thread", thread_name); + } +} + +/* + initialise a UDP client + */ +void AP_Networking::Port::udp_client_init(void) +{ + sock = new SocketAPM(true); + if (sock == nullptr) { return; } + sock->set_blocking(false); + + // setup for packet boundaries if this is mavlink + packetise = (state.protocol == AP_SerialManager::SerialProtocol_MAVLink || + state.protocol == AP_SerialManager::SerialProtocol_MAVLink2); + + thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::Port::udp_client_loop, void)); +} + +/* + initialise a UDP server + */ +void AP_Networking::Port::udp_server_init(void) +{ sock = new SocketAPM(true); if (sock == nullptr) { return; } - if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::Port::udp_client_loop, void), "NET", 2048, AP_HAL::Scheduler::PRIORITY_UART, 0)) { - AP_BoardConfig::allocation_error("Failed to allocate UDP client thread"); + sock->set_blocking(false); + + // setup for packet boundaries if this is mavlink + packetise = (state.protocol == AP_SerialManager::SerialProtocol_MAVLink || + state.protocol == AP_SerialManager::SerialProtocol_MAVLink2); + + thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::Port::udp_server_loop, void)); +} + +/* + initialise a TCP server + */ +void AP_Networking::Port::tcp_server_init(void) +{ + listen_sock = new SocketAPM(false); + if (listen_sock == nullptr) { + return; + } + listen_sock->reuseaddress(); + + thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::Port::tcp_server_loop, void)); +} + +/* + initialise a TCP client + */ +void AP_Networking::Port::tcp_client_init(void) +{ + sock = new SocketAPM(false); + if (sock != nullptr) { + sock->set_blocking(true); + thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::Port::tcp_client_loop, void)); } } /* - update a UDP client + wait for networking stack to be up */ -void AP_Networking::Port::udp_client_loop(void) +void AP_Networking::Port::wait_startup(void) { while (!hal.scheduler->is_system_initialized()) { hal.scheduler->delay(100); } hal.scheduler->delay(1000); +} + +/* + update a UDP client + */ +void AP_Networking::Port::udp_client_loop(void) +{ + wait_startup(); const char *dest = ip.get_str(); if (!sock->connect(dest, port.get())) { @@ -116,31 +198,190 @@ void AP_Networking::Port::udp_client_loop(void) GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UDP[%u]: connected to %s:%u", (unsigned)state.idx, dest, unsigned(port.get())); + connected = true; + + bool active = false; while (true) { - hal.scheduler->delay_microseconds(500); - WITH_SEMAPHORE(sem); + if (!active) { + hal.scheduler->delay_microseconds(100); + } + active = send_receive(); + } +} - // handle outgoing packets - uint32_t available; - const auto *ptr = writebuffer->readptr(available); - if (ptr != nullptr) { - const auto ret = sock->send(ptr, available); - if (ret > 0) { - writebuffer->advance(ret); +/* + update a UDP server + */ +void AP_Networking::Port::udp_server_loop(void) +{ + wait_startup(); + + const char *addr = ip.get_str(); + if (!sock->bind(addr, port.get())) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UDP[%u]: Failed to bind to %s:%u", (unsigned)state.idx, addr, unsigned(port.get())); + delete sock; + sock = nullptr; + return; + } + sock->reuseaddress(); + + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UDP[%u]: bound to %s:%u", (unsigned)state.idx, addr, unsigned(port.get())); + + bool active = false; + while (true) { + if (!active) { + hal.scheduler->delay_microseconds(100); + } + active = send_receive(); + } +} + +/* + update a TCP server + */ +void AP_Networking::Port::tcp_server_loop(void) +{ + wait_startup(); + + const char *addr = ip.get_str(); + if (!listen_sock->bind(addr, port.get()) || !listen_sock->listen(1)) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "TCP[%u]: Failed to bind to %s:%u", (unsigned)state.idx, addr, unsigned(port.get())); + delete listen_sock; + listen_sock = nullptr; + return; + } + + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP[%u]: bound to %s:%u", (unsigned)state.idx, addr, unsigned(port.get())); + + close_on_recv_error = true; + + bool active = false; + while (true) { + if (!active) { + hal.scheduler->delay_microseconds(100); + } + if (sock == nullptr) { + sock = listen_sock->accept(100); + if (sock != nullptr) { + sock->set_blocking(false); + char buf[16]; + uint16_t last_port; + const char *last_addr = listen_sock->last_recv_address(buf, sizeof(buf), last_port); + if (last_addr != nullptr) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP[%u]: connection from %s:%u", (unsigned)state.idx, last_addr, unsigned(last_port)); + } + connected = true; + sock->reuseaddress(); } } + if (sock != nullptr) { + active = send_receive(); + } + } +} - // handle incoming packets - const auto space = readbuffer->space(); - if (space > 0) { - const uint32_t n = MIN(350U, space); - uint8_t buf[n]; - const auto ret = sock->recv(buf, n, 0); - if (ret > 0) { - readbuffer->write(buf, ret); +/* + update a TCP client + */ +void AP_Networking::Port::tcp_client_loop(void) +{ + wait_startup(); + + close_on_recv_error = true; + + bool active = false; + while (true) { + if (!active) { + hal.scheduler->delay_microseconds(100); + } + if (sock == nullptr) { + sock = new SocketAPM(false); + if (sock == nullptr) { + continue; + } + sock->set_blocking(true); + connected = false; + } + if (!connected) { + const char *dest = ip.get_str(); + connected = sock->connect(dest, port.get()); + if (connected) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP[%u]: connected to %s:%u", unsigned(state.idx), dest, unsigned(port.get())); + sock->set_blocking(false); + } else { + delete sock; + sock = nullptr; + // don't try and connect too fast + hal.scheduler->delay(100); + } + } + if (sock != nullptr && connected) { + active = send_receive(); + } + } +} + +/* + run one send/receive loop + */ +bool AP_Networking::Port::send_receive(void) +{ + + bool active = false; + WITH_SEMAPHORE(sem); + + // handle incoming packets + const auto space = readbuffer->space(); + if (space > 0) { + const uint32_t n = MIN(300U, space); + uint8_t buf[n]; + const auto ret = sock->recv(buf, n, 0); + if (close_on_recv_error && ret == 0) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP[%u]: closed connection", unsigned(state.idx)); + delete sock; + sock = nullptr; + return false; + } + if (ret > 0) { + readbuffer->write(buf, ret); + active = true; + have_received = true; + } + } + + if (connected) { + // handle outgoing packets + uint32_t available = writebuffer->available(); + available = MIN(300U, available); +#if HAL_GCS_ENABLED + if (packetise) { + available = mavlink_packetise(*writebuffer, available); + } +#endif + if (available > 0) { + uint8_t buf[available]; + auto n = writebuffer->peekbytes(buf, available); + if (n > 0) { + const auto ret = sock->send(buf, n); + if (ret > 0) { + writebuffer->advance(ret); + active = true; + } + } + } + } else { + if (type == NetworkPortType::UDP_SERVER && have_received) { + // connect the socket to the last receive address if we have one + char buf[16]; + uint16_t last_port; + const char *last_addr = sock->last_recv_address(buf, sizeof(buf), last_port); + if (last_addr != nullptr && port != 0) { + connected = sock->connect(last_addr, last_port); } } } + + return active; } /* @@ -210,4 +451,22 @@ bool AP_Networking::Port::init_buffers(const uint32_t size_rx, const uint32_t si return readbuffer != nullptr && writebuffer != nullptr; } +/* + return flow control state + */ +enum AP_HAL::UARTDriver::flow_control AP_Networking::Port::get_flow_control(void) +{ + const NetworkPortType ptype = (NetworkPortType)type; + switch (ptype) { + case NetworkPortType::TCP_CLIENT: + case NetworkPortType::TCP_SERVER: + return AP_HAL::UARTDriver::FLOW_CONTROL_ENABLE; + case NetworkPortType::UDP_CLIENT: + case NetworkPortType::UDP_SERVER: + case NetworkPortType::NONE: + break; + } + return AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE; +} + #endif // AP_NETWORKING_ENABLED diff --git a/libraries/AP_Networking/AP_Networking_tests.cpp b/libraries/AP_Networking/AP_Networking_tests.cpp index 9370d93a23b407..2fc050d9ddd4a6 100644 --- a/libraries/AP_Networking/AP_Networking_tests.cpp +++ b/libraries/AP_Networking/AP_Networking_tests.cpp @@ -28,6 +28,11 @@ void AP_Networking::start_tests(void) "TCP_client", 8192, AP_HAL::Scheduler::PRIORITY_IO, -1); } + if (param.tests & TEST_TCP_DISCARD) { + hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::test_TCP_discard, void), + "TCP_discard", + 8192, AP_HAL::Scheduler::PRIORITY_UART, -1); + } } /* @@ -100,4 +105,44 @@ void AP_Networking::test_TCP_client(void) } } +/* + start TCP discard (throughput) test + */ +void AP_Networking::test_TCP_discard(void) +{ + while (!hal.scheduler->is_system_initialized()) { + hal.scheduler->delay(100); + } + hal.scheduler->delay(1000); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP_discard: starting"); + const char *dest = param.test_ipaddr.get_str(); + auto *sock = new SocketAPM(false); + if (sock == nullptr) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "TCP_discard: failed to create socket"); + return; + } + // connect to the discard service, which is port 9 + if (!sock->connect(dest, 9)) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "TCP_discard: connect failed"); + return; + } + const uint32_t bufsize = 1024; + uint8_t *buf = (uint8_t*)malloc(bufsize); + for (uint32_t i=0; isend(buf, bufsize); + const uint32_t now = AP_HAL::millis(); + if (now - last_report_ms >= 1000) { + float dt = (now - last_report_ms)*0.001; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Discard throughput %.3f kbyte/sec", (total_sent/dt)*1.0e-3); + total_sent = 0; + last_report_ms = now; + } + } +} + #endif // AP_NETWORKING_ENABLED && AP_NETWORKING_TESTS_ENABLED diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 9e0ad48b854a40..56d9cbf8ecbf63 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -2999,6 +2999,11 @@ bool AP_Param::load_int32(uint16_t key, uint32_t group_element, int32_t &value) */ bool AP_Param::add_param(uint8_t _key, uint8_t param_num, const char *pname, float default_value) { + if (_var_info_dynamic == nullptr) { + // No dynamic tables available + return false; + } + // check for valid values if (param_num == 0 || param_num > 63 || strlen(pname) > 16) { return false; diff --git a/libraries/AR_Motors/AP_MotorsUGV.cpp b/libraries/AR_Motors/AP_MotorsUGV.cpp index 5bb429ef0564f1..357815613e6ae2 100644 --- a/libraries/AR_Motors/AP_MotorsUGV.cpp +++ b/libraries/AR_Motors/AP_MotorsUGV.cpp @@ -457,6 +457,18 @@ bool AP_MotorsUGV::output_test_pwm(motor_test_order motor_seq, float pwm) // returns true if checks pass, false if they fail. report should be true to send text messages to GCS bool AP_MotorsUGV::pre_arm_check(bool report) const { + // check that there's defined outputs, inc scripting and sail + if(!SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) && + !SRV_Channels::function_assigned(SRV_Channel::k_throttleRight) && + !SRV_Channels::function_assigned(SRV_Channel::k_throttle) && + !SRV_Channels::function_assigned(SRV_Channel::k_steering) && + !SRV_Channels::function_assigned(SRV_Channel::k_scripting1) && + !has_sail()) { + if (report) { + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: no motor, sail or scripting outputs defined"); + } + return false; + } // check if only one of skid-steering output has been configured if (SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) != SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) { if (report) { diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index afb6f4956cc685..1ab6c817d86fb5 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -505,6 +505,10 @@ void AR_WPNav::update_steering_and_speed(const Location ¤t_loc, float dt) { _cross_track_error = calc_crosstrack_error(current_loc); + // update position controller + _pos_control.set_reversed(_reversed); + _pos_control.update(dt); + // handle pivot turns if (_pivot.active()) { // decelerate to zero @@ -512,14 +516,11 @@ void AR_WPNav::update_steering_and_speed(const Location ¤t_loc, float dt) _desired_heading_cd = _reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd(); _desired_turn_rate_rads = is_zero(_desired_speed_limited) ? _pivot.get_turn_rate_rads(_desired_heading_cd * 0.01, dt) : 0; _desired_lat_accel = 0.0f; - return; + } else { + _desired_speed_limited = _pos_control.get_desired_speed(); + _desired_turn_rate_rads = _pos_control.get_desired_turn_rate_rads(); + _desired_lat_accel = _pos_control.get_desired_lat_accel(); } - - _pos_control.set_reversed(_reversed); - _pos_control.update(dt); - _desired_speed_limited = _pos_control.get_desired_speed(); - _desired_turn_rate_rads = _pos_control.get_desired_turn_rate_rads(); - _desired_lat_accel = _pos_control.get_desired_lat_accel(); } // settor to allow vehicle code to provide turn related param values to this library (should be updated regularly) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 82ae789ba5d97a..5a35ea5b7416d2 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -20,9 +20,21 @@ #include "HarmonicNotchFilter.h" #include +#include +#include +#include +#include #define HNF_MAX_FILTERS HAL_HNF_MAX_FILTERS // must be even for double-notch filters +/* + optional logging for SITL only of all notch frequencies + */ +#ifndef NOTCH_DEBUG_LOGGING +#define NOTCH_DEBUG_LOGGING 0 +#endif + + // table of user settable parameters const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { @@ -177,13 +189,8 @@ void HarmonicNotchFilter::allocate_filters(uint8_t num_notches, uint32_t harm expand the number of filters at runtime, allowing for RPM sources such as lua scripts */ template -void HarmonicNotchFilter::expand_filter_count(uint8_t num_notches) +void HarmonicNotchFilter::expand_filter_count(uint16_t total_notches) { - uint8_t num_filters = _num_harmonics * num_notches * _composite_notches; - if (num_filters <= _num_filters) { - // enough already - return; - } if (_alloc_has_failed) { // we've failed to allocate before, don't try again return; @@ -192,7 +199,7 @@ void HarmonicNotchFilter::expand_filter_count(uint8_t num_notches) note that we rely on the semaphore in AP_InertialSensor_Backend.cpp to make this thread safe */ - auto filters = new NotchFilter[num_filters]; + auto filters = new NotchFilter[total_notches]; if (filters == nullptr) { _alloc_has_failed = true; return; @@ -200,7 +207,7 @@ void HarmonicNotchFilter::expand_filter_count(uint8_t num_notches) memcpy(filters, _filters, sizeof(filters[0])*_num_filters); auto _old_filters = _filters; _filters = filters; - _num_filters = num_filters; + _num_filters = total_notches; delete[] _old_filters; } @@ -217,7 +224,7 @@ void HarmonicNotchFilter::update(float center_freq_hz) // adjust the fundamental center frequency to be in the allowable range const float nyquist_limit = _sample_freq_hz * 0.48f; - center_freq_hz = constrain_float(center_freq_hz, 1.0f, nyquist_limit); + center_freq_hz = constrain_float(center_freq_hz, 0.0f, nyquist_limit); _num_enabled_filters = 0; // update all of the filters using the new center frequency and existing A & Q @@ -261,15 +268,16 @@ void HarmonicNotchFilter::update(uint8_t num_centers, const float center_freq // adjust the frequencies to be in the allowable range const float nyquist_limit = _sample_freq_hz * 0.48f; - if (num_centers > _num_filters) { + uint16_t total_notches = num_centers * _num_harmonics * _composite_notches; + if (total_notches > _num_filters) { // alloc realloc of filters - expand_filter_count(num_centers); + expand_filter_count(total_notches); } _num_enabled_filters = 0; // update all of the filters using the new center frequencies and existing A & Q - for (uint8_t i = 0; i < num_centers * HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) { + for (uint16_t i = 0; i < num_centers * HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) { const uint8_t harmonic_n = i / num_centers; const uint8_t center_n = i % num_centers; // the filters are ordered by center and then harmonic so @@ -278,7 +286,7 @@ void HarmonicNotchFilter::update(uint8_t num_centers, const float center_freq continue; } - const float notch_center = constrain_float(center_freq_hz[center_n] * (harmonic_n+1), 1.0f, nyquist_limit); + const float notch_center = constrain_float(center_freq_hz[center_n] * (harmonic_n+1), 0.0f, nyquist_limit); if (_composite_notches != 2) { // only enable the filter if its center frequency is below the nyquist frequency if (notch_center < nyquist_limit) { @@ -311,10 +319,29 @@ T HarmonicNotchFilter::apply(const T &sample) return sample; } +#if NOTCH_DEBUG_LOGGING + static int dfd = -1; + if (dfd == -1) { + dfd = ::open("notch.txt", O_WRONLY|O_CREAT|O_TRUNC, 0644); + } +#endif + T output = sample; - for (uint8_t i = 0; i < _num_enabled_filters; i++) { + for (uint16_t i = 0; i < _num_enabled_filters; i++) { +#if NOTCH_DEBUG_LOGGING + if (!_filters[i].initialised) { + ::dprintf(dfd, "------- "); + } else { + ::dprintf(dfd, "%.4f ", _filters[i]._center_freq_hz); + } +#endif output = _filters[i].apply(output); } +#if NOTCH_DEBUG_LOGGING + if (_num_enabled_filters > 0) { + ::dprintf(dfd, "\n"); + } +#endif return output; } @@ -328,7 +355,7 @@ void HarmonicNotchFilter::reset() return; } - for (uint8_t i = 0; i < _num_filters; i++) { + for (uint16_t i = 0; i < _num_filters; i++) { _filters[i].reset(); } } diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index 7de4df799df4e7..898857b82e58ec 100644 --- a/libraries/Filter/HarmonicNotchFilter.h +++ b/libraries/Filter/HarmonicNotchFilter.h @@ -32,7 +32,7 @@ class HarmonicNotchFilter { // allocate a bank of notch filters for this harmonic notch filter void allocate_filters(uint8_t num_notches, uint32_t harmonics, uint8_t composite_notches); // expand filter bank with new filters - void expand_filter_count(uint8_t num_notches); + void expand_filter_count(uint16_t total_notches); // initialize the underlying filters using the provided filter parameters void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB); // update the underlying filters' center frequencies using center_freq_hz as the fundamental @@ -60,11 +60,11 @@ class HarmonicNotchFilter { // number of notches that make up a composite notch uint8_t _composite_notches; // number of allocated filters - uint8_t _num_filters; + uint16_t _num_filters; // pre-calculated number of harmonics uint8_t _num_harmonics; // number of enabled filters - uint8_t _num_enabled_filters; + uint16_t _num_enabled_filters; bool _initialised; // have we failed to expand filters? diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp index 14ce778efa4cf2..fa2c024721be91 100644 --- a/libraries/Filter/NotchFilter.cpp +++ b/libraries/Filter/NotchFilter.cpp @@ -71,7 +71,7 @@ void NotchFilter::init_with_A_and_Q(float sample_freq_hz, float center_freq_h _center_freq_hz * NOTCH_MAX_SLEW_UPPER); } - if ((new_center_freq > 2.0) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) { + if (is_positive(new_center_freq) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) { float omega = 2.0 * M_PI * new_center_freq / sample_freq_hz; float alpha = sinf(omega) / (2 * Q); b0 = 1.0 + alpha*sq(A); diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h index 476d06d79b3cc2..624929208e1551 100644 --- a/libraries/Filter/NotchFilter.h +++ b/libraries/Filter/NotchFilter.h @@ -26,9 +26,13 @@ #include +template +class HarmonicNotchFilter; + template class NotchFilter { public: + friend class HarmonicNotchFilter; // set parameters void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB); void init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q); diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.h b/libraries/GCS_MAVLink/GCS_MAVLink.h index dd4ab4271a5532..26a30d068d312c 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.h +++ b/libraries/GCS_MAVLink/GCS_MAVLink.h @@ -3,6 +3,7 @@ #pragma once #include +#include // we have separate helpers disabled to make it possible // to select MAVLink 1.0 in the arduino GUI build @@ -14,8 +15,13 @@ #define MAVLINK_START_UART_SEND(chan, size) comm_send_lock(chan, size) #define MAVLINK_END_UART_SEND(chan, size) comm_send_unlock(chan) +#if AP_NETWORKING_ENABLED +// allow 7 telemetry ports with networking +#define MAVLINK_COMM_NUM_BUFFERS 7 +#else // allow five telemetry ports #define MAVLINK_COMM_NUM_BUFFERS 5 +#endif #define MAVLINK_GET_CHANNEL_BUFFER 1 #define MAVLINK_GET_CHANNEL_STATUS 1 diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index fac080b717a4e6..fb85273f26eccb 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -171,7 +171,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Values{Copter}: 69:POSHOLD Mode // @Values{Copter}: 70:ALTHOLD Mode // @Values{Copter}: 71:FLOWHOLD Mode - // @Values{Copter,Plane}: 72:CIRCLE Mode + // @Values{Copter,Rover,Plane}: 72:CIRCLE Mode // @Values{Copter}: 73:DRIFT Mode // @Values{Rover}: 74:Sailboat motoring 3pos // @Values{Copter}: 75:SurfaceTrackingUpDown diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index ac45d68828cdd8..61eeaaecc70616 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/libraries/SITL/SIM_GPS.cpp @@ -986,198 +986,447 @@ uint32_t GPS_NOVA::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_ } void GPS_GSOF::publish(const GPS_Data *d) -{ - // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25 - constexpr uint8_t GSOF_POS_TIME_TYPE { 0x01 }; - constexpr uint8_t GSOF_POS_TIME_LEN { 0x0A }; - // TODO magic number until SITL supports GPS bootcount based on GPSN_ENABLE - const uint8_t bootcount = 17; - - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201 - enum class POS_FLAGS_1 : uint8_t { - NEW_POSITION = 1U << 0, - CLOCK_FIX_CALULATED = 1U << 1, - HORIZ_FROM_THIS_POS = 1U << 2, - HEIGHT_FROM_THIS_POS = 1U << 3, - RESERVED_4 = 1U << 4, - LEAST_SQ_POSITION = 1U << 5, - RESERVED_6 = 1U << 6, - POSITION_L1_PSEUDORANGES = 1U << 7 - }; - const uint8_t pos_flags_1 { - uint8_t(POS_FLAGS_1::NEW_POSITION) | - uint8_t(POS_FLAGS_1::CLOCK_FIX_CALULATED) | - uint8_t(POS_FLAGS_1::HORIZ_FROM_THIS_POS) | - uint8_t(POS_FLAGS_1::HEIGHT_FROM_THIS_POS) | - uint8_t(POS_FLAGS_1::RESERVED_4) | - uint8_t(POS_FLAGS_1::LEAST_SQ_POSITION) | - uint8_t(POS_FLAGS_1::POSITION_L1_PSEUDORANGES) - }; +{ + // This logic is to populate output buffer only with enabled channels. + // It also only sends each channel at the configured rate. + const uint64_t now = AP_HAL::millis(); + uint8_t buf[MAX_PAYLOAD_SIZE] = {}; + uint8_t payload_sz = 0; + uint8_t offset = 0; + if (channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)] != Output_Rate::OFF){ + const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)]; + const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)]; + + if (now - last_time > RateToPeriodMs(desired_rate)) { + + // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25 + constexpr uint8_t GSOF_POS_TIME_LEN { 0x0A }; + // TODO magic number until SITL supports GPS bootcount based on GPSN_ENABLE + const uint8_t bootcount = 17; + + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201 + enum class POS_FLAGS_1 : uint8_t { + NEW_POSITION = 1U << 0, + CLOCK_FIX_CALULATED = 1U << 1, + HORIZ_FROM_THIS_POS = 1U << 2, + HEIGHT_FROM_THIS_POS = 1U << 3, + RESERVED_4 = 1U << 4, + LEAST_SQ_POSITION = 1U << 5, + RESERVED_6 = 1U << 6, + POSITION_L1_PSEUDORANGES = 1U << 7 + }; + const uint8_t pos_flags_1 { + uint8_t(POS_FLAGS_1::NEW_POSITION) | + uint8_t(POS_FLAGS_1::CLOCK_FIX_CALULATED) | + uint8_t(POS_FLAGS_1::HORIZ_FROM_THIS_POS) | + uint8_t(POS_FLAGS_1::HEIGHT_FROM_THIS_POS) | + uint8_t(POS_FLAGS_1::RESERVED_4) | + uint8_t(POS_FLAGS_1::LEAST_SQ_POSITION) | + uint8_t(POS_FLAGS_1::POSITION_L1_PSEUDORANGES) + }; + + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202 + enum class POS_FLAGS_2 : uint8_t { + DIFFERENTIAL_POS = 1U << 0, + DIFFERENTIAL_POS_PHASE_RTK = 1U << 1, + POSITION_METHOD_FIXED_PHASE = 1U << 2, + OMNISTAR_ACTIVE = 1U << 3, + DETERMINED_WITH_STATIC_CONSTRAINT = 1U << 4, + NETWORK_RTK = 1U << 5, + DITHERED_RTK = 1U << 6, + BEACON_DGNSS = 1U << 7, + }; + + // Simulate a GPS without RTK in SIM since there is no RTK SIM params. + // This means these flags are unset: + // NETWORK_RTK, DITHERED_RTK, BEACON_DGNSS + uint8_t pos_flags_2 {0}; + if(d->have_lock) { + pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS); + pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS_PHASE_RTK); + pos_flags_2 |= uint8_t(POS_FLAGS_2::POSITION_METHOD_FIXED_PHASE); + pos_flags_2 |= uint8_t(POS_FLAGS_2::OMNISTAR_ACTIVE); + pos_flags_2 |= uint8_t(POS_FLAGS_2::DETERMINED_WITH_STATIC_CONSTRAINT); + } - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202 - enum class POS_FLAGS_2 : uint8_t { - DIFFERENTIAL_POS = 1U << 0, - DIFFERENTIAL_POS_PHASE_RTK = 1U << 1, - POSITION_METHOD_FIXED_PHASE = 1U << 2, - OMNISTAR_ACTIVE = 1U << 3, - DETERMINED_WITH_STATIC_CONSTRAINT = 1U << 4, - NETWORK_RTK = 1U << 5, - DITHERED_RTK = 1U << 6, - BEACON_DGNSS = 1U << 7, - }; + const auto gps_tow = gps_time(); + const struct PACKED gsof_pos_time { + const uint8_t OUTPUT_RECORD_TYPE; + const uint8_t RECORD_LEN; + uint32_t time_week_ms; + uint16_t time_week; + uint8_t num_sats; + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201 + uint8_t pos_flags_1; + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202 + uint8_t pos_flags_2; + uint8_t initialized_num; + } pos_time { + uint8_t(Gsof_Msg_Record_Type::POSITION_TIME), + GSOF_POS_TIME_LEN, + htobe32(gps_tow.ms), + htobe16(gps_tow.week), + d->have_lock ? _sitl->gps_numsats[instance] : uint8_t(3), + pos_flags_1, + pos_flags_2, + bootcount + }; + static_assert(sizeof(gsof_pos_time) - (sizeof(gsof_pos_time::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_time::RECORD_LEN)) == GSOF_POS_TIME_LEN); + + payload_sz += sizeof(pos_time); + memcpy(&buf[offset], &pos_time, sizeof(pos_time)); + offset += sizeof(pos_time); + } + } - // Simulate a GPS without RTK in SIM since there is no RTK SIM params. - // This means these flags are unset: - // NETWORK_RTK, DITHERED_RTK, BEACON_DGNSS - uint8_t pos_flags_2 {0}; - if(d->have_lock) { - pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS); - pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS_PHASE_RTK); - pos_flags_2 |= uint8_t(POS_FLAGS_2::POSITION_METHOD_FIXED_PHASE); - pos_flags_2 |= uint8_t(POS_FLAGS_2::OMNISTAR_ACTIVE); - pos_flags_2 |= uint8_t(POS_FLAGS_2::DETERMINED_WITH_STATIC_CONSTRAINT); + if (channel_rates[uint8_t(Gsof_Msg_Record_Type::LLH)] != Output_Rate::OFF){ + const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::LLH)]; + const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::LLH)]; + + if (now - last_time > RateToPeriodMs(desired_rate)) { + // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_LLH.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____20 + constexpr uint8_t GSOF_POS_LEN = 0x18; + + const struct PACKED gsof_pos { + const uint8_t OUTPUT_RECORD_TYPE; + const uint8_t RECORD_LEN; + uint64_t lat; + uint64_t lng; + uint64_t alt; + } pos { + uint8_t(Gsof_Msg_Record_Type::LLH), + GSOF_POS_LEN, + gsof_pack_double(d->latitude * DEG_TO_RAD_DOUBLE), + gsof_pack_double(d->longitude * DEG_TO_RAD_DOUBLE), + gsof_pack_double(static_cast(d->altitude)) + }; + static_assert(sizeof(gsof_pos) - (sizeof(gsof_pos::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos::RECORD_LEN)) == GSOF_POS_LEN); + + payload_sz += sizeof(pos); + memcpy(&buf[offset], &pos, sizeof(pos)); + offset += sizeof(pos); + } } - const auto gps_tow = gps_time(); - const struct PACKED gsof_pos_time { - const uint8_t OUTPUT_RECORD_TYPE; - const uint8_t RECORD_LEN; - uint32_t time_week_ms; - uint16_t time_week; - uint8_t num_sats; - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201 - uint8_t pos_flags_1; - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202 - uint8_t pos_flags_2; - uint8_t initialized_num; - } pos_time { - GSOF_POS_TIME_TYPE, - GSOF_POS_TIME_LEN, - htobe32(gps_tow.ms), - htobe16(gps_tow.week), - d->have_lock ? _sitl->gps_numsats[instance] : uint8_t(3), - pos_flags_1, - pos_flags_2, - bootcount - }; - static_assert(sizeof(gsof_pos_time) - (sizeof(gsof_pos_time::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_time::RECORD_LEN)) == GSOF_POS_TIME_LEN); - - constexpr uint8_t GSOF_POS_TYPE = 0x02; - constexpr uint8_t GSOF_POS_LEN = 0x18; - - const struct PACKED gsof_pos { - const uint8_t OUTPUT_RECORD_TYPE; - const uint8_t RECORD_LEN; - uint64_t lat; - uint64_t lng; - uint64_t alt; - } pos { - GSOF_POS_TYPE, - GSOF_POS_LEN, - gsof_pack_double(d->latitude * DEG_TO_RAD_DOUBLE), - gsof_pack_double(d->longitude * DEG_TO_RAD_DOUBLE), - gsof_pack_double(static_cast(d->altitude)) - }; - static_assert(sizeof(gsof_pos) - (sizeof(gsof_pos::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos::RECORD_LEN)) == GSOF_POS_LEN); - - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Velocity.html - constexpr uint8_t GSOF_VEL_TYPE = 0x08; - // use the smaller packet by ignoring local coordinate system - constexpr uint8_t GSOF_VEL_LEN = 0x0D; - - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags - enum class VEL_FIELDS : uint8_t { - VALID = 1U << 0, - CONSECUTIVE_MEASUREMENTS = 1U << 1, - HEADING_VALID = 1U << 2, - RESERVED_3 = 1U << 3, - RESERVED_4 = 1U << 4, - RESERVED_5 = 1U << 5, - RESERVED_6 = 1U << 6, - RESERVED_7 = 1U << 7, - }; - uint8_t vel_flags {0}; - if(d->have_lock) { - vel_flags |= uint8_t(VEL_FIELDS::VALID); - vel_flags |= uint8_t(VEL_FIELDS::CONSECUTIVE_MEASUREMENTS); - vel_flags |= uint8_t(VEL_FIELDS::HEADING_VALID); + if (channel_rates[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)] != Output_Rate::OFF){ + const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)]; + const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)]; + + if (now - last_time > RateToPeriodMs(desired_rate)) { + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Velocity.html + // use the smaller packet by ignoring local coordinate system + constexpr uint8_t GSOF_VEL_LEN = 0x0D; + + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags + enum class VEL_FIELDS : uint8_t { + VALID = 1U << 0, + CONSECUTIVE_MEASUREMENTS = 1U << 1, + HEADING_VALID = 1U << 2, + RESERVED_3 = 1U << 3, + RESERVED_4 = 1U << 4, + RESERVED_5 = 1U << 5, + RESERVED_6 = 1U << 6, + RESERVED_7 = 1U << 7, + }; + uint8_t vel_flags {0}; + if(d->have_lock) { + vel_flags |= uint8_t(VEL_FIELDS::VALID); + vel_flags |= uint8_t(VEL_FIELDS::CONSECUTIVE_MEASUREMENTS); + vel_flags |= uint8_t(VEL_FIELDS::HEADING_VALID); + } + + const struct PACKED gsof_vel { + const uint8_t OUTPUT_RECORD_TYPE; + const uint8_t RECORD_LEN; + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags + uint8_t flags; + uint32_t horiz_m_p_s; + uint32_t heading_rad; + uint32_t vertical_m_p_s; + } vel { + uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA), + GSOF_VEL_LEN, + vel_flags, + gsof_pack_float(d->speed_2d()), + gsof_pack_float(d->heading()), + // Trimble API has ambiguous direction here. + // Intentionally narrow from double. + gsof_pack_float(static_cast(d->speedD)) + }; + static_assert(sizeof(gsof_vel) - (sizeof(gsof_vel::OUTPUT_RECORD_TYPE) + sizeof(gsof_vel::RECORD_LEN)) == GSOF_VEL_LEN); + + payload_sz += sizeof(vel); + memcpy(&buf[offset], &vel, sizeof(vel)); + offset += sizeof(vel); + } + } + if (channel_rates[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)] != Output_Rate::OFF){ + const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)]; + const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)]; + + if (now - last_time > RateToPeriodMs(desired_rate)) { + // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_PDOP.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____12 + constexpr uint8_t GSOF_DOP_LEN = 0x10; + const struct PACKED gsof_dop { + const uint8_t OUTPUT_RECORD_TYPE { uint8_t(Gsof_Msg_Record_Type::PDOP_INFO) }; + const uint8_t RECORD_LEN { GSOF_DOP_LEN }; + uint32_t pdop = htobe32(1); + uint32_t hdop = htobe32(1); + uint32_t vdop = htobe32(1); + uint32_t tdop = htobe32(1); + } dop {}; + // Check the payload size calculation in the compiler + constexpr auto dop_size = sizeof(gsof_dop); + static_assert(dop_size == 18); + constexpr auto dop_record_type_size = sizeof(gsof_dop::OUTPUT_RECORD_TYPE); + static_assert(dop_record_type_size == 1); + constexpr auto len_size = sizeof(gsof_dop::RECORD_LEN); + static_assert(len_size == 1); + constexpr auto dop_payload_size = dop_size - (dop_record_type_size + len_size); + static_assert(dop_payload_size == GSOF_DOP_LEN); + + payload_sz += sizeof(dop); + memcpy(&buf[offset], &dop, sizeof(dop)); + offset += sizeof(dop); + } + } + if (channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)] != Output_Rate::OFF){ + const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)]; + const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)]; + if (now - last_time > RateToPeriodMs(desired_rate)) { + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_SIGMA.html + constexpr uint8_t GSOF_POS_SIGMA_LEN = 0x26; + const struct PACKED gsof_pos_sigma { + const uint8_t OUTPUT_RECORD_TYPE { uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO) }; + const uint8_t RECORD_LEN { GSOF_POS_SIGMA_LEN }; + uint32_t pos_rms = htobe32(0); + uint32_t sigma_e = htobe32(0); + uint32_t sigma_n = htobe32(0); + uint32_t cov_en = htobe32(0); + uint32_t sigma_up = htobe32(0); + uint32_t semi_major_axis = htobe32(0); + uint32_t semi_minor_axis = htobe32(0); + uint32_t orientation = htobe32(0); + uint32_t unit_variance = htobe32(0); + uint16_t n_epocs = htobe32(1); // Always 1 for kinematic. + } pos_sigma {}; + static_assert(sizeof(gsof_pos_sigma) - (sizeof(gsof_pos_sigma::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_sigma::RECORD_LEN)) == GSOF_POS_SIGMA_LEN); + payload_sz += sizeof(pos_sigma); + memcpy(&buf[offset], &pos_sigma, sizeof(pos_sigma)); + offset += sizeof(pos_sigma); + } } - const struct PACKED gsof_vel { - const uint8_t OUTPUT_RECORD_TYPE; - const uint8_t RECORD_LEN; - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags - uint8_t flags; - uint32_t horiz_m_p_s; - uint32_t heading_rad; - uint32_t vertical_m_p_s; - } vel { - GSOF_VEL_TYPE, - GSOF_VEL_LEN, - vel_flags, - gsof_pack_float(d->speed_2d()), - gsof_pack_float(d->heading()), - // Trimble API has ambiguous direction here. - // Intentionally narrow from double. - gsof_pack_float(static_cast(d->speedD)) - }; - static_assert(sizeof(gsof_vel) - (sizeof(gsof_vel::OUTPUT_RECORD_TYPE) + sizeof(gsof_vel::RECORD_LEN)) == GSOF_VEL_LEN); - - // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_PDOP.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____12 - constexpr uint8_t GSOF_DOP_TYPE = 0x09; - constexpr uint8_t GSOF_DOP_LEN = 0x10; - const struct PACKED gsof_dop { - const uint8_t OUTPUT_RECORD_TYPE { GSOF_DOP_TYPE }; - const uint8_t RECORD_LEN { GSOF_DOP_LEN }; - uint32_t pdop = htobe32(1); - uint32_t hdop = htobe32(1); - uint32_t vdop = htobe32(1); - uint32_t tdop = htobe32(1); - } dop {}; - // Check the payload size calculation in the compiler - constexpr auto dop_size = sizeof(gsof_dop); - static_assert(dop_size == 18); - constexpr auto dop_record_type_size = sizeof(gsof_dop::OUTPUT_RECORD_TYPE); - static_assert(dop_record_type_size == 1); - constexpr auto len_size = sizeof(gsof_dop::RECORD_LEN); - static_assert(len_size == 1); - constexpr auto dop_payload_size = dop_size - (dop_record_type_size + len_size); - static_assert(dop_payload_size == GSOF_DOP_LEN); - - constexpr uint8_t GSOF_POS_SIGMA_TYPE = 0x0C; - constexpr uint8_t GSOF_POS_SIGMA_LEN = 0x26; - const struct PACKED gsof_pos_sigma { - const uint8_t OUTPUT_RECORD_TYPE { GSOF_POS_SIGMA_TYPE }; - const uint8_t RECORD_LEN { GSOF_POS_SIGMA_LEN }; - uint32_t pos_rms = htobe32(0); - uint32_t sigma_e = htobe32(0); - uint32_t sigma_n = htobe32(0); - uint32_t cov_en = htobe32(0); - uint32_t sigma_up = htobe32(0); - uint32_t semi_major_axis = htobe32(0); - uint32_t semi_minor_axis = htobe32(0); - uint32_t orientation = htobe32(0); - uint32_t unit_variance = htobe32(0); - uint16_t n_epocs = htobe32(1); // Always 1 for kinematic. - } pos_sigma {}; - static_assert(sizeof(gsof_pos_sigma) - (sizeof(gsof_pos_sigma::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_sigma::RECORD_LEN)) == GSOF_POS_SIGMA_LEN); - - // TODO add GSOF49 - const uint8_t payload_sz = sizeof(pos_time) + sizeof(pos) + sizeof(vel) + sizeof(dop) + sizeof(pos_sigma); - uint8_t buf[payload_sz] = {}; - uint8_t offset = 0; - memcpy(&buf[offset], &pos_time, sizeof(pos_time)); - offset += sizeof(pos_time); - memcpy(&buf[offset], &pos, sizeof(pos)); - offset += sizeof(pos); - memcpy(&buf[offset], &vel, sizeof(vel)); - offset += sizeof(vel); - memcpy(&buf[offset], &dop, sizeof(dop)); - offset += sizeof(dop); - memcpy(&buf[offset], &pos_sigma, sizeof(pos_sigma)); - offset += sizeof(pos_sigma); assert(offset == payload_sz); - send_gsof(buf, sizeof(buf)); + + // Don't send empy frames when all channels are disabled; + if (payload_sz > 0) { + send_gsof(buf, payload_sz); + } + +} + +bool DCOL_Parser::dcol_parse(const char data_in) { + bool ret = false; + switch (parse_state) { + case Parse_State::WAITING_ON_STX: + if (data_in == STX) { + reset(); + parse_state = Parse_State::WAITING_ON_STATUS; + } + break; + case Parse_State::WAITING_ON_STATUS: + if (data_in != (uint8_t)Status::OK) { + // Invalid, status should always be OK. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } else { + status = static_cast(data_in); + parse_state = Parse_State::WAITING_ON_PACKET_TYPE; + } + break; + case Parse_State::WAITING_ON_PACKET_TYPE: + if (data_in == (uint8_t)Packet_Type::COMMAND_APPFILE) { + packet_type = Packet_Type::COMMAND_APPFILE; + } else { + // Unsupported command in this simulator. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + parse_state = Parse_State::WAITING_ON_LENGTH; + break; + case Parse_State::WAITING_ON_LENGTH: + expected_payload_length = data_in; + parse_state = Parse_State::WAITING_ON_PACKET_DATA; + break; + case Parse_State::WAITING_ON_PACKET_DATA: + payload[cur_payload_idx] = data_in; + if (++cur_payload_idx == expected_payload_length) { + parse_state = Parse_State::WAITING_ON_CSUM; + } + break; + case Parse_State::WAITING_ON_CSUM: + expected_csum = data_in; + parse_state = Parse_State::WAITING_ON_ETX; + break; + case Parse_State::WAITING_ON_ETX: + if (data_in != ETX) { + reset(); + } + if (!valid_csum()) { + // GSOF driver sent a packet with invalid CSUM. + // In real GSOF driver, the packet is simply ignored with no reply. + // In the SIM, treat this as a coding error. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } else { + ret = parse_payload(); + } + reset(); + break; + } + + return ret; +} + +uint32_t DCOL_Parser::RateToPeriodMs(const Output_Rate rate) { + uint32_t min_period_ms = 0; + switch (rate) { + case Output_Rate::OFF: + min_period_ms = 0; + break; + case Output_Rate::FREQ_10_HZ: + min_period_ms = 100; + break; + case Output_Rate::FREQ_50_HZ: + min_period_ms = 20; + break; + case Output_Rate::FREQ_100_HZ: + min_period_ms = 10; + break; + } + return min_period_ms; +} + + +bool DCOL_Parser::valid_csum() { + uint8_t sum = (uint8_t)status; + sum += (uint8_t)packet_type; + sum += expected_payload_length; + sum += crc_sum_of_bytes(payload, expected_payload_length); + return sum == expected_csum; +} + +bool DCOL_Parser::parse_payload() { + bool success = false; + if (packet_type == Packet_Type::COMMAND_APPFILE) { + success = parse_cmd_appfile(); + } + + return success; +} + +bool DCOL_Parser::parse_cmd_appfile() { + // A file control info block contains: + // * version + // * device type + // * start application file flag + // * factory settings flag + constexpr uint8_t file_control_info_block_sz = 4; + // An appfile header contains: + // * transmisison number + // * page index + // * max page index + constexpr uint8_t appfile_header_sz = 3; + constexpr uint8_t min_cmd_appfile_sz = file_control_info_block_sz + appfile_header_sz; + if (expected_payload_length < min_cmd_appfile_sz) { + return false; + } + + // For now, ignore appfile_trans_num, return success regardless. + // If the driver doesn't send the right value, it's not clear what the behavior is supposed to be. + // Also would need to handle re-sync. + // For now, just store it for debugging. + appfile_trans_num = payload[0]; + + // File control information block parsing: + // https://receiverhelp.trimble.com/oem-gnss/ICD_Subtype_Command64h_FileControlInfo.html + constexpr uint8_t expected_app_file_spec_version = 0x03; + constexpr uint8_t file_ctrl_idx = appfile_header_sz; + if (payload[file_ctrl_idx] != expected_app_file_spec_version) { + // Only version 3 is supported at this time. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + constexpr uint8_t expected_dev_type = 0x00; + if (payload[file_ctrl_idx+1] != expected_dev_type) { + // Only "all" device type is supported. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + constexpr uint8_t expected_start_flag = 0x01; + if (payload[file_ctrl_idx+2] != expected_start_flag) { + // Only "immediate" start type is supported. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + + constexpr uint8_t expected_factory_settings_flag = 0x00; + if (payload[file_ctrl_idx+3] != expected_factory_settings_flag) { + // Factory settings restore before appfile is not supported. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + constexpr uint8_t app_file_records_idx = appfile_header_sz + file_control_info_block_sz; + const uint8_t record_type = payload[app_file_records_idx]; + if (record_type == (uint8_t)Appfile_Record_Type::SERIAL_PORT_BAUD_RATE_FORMAT) { + // Serial port baud/format + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_SerialPort.html + + // Ignore serial port index (COM Port) since there's only one in SITL. + // Ignore baud rate because you can't change baud yet in SITL. + // Ignore parity because it can't be changed in SITL. + // Ignore flow control because it's not yet in SITL. + } else if (record_type == (uint8_t)Appfile_Record_Type::OUTPUT_MESSAGE){ + // Output Message + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html + + + // Ignore record length to save flash. + // Ignore port index since SITL is only one port. + if (payload[app_file_records_idx + 2] == (uint8_t)(Output_Msg_Msg_Type::GSOF)) { + const auto gsof_submessage_type = payload[app_file_records_idx + 6]; + const auto rate = payload[app_file_records_idx + 4]; + if (rate == (uint8_t)Output_Rate::OFF) { + channel_rates[gsof_submessage_type] = static_cast(rate); + } else if (rate == (uint8_t)Output_Rate::FREQ_10_HZ) { + channel_rates[gsof_submessage_type] = static_cast(rate); + } else if (rate == (uint8_t)Output_Rate::FREQ_50_HZ) { + channel_rates[gsof_submessage_type] = static_cast(rate); + } else if (rate == (uint8_t)Output_Rate::FREQ_100_HZ) { + channel_rates[gsof_submessage_type] = static_cast(rate); + } else { + // Unsupported GSOF rate in SITL. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + } else { + // Only some data output protocols are supported in SITL. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + } else { + // Other application file packets are not yet supported. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + return true; +} + +void DCOL_Parser::reset() { + cur_payload_idx = 0; + expected_payload_length = 0; + parse_state = Parse_State::WAITING_ON_STX; + // To be pedantic, one could zero the bytes in the payload, + // but this is skipped because it's extra CPU. + + // Note - appfile_trans_number is intended to persist over parser resets. } @@ -1189,7 +1438,6 @@ void GPS_GSOF::send_gsof(const uint8_t *buf, const uint16_t size) // * A fixed-length packet trailer (dcol_trailer) // Reference: // https://receiverhelp.trimble.com/oem-gnss/index.html#API_DataCollectorFormatPacketStructure.html?TocPath=API%2520Documentation%257CData%2520collector%2520format%2520packets%257CData%2520collector%2520format%253A%2520packet%2520structure%257C_____0 - const uint8_t STX = 0x02; // status bitfield // https://receiverhelp.trimble.com/oem-gnss/index.html#API_ReceiverStatusByte.html?TocPath=API%2520Documentation%257CData%2520collector%2520format%2520packets%257CData%2520collector%2520format%253A%2520packet%2520structure%257C_____1 const uint8_t STATUS = 0xa8; @@ -1210,8 +1458,8 @@ void GPS_GSOF::send_gsof(const uint8_t *buf, const uint16_t size) }; ++TRANSMISSION_NUMBER; - // A captured GSOF49 packet from BD940 has LENGTH field set to 0x6d = 109 bytes. - // A captured GSOF49 packet from BD940 has total bytes of 115 bytes. + // A captured GSOF49 packet from BD940 has LENGTH field set to 0x6d = 109 bytes. + // A captured GSOF49 packet from BD940 has total bytes of 115 bytes. // Thus, the following 5 bytes are not counted. // 1) STX // 2) STATUS @@ -1242,7 +1490,6 @@ void GPS_GSOF::send_gsof(const uint8_t *buf, const uint16_t size) csum += buf[i]; } - constexpr uint8_t ETX = 0x03; const uint8_t dcol_trailer[2] = { csum, ETX @@ -1277,6 +1524,22 @@ uint32_t GPS_GSOF::gsof_pack_float(const float& src) return dst; } +void GPS_GSOF::update_read() { + // Technically, the max command is slightly larger. + // This will only slightly slow the response for packets that big. + char c[MAX_PAYLOAD_SIZE]; + const auto n_read = read_from_autopilot(c, MAX_PAYLOAD_SIZE); + if (n_read > 0) { + for (uint8_t i = 0; i < n_read; i++) { + if (dcol_parse(c[i])) { + constexpr uint8_t response[1] = {(uint8_t)Command_Response::ACK}; + write_to_autopilot((char*)response, sizeof(response)); + } + // TODO handle NACK for failure + } + } +} + /* send MSP GPS data */ @@ -1516,13 +1779,16 @@ void GPS::update() // simulate delayed lock times bool have_lock = (!_sitl->gps_disable[idx] && now_ms >= _sitl->gps_lock_time[idx]*1000UL); - // run at configured GPS rate (default 5Hz) - if ((now_ms - last_update) < (uint32_t)(1000/_sitl->gps_hertz[idx])) { + // Only let physics run and GPS write at configured GPS rate (default 5Hz). + if ((now_ms - last_write_update_ms) < (uint32_t)(1000/_sitl->gps_hertz[instance])) { + // Reading runs every iteration. + // Beware- physics don't update every iteration with this approach. + // Currently, none of the drivers rely on quickly updated physics. backend->update_read(); return; } - last_update = now_ms; + last_write_update_ms = now_ms; d.latitude = latitude; d.longitude = longitude; @@ -1593,9 +1859,9 @@ void GPS::update() void GPS_Backend::update_read() { - // swallow any config bytes - char c; - read_from_autopilot(&c, 1); + // swallow any config bytes + char c; + read_from_autopilot(&c, 1); } /* diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h index 235234fd6ad7a0..c9f8c34e415480 100644 --- a/libraries/SITL/SIM_GPS.h +++ b/libraries/SITL/SIM_GPS.h @@ -98,13 +98,132 @@ class GPS_FILE : public GPS_Backend { void publish(const GPS_Data *d) override; }; -class GPS_GSOF : public GPS_Backend { +class DCOL_Parser { + // The DCOL parser is used by Trimble GSOF devices. + // It's used for doing configuration. + // https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPackets.html +public: + // Feed data in to the DCOL parser. + // If the data reaches a parse state that needs to write ACK/NACK back out, + // the function returns true with a populated data_out value. + // Otherwise, it returns false waiting for more data. + bool dcol_parse(const char data_in); + + static constexpr uint8_t STX = 0x02; + static constexpr uint8_t ETX = 0x03; + + // Receiver status code + enum class Status : uint8_t { + OK = 0x00, + }; + + // https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPackets.html + enum class Command_Response : uint8_t { + ACK = 0x06, + NACK = 0x15, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Frequenc + enum class Output_Rate : uint8_t { + OFF = 0, + FREQ_10_HZ = 1, + FREQ_50_HZ = 15, + FREQ_100_HZ = 16, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_ApplicationFilePackets.html?tocpath=API%20Documentation%7CCommand%20and%20report%20packets%7CApplication%20file%20packets%7C_____0 + enum class Packet_Type : uint8_t { + COMMAND_APPFILE = 0x64, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_Pkt_Command64h_APPFILE.html + enum class Appfile_Record_Type : uint8_t { + SERIAL_PORT_BAUD_RATE_FORMAT = 0x02, + OUTPUT_MESSAGE = 0x07, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output + enum class Output_Msg_Msg_Type : uint8_t { + GSOF = 10, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output2 + enum class Gsof_Msg_Record_Type : uint8_t { + POSITION_TIME = 1, + LLH = 2, + VELOCITY_DATA = 8, + PDOP_INFO = 9, + POSITION_SIGMA_INFO = 12, + }; + +protected: + // https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPacketStructure.html + static constexpr uint8_t MAX_PAYLOAD_SIZE = 255; + + // GSOF supports this many different packet types. + // Only a fraction are supported by the simulator. + // Waste some RAM and allocate arrays for the whole set. + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output2 + static constexpr uint8_t MAX_CHANNEL_NUM = 70; + // Rates of dynamically enabled channels. + // Assume factory behavior of no enabled channels. + // Each channel can send data out at its own rate. + Output_Rate channel_rates[MAX_CHANNEL_NUM] = {Output_Rate::OFF}; + + // Last publish time of dynamically enabled channels. + uint32_t last_publish_ms[MAX_CHANNEL_NUM]; + + static uint32_t RateToPeriodMs(const Output_Rate rate); + +private: + + // Internal parser implementation state + enum class Parse_State { + WAITING_ON_STX, + WAITING_ON_STATUS, + WAITING_ON_PACKET_TYPE, + WAITING_ON_LENGTH, + WAITING_ON_PACKET_DATA, + WAITING_ON_CSUM, + WAITING_ON_ETX, + }; + + bool valid_csum(); + bool parse_payload(); + // https://receiverhelp.trimble.com/oem-gnss/ICD_Pkt_Command64h_APPFILE.html + bool parse_cmd_appfile(); + + + // states for currently parsing packet + Status status; + Parse_State parse_state = {Parse_State::WAITING_ON_STX}; + Packet_Type packet_type; + // This is the length in the header. + uint8_t expected_payload_length; + // This is the increasing tally of bytes per packet. + uint8_t cur_payload_idx; + // This is the expected packet checksum in the trailer. + uint8_t expected_csum; + + // The application file record transmission number + uint8_t appfile_trans_num; + + uint8_t payload[MAX_PAYLOAD_SIZE]; + + // Clear all parser state/flags for handling a fresh packet. + void reset(); +}; + +class GPS_GSOF : public GPS_Backend, public DCOL_Parser { public: CLASS_NO_COPY(GPS_GSOF); using GPS_Backend::GPS_Backend; + + // GPS_Backend overrides void publish(const GPS_Data *d) override; + void update_read() override; private: void send_gsof(const uint8_t *buf, const uint16_t size); @@ -235,7 +354,8 @@ class GPS : public SerialDevice { int ext_fifo_fd; - uint32_t last_update; // milliseconds + // The last time GPS data was written [mS] + uint32_t last_write_update_ms; // last 20 samples, allowing for up to 20 samples of delay GPS_Data _gps_history[20]; diff --git a/libraries/SITL/SIM_IntelligentEnergy24.cpp b/libraries/SITL/SIM_IntelligentEnergy24.cpp index 2445df9a863a95..7b2456ee6b5838 100644 --- a/libraries/SITL/SIM_IntelligentEnergy24.cpp +++ b/libraries/SITL/SIM_IntelligentEnergy24.cpp @@ -29,25 +29,27 @@ extern const AP_HAL::HAL& hal; using namespace SITL; +#define MAX_TANK_PRESSURE 300 //(bar) + // table of user settable parameters const AP_Param::GroupInfo IntelligentEnergy24::var_info[] = { // @Param: ENABLE // @DisplayName: IntelligentEnergy 2.4kWh FuelCell sim enable/disable // @Description: Allows you to enable (1) or disable (0) the FuelCell simulator - // @Values: 0:Disabled,1:Enabled + // @Values: 0:Disabled,1:V1 Protocol,2:V2 Protocol // @User: Advanced AP_GROUPINFO("ENABLE", 1, IntelligentEnergy24, enabled, 0), // @Param: STATE // @DisplayName: Explicitly set state - // @Description: Explicity specify a state for the generator to be in + // @Description: Explicitly specify a state for the generator to be in // @User: Advanced AP_GROUPINFO("STATE", 2, IntelligentEnergy24, set_state, -1), // @Param: ERROR // @DisplayName: Explicitly set error code - // @Description: Explicity specify an error code to send to the generator + // @Description: Explicitly specify an error code to send to the generator // @User: Advanced AP_GROUPINFO("ERROR", 3, IntelligentEnergy24, err_code, 0), @@ -64,15 +66,14 @@ void IntelligentEnergy24::update(const struct sitl_input &input) if (!enabled.get()) { return; } - // gcs().send_text(MAV_SEVERITY_INFO, "fuelcell update"); update_send(); } void IntelligentEnergy24::update_send() { - // just send a chunk of data at 1Hz: + // just send a chunk of data at 2 Hz: const uint32_t now = AP_HAL::millis(); - if (now - last_sent_ms < 500) { + if (now - last_data_sent_ms < 500) { return; } @@ -80,7 +81,7 @@ void IntelligentEnergy24::update_send() float amps = discharge ? -20.0f : 20.0f; // Update pack capacity remaining - bat_capacity_mAh += amps*(now - last_sent_ms)/3600.0f; + bat_capacity_mAh += amps*(now - last_data_sent_ms)/3600.0f; // From capacity remaining approximate voltage by linear interpolation const float min_bat_vol = 42.0f; @@ -90,7 +91,7 @@ void IntelligentEnergy24::update_send() // Simulate tank pressure // Scale tank pressure linearly to a percentage. // Min = 5 bar, max = 300 bar, PRESS_GRAD = 1/295. - const int16_t tank_bar = linear_interpolate(5, 295, bat_capacity_mAh / max_bat_capactiy_mAh, 0, 1); + const int16_t tank_bar = linear_interpolate(5, MAX_TANK_PRESSURE, bat_capacity_mAh / max_bat_capactiy_mAh, 0, 1); battery_voltage = bat_capacity_mAh / max_bat_capactiy_mAh * (max_bat_vol - min_bat_vol) + min_bat_vol; @@ -112,10 +113,13 @@ void IntelligentEnergy24::update_send() state = 2; // Running } - last_sent_ms = now; + last_data_sent_ms = now; char message[128]; - hal.util->snprintf(message, ARRAY_SIZE(message), "<%i,%.1f,%i,%u,%i,%u,%u>\n", + + if (enabled.get() == 1) { + // V1 Protocol + hal.util->snprintf(message, ARRAY_SIZE(message), "<%i,%.1f,%i,%u,%i,%u,%u>\n", tank_bar, battery_voltage, (signed)pwr_out, @@ -124,7 +128,69 @@ void IntelligentEnergy24::update_send() (unsigned)state, (unsigned)err_code); + } else { + // V2 Protocol + + // version message sent at 0.2 Hz + if (now - last_ver_sent_ms > 5e3) { + // PCM software part number, software version number, protocol number, hardware serial number, check-sum + hal.util->snprintf(message, ARRAY_SIZE(message), "[10011867,2.132,4,IE12160A8040015,7]\n"); + + if ((unsigned)write_to_autopilot(message, strlen(message)) != strlen(message)) { + AP_HAL::panic("Failed to write to autopilot: %s", strerror(errno)); + } + last_ver_sent_ms = now; + } + + // data message + memset(&message, 0, sizeof(message)); + int8_t tank_remaining_pct = (float)tank_bar / MAX_TANK_PRESSURE * 100.0; + + hal.util->snprintf(message, ARRAY_SIZE(message), "<%i,%.2f,%.1f,%i,%u,%i,%i,%u,%u,%i,%s,", // last blank , is for fuel cell to send info string up to 32 char ASCII + tank_remaining_pct, + 0.67f, // inlet pressure (bar) + battery_voltage, + (signed)pwr_out, + (unsigned)spm_pwr, + 0, // unit at fault (0 = no fault) + (signed)battery_pwr, + (unsigned)state, + (unsigned)err_code, + 0, // fault state 2 (0 = no fault) + get_error_string(err_code)); + + // calculate the checksum + uint8_t checksum = 0; + for (uint8_t i = 0; i < ARRAY_SIZE(message); i++) { + if (message[i] == 0) { + break; + } + checksum += message[i]; + } + // checksum is inverted 8-bit + checksum = ~checksum; + + // add the checksum to the end of the message + char data_end[7]; + hal.util->snprintf(data_end, ARRAY_SIZE(data_end), "%u>\n", checksum); + strncat(message, data_end, ARRAY_SIZE(data_end)); + + } + if ((unsigned)write_to_autopilot(message, strlen(message)) != strlen(message)) { AP_HAL::panic("Failed to write to autopilot: %s", strerror(errno)); } } + +const char * IntelligentEnergy24::get_error_string(const uint32_t code) +{ + switch (code) { + case 20: + return "THERMAL MNGMT"; + + default: + break; + } + + return ""; +} diff --git a/libraries/SITL/SIM_IntelligentEnergy24.h b/libraries/SITL/SIM_IntelligentEnergy24.h index eecd0939e27d52..68d8782937f627 100644 --- a/libraries/SITL/SIM_IntelligentEnergy24.h +++ b/libraries/SITL/SIM_IntelligentEnergy24.h @@ -60,6 +60,8 @@ class IntelligentEnergy24 : public IntelligentEnergy { void update_send(); + const char * get_error_string(const uint32_t code); + AP_Int8 enabled; // enable sim AP_Int8 set_state; AP_Int32 err_code; @@ -67,7 +69,8 @@ class IntelligentEnergy24 : public IntelligentEnergy { float battery_voltage = 50.4f; float bat_capacity_mAh = 3300; bool discharge = true; // used to switch between battery charging and discharging - uint32_t last_sent_ms; + uint32_t last_data_sent_ms; + uint32_t last_ver_sent_ms; }; diff --git a/libraries/SITL/SIM_SerialDevice.h b/libraries/SITL/SIM_SerialDevice.h index 00f58ab93511b1..d04e573506c8ae 100644 --- a/libraries/SITL/SIM_SerialDevice.h +++ b/libraries/SITL/SIM_SerialDevice.h @@ -46,7 +46,7 @@ class SerialDevice { ByteBuffer *to_autopilot; ByteBuffer *from_autopilot; - bool init_sitl_pointer(); + bool init_sitl_pointer() WARN_IF_UNUSED; private: diff --git a/wscript b/wscript index a2e8a7347808c6..19630462ecdd70 100644 --- a/wscript +++ b/wscript @@ -360,10 +360,10 @@ configuration in order to save typing. default=False, help='Use flash storage emulation.') - g.add_option('--disable-ekf2', + g.add_option('--enable-ekf2', action='store_true', default=False, - help='Configure without EKF2.') + help='Configure with EKF2.') g.add_option('--disable-ekf3', action='store_true',