diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 90ebf5087c922..fd5a9a6c54c5f 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -7786,7 +7786,8 @@ def test_mount_rc_targetting(self, pitch_rc_neutral=1500, do_rate_tests=True, pi pitch_tolerance defaults to the original tight 0.1deg check; backends whose actuator has a coarser confirmed physical resolution (e.g. a rate-only actuator closing an angle loop via a quantized speed command) may need to - pass a wider value''' + pass a wider value - see MountSkyDroid()'s use of this for a concrete + example with the reasoning''' if True: self.context_push() self.set_parameters({ @@ -8460,6 +8461,172 @@ def MountTopotek(self): self.mount_test_body(pitch_rc_neutral=1818, do_rate_tests=False, constrain_sysid_target=False) + def MountSkyDroid(self): + '''test SkyDroid gimbal using SIM_SkyDroid simulator''' + # pitch_rc_neutral=1818: with RC6 min=1000 max=2000 trim=1500 and + # default MNT1_PITCH_MIN=-90 / MNT1_PITCH_MAX=20, norm_input=0.636 + # maps to exactly 0 deg pitch. + pitch_rc_neutral = 1818 + # centre RC6 *before* the parameter changes below reboot the FC. MNT1's + # default mode is RC_TARGETING, so without this the mount starts driving + # toward whatever angle RC6's un-centred default value maps to the moment + # it boots, well before the test gets to explicitly select NEUTRAL mode. + # Harmless for backends with a fast control loop (they recover from that + # transient inside the neutral check's 5s budget), but avoid causing a large + # transient in the first place anyway rather than rely on recovering from it + self.set_rc(6, pitch_rc_neutral) + self.set_parameters({ + "MNT1_TYPE": 15, # SkyDroid + "CAM1_TYPE": 4, # Mount + "SERIAL5_PROTOCOL": 8, # gimbal + "RC6_OPTION": 213, # MOUNT1_PITCH + }) + self.customise_SITL_commandline(["--serial5=sim:skydroid:"]) + # version "V1.0.0" from SIM_SkyDroid: major=1 | (minor=0)<<8 | (patch=0)<<16 = 1 + # cap flags: CAPTURE_VIDEO | CAPTURE_IMAGE | HAS_BASIC_ZOOM + self.mount_check_camera_information( + "SkyDroid", "C11", + expected_fw_version=1, + expected_cap_flags=0x43, + ) + # constrain_sysid_target=True (the default): unlike Topotek/Viewpro, + # AP_Mount_SkyDroid::send_target_angles does clamp pitch/yaw to the + # configured MNT1_PITCH/YAW_MIN/MAX before sending, so the 68-deg + # sysid test (which expects that clamp) is exercised here. + # neutral_tol_deg=3.5: confirmed on real hardware that SkyDroid gimbals only + # respond to the individual-axis GSY/GSP speed commands (GAM/GSM are silently + # ignored); since there's no working absolute-angle command, the driver closes + # an angle P-controller loop on top of them, so allow a little settling + # tolerance rather than the exact positioning an absolute-angle backend gives. + # + # rc_targetting_pitch_tolerance=0.3: GSY/GSP's wire value is a quantized 8bit + # signed LSB (see AP_MOUNT_SKYDROID_AXIS_DPS_PER_LSB, confirmed on real + # hardware via dataflash log analysis to be 0.5deg/s per LSB), which puts a + # genuine, measured floor of ~0.25deg of angular error below which the + # closed-loop P-controller's commanded rate rounds to 0 LSB and it simply + # stops correcting - this is a real actuator resolution limit, not a driver + # bug, and the shared test's default 0.1deg tolerance is tighter than this + # actuator can physically deliver. 0.3 gives a little margin above the + # measured ~0.25deg floor + self.mount_test_body(pitch_rc_neutral=pitch_rc_neutral, do_rate_tests=False, neutral_tol_deg=3.5, + rc_targetting_pitch_tolerance=0.3) + + def MountSkyDroidC13(self): + '''test SkyDroid C13 gimbal using SIM_SkyDroid simulator + + SkyDroid have confirmed the gimbal-control commands are IDENTICAL across + models - the C13's extra features over the C11 are infrared thermal imaging + and laser ranging, neither of which this driver uses. So this test is + deliberately the same body as MountSkyDroid(): it is a regression guard + that the driver stays model-independent, and that a differently-named model + does not take a different control path. It also asserts roll stays + uncontrollable even when MNT1_ROLL_MIN/MAX is configured, since SkyDroid + have confirmed roll is self-stabilized with no control command at all''' + # pitch_rc_neutral=1818: with RC6 min=1000 max=2000 trim=1500 and + # default MNT1_PITCH_MIN=-90 / MNT1_PITCH_MAX=20, norm_input=0.636 + # maps to exactly 0 deg pitch. + pitch_rc_neutral = 1818 + # centre RC6 *before* the parameter changes below reboot the FC - same fix as + # MountSkyDroid() needed, and for the same reason: see the comment there + self.set_rc(6, pitch_rc_neutral) + self.set_parameters({ + "MNT1_TYPE": 15, # SkyDroid + "CAM1_TYPE": 4, # Mount + "SERIAL5_PROTOCOL": 8, # gimbal + "RC6_OPTION": 213, # MOUNT1_PITCH + # deliberately configure a roll range the gimbal cannot actually use, to + # prove the driver still refuses to drive roll - see the roll check below + "MNT1_ROLL_MIN": -45, + "MNT1_ROLL_MAX": 45, + }) + self.customise_SITL_commandline(["--serial5=sim:skydroid_c13:"]) + # version "V1.0.0" from SIM_SkyDroid: major=1 | (minor=0)<<8 | (patch=0)<<16 = 1 + # cap flags: CAPTURE_VIDEO | CAPTURE_IMAGE | HAS_BASIC_ZOOM + # model name "C13" confirms the differently-named variant was actually selected + self.mount_check_camera_information( + "SkyDroid", "C13", + expected_fw_version=1, + expected_cap_flags=0x43, + ) + # identical expectations to MountSkyDroid() - that is the point of this test, + # rc_targetting_pitch_tolerance included - see MountSkyDroid()'s comment for + # why 0.3 rather than the shared default of 0.1 + self.mount_test_body(pitch_rc_neutral=pitch_rc_neutral, do_rate_tests=False, neutral_tol_deg=3.5, + rc_targetting_pitch_tolerance=0.3) + + # roll must NOT respond: SkyDroid have confirmed roll is self-stabilized by the + # gimbal with no control command on any model, so AP_Mount_SkyDroid reports + # has_roll_control() == false and never sends a roll command. This is the + # inverse of a test that used to live here, which drove roll via "GAR" and + # expected it to move - that command turned out not to be implemented in the + # firmware at all despite being in the protocol document + self.progress("Testing mount roll stays uncommanded (roll is not controllable)") + # mount_test_body() above ends with its own RTL+landing sequence, so the + # vehicle's resting attitude here is whatever it happens to land at - NOT + # guaranteed level. So this deliberately checks for CHANGE in response to the + # RC command, not an absolute near-zero value - the claim under test is "roll + # doesn't respond to input", which holds regardless of the vehicle's own + # attitude, unlike a fixed absolute-value check + self.context_push() + self.set_parameters({ + 'RC11_OPTION': 212, # MOUNT1_ROLL + }) + self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) + start_roll_deg, _, _, _ = self.get_mount_roll_pitch_yaw_deg() + self.set_rc(11, 1100) # would demand roll to the MNT1_ROLL_MIN extreme + tstart = self.get_sim_time() + max_roll_change_deg = 0 + while self.get_sim_time_cached() - tstart < 10: + mount_roll_deg, _, _, _ = self.get_mount_roll_pitch_yaw_deg() + self.progress("roll=%f (start was %f)" % (mount_roll_deg, start_roll_deg)) + max_roll_change_deg = max(max_roll_change_deg, abs(mount_roll_deg - start_roll_deg)) + self.set_rc(11, 1500) + self.context_pop() + # 15deg is well clear of the 45deg the RC input demands, while leaving room for + # whatever incidental roll change the gimbal's own stabilization shows as the + # vehicle moves - we are checking nothing *drives* roll, not that it is pinned + # at any particular value + if max_roll_change_deg > 15: + raise NotAchievedException( + "Mount roll changed %.1fdeg in response to RC input - roll should not be commandable" % + max_roll_change_deg) + + def MountSkyDroidNetwork(self): + '''test SkyDroid gimbal connected via a UDP network port rather than a serial port + + the real C11 hardware is UDP-only (no serial control interface), so this + exercises the actual transport used in the field rather than the SITL + serial-port path used by MountSkyDroid''' + self.set_parameters({ + "MNT1_TYPE": 15, # SkyDroid + "CAM1_TYPE": 4, # Mount + "NET_ENABLE": 1, + "NET_P1_TYPE": 1, # UDP client + "NET_P1_PROTOCOL": 8, # gimbal + "NET_P1_IP0": 127, + "NET_P1_IP1": 0, + "NET_P1_IP2": 0, + "NET_P1_IP3": 1, + "NET_P1_PORT": 15006, + }) + # the simulated gimbal listens on a UDP socket rather than + # being attached to one of the autopilot's serial ports: + self.customise_SITL_commandline(["--net-device=skydroid:15006,udp"]) + self.mount_check_camera_information( + "SkyDroid", "C11", + expected_fw_version=1, + expected_cap_flags=0x43, + ) + # command an angle and check the gimbal reports reaching it, + # which requires traffic in both directions: + self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p1=-30, # pitch angle in degrees + p7=mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, + ) + self.wait_mount_roll_pitch_yaw_deg(p=-30) + def MountTopotekNetwork(self): '''test Topotek gimbal connected via a network port rather than a serial port''' self.set_parameters({ @@ -20221,6 +20388,9 @@ def tests2b(self): # this block currently around 9.5mins here self.TakeoffWithLocation, self.MountTopotek, self.MountTopotekNetwork, + self.MountSkyDroid, + self.MountSkyDroidC13, + self.MountSkyDroidNetwork, self.MountViewPro, self.MountAVTCM62, self.MountAVTCM62Dual, diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 8518f7a2eeb11..c445584967d40 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -247,6 +247,7 @@ def config_option(self): Feature('Gimbal', 'SOLOGIMBAL', 'HAL_SOLO_GIMBAL_ENABLED', 'Enable Solo gimbal', 0, "MOUNT"), Feature('Gimbal', 'STORM32_MAVLINK', 'HAL_MOUNT_STORM32MAVLINK_ENABLED', 'Enable SToRM32 MAVLink gimbal', 0, "MOUNT"), Feature('Gimbal', 'STORM32_SERIAL', 'HAL_MOUNT_STORM32SERIAL_ENABLED', 'Enable SToRM32 Serial gimbal', 0, "MOUNT"), + Feature('Gimbal', 'SKYDROID', 'HAL_MOUNT_SKYDROID_ENABLED', 'Enable SkyDroid gimbal', 0, "MOUNT"), Feature('Gimbal', 'TOPOTEK', 'HAL_MOUNT_TOPOTEK_ENABLED', 'Enable Topotek gimbal', 0, "MOUNT"), Feature('Gimbal', 'XACTI', 'HAL_MOUNT_XACTI_ENABLED', 'Enable Xacti gimbal', 0, "MOUNT,DroneCAN"), Feature('Gimbal', 'XFROBOT', 'HAL_MOUNT_XFROBOT_ENABLED', 'Enable XFRobot gimbal', 0, "MOUNT"), diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp index dca94ada05d2e..121a465e5fed7 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.cpp +++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include @@ -243,6 +244,18 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const sitl_model->add_gimbal_sim(*topotek); return topotek; #endif // AP_SIM_TOPOTEK_ENABLED +#if AP_SIM_SKYDROID_ENABLED + } else if (streq(name, "skydroid")) { + const auto skydroid = NEW_NOTHROW SITL::SkyDroid("C11"); + sitl_model->add_gimbal_sim(*skydroid); + return skydroid; + } else if (streq(name, "skydroid_c13")) { + // the model name is the ONLY difference from "skydroid" above - SkyDroid have + // confirmed the gimbal-control commands are identical across models + const auto skydroid_c13 = NEW_NOTHROW SITL::SkyDroid("C13"); + sitl_model->add_gimbal_sim(*skydroid_c13); + return skydroid_c13; +#endif // AP_SIM_SKYDROID_ENABLED #if AP_SIM_VIEWPRO_ENABLED } else if (streq(name, "viewpro")) { const auto viewpro = NEW_NOTHROW SITL::Viewpro(); @@ -359,10 +372,16 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED /* - create a simulated device which the autopilot connects to over TCP - rather than over one of its simulated serial ports. This is used to - simulate devices attached to the autopilot's network ports (NET_Pn). - spec is of the form NAME:TCPPORT e.g. "topotek:15005" + create a simulated device which the autopilot connects to over the + network (TCP by default, or UDP) rather than over one of its + simulated serial ports. This is used to simulate devices attached + to the autopilot's network ports (NET_Pn). + spec is of the form NAME:PORT or NAME:PORT,OPTION,OPTION,..., e.g. + "topotek:15005" (TCP, the default) or "skydroid:15005,udp" - any + options beyond the port number are comma-separated from each other + (and from the port number), rather than each being tacked on with + another colon, since they're logically grouped with the port rather + than being another NAME-like top-level field */ void SITL_State_Common::create_net_serial_sim(const char *spec) { @@ -376,14 +395,27 @@ void SITL_State_Common::create_net_serial_sim(const char *spec) } char *saveptr = nullptr; const char *name = strtok_r(s, ":", &saveptr); - const char *port_str = strtok_r(nullptr, ":", &saveptr); - if (name == nullptr || port_str == nullptr) { - AP_HAL::panic("Bad network device (%s); expected NAME:TCPPORT", spec); + char *port_and_options = strtok_r(nullptr, ":", &saveptr); + if (name == nullptr || port_and_options == nullptr) { + AP_HAL::panic("Bad network device (%s); expected NAME:PORT[,PROTOCOL]", spec); + } + char *saveptr2 = nullptr; + const char *port_str = strtok_r(port_and_options, ",", &saveptr2); + const char *protocol_str = strtok_r(nullptr, ",", &saveptr2); // optional, defaults to "tcp" + if (port_str == nullptr) { + AP_HAL::panic("Bad network device (%s); expected NAME:PORT[,PROTOCOL]", spec); + } + const bool use_udp = (protocol_str != nullptr) && (strcasecmp(protocol_str, "udp") == 0); + if (protocol_str != nullptr && !use_udp && strcasecmp(protocol_str, "tcp") != 0) { + AP_HAL::panic("Bad network device protocol (%s); expected 'tcp' or 'udp'", protocol_str); } SITL::SerialDevice *device = create_serial_sim(name, nullptr, 0); - if (!device->listen_on_tcp_port(atoi(port_str))) { - AP_HAL::panic("Failed to attach %s to TCP port %s", name, port_str); + const bool ok = use_udp ? + device->listen_on_udp_port(atoi(port_str)) : + device->listen_on_tcp_port(atoi(port_str)); + if (!ok) { + AP_HAL::panic("Failed to attach %s to %s port %s", name, use_udp ? "UDP" : "TCP", port_str); } net_serial_sims[num_net_serial_sims++] = device; diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h index 88662d4c86971..7668b4d238ee2 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.h +++ b/libraries/AP_HAL_SITL/SITL_State_common.h @@ -28,6 +28,7 @@ #include #include +#include #include #include diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index b8f3f6ce08431..fdf478490e9a4 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -109,7 +109,7 @@ void SITL_State::_usage(void) "\t--serial8 device set device string for SERIAL8\n" "\t--serial9 device set device string for SERIAL9\n" "\t--uartA device alias for --serial0 (do not use)\n" - "\t--net-device NAME:PORT attach simulated device NAME to TCP port PORT rather than to a serial port\n" + "\t--net-device NAME:PORT[,udp] attach simulated device NAME to TCP (or, with ',udp', UDP) port PORT rather than to a serial port\n" "\t--base-port PORT set port num for base port(default 5670) must be before -I option\n" "\t--rc-in-port PORT set port num for rc in\n" "\t--sim-address ADDR set address string for simulator\n" @@ -267,7 +267,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) struct AP_Param::defaults_table_struct temp_cmdline_param{}; #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED - // NAME:TCPPORT strings from --net-device options: + // NAME:PORT[,udp] strings from --net-device options: const char *net_device_strings[4]; uint8_t num_net_device_strings = 0; #endif // AP_SIM_SERIALDEVICE_NETWORK_ENABLED diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 6b66439e01b5d..a751503827cd2 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -20,6 +20,7 @@ #include "AP_Mount_Topotek.h" #include "AP_Mount_CADDX.h" #include "AP_Mount_XFRobot.h" +#include "AP_Mount_SkyDroid.h" #include #include #include @@ -188,6 +189,15 @@ void AP_Mount::init() serial_instance++; break; #endif // HAL_MOUNT_XFROBOT_ENABLED + +#if HAL_MOUNT_SKYDROID_ENABLED + // check for SkyDroid gimbal + case Type::SkyDroid: + _backends[instance] = NEW_NOTHROW AP_Mount_SkyDroid(*this, _params[instance], instance, serial_instance); + _num_instances++; + serial_instance++; + break; +#endif // HAL_MOUNT_SKYDROID_ENABLED } // init new instance diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 2b601562b6770..83fdca535c72b 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -50,6 +50,7 @@ class AP_Mount_Viewpro; class AP_Mount_Topotek; class AP_Mount_CADDX; class AP_Mount_XFRobot; +class AP_Mount_SkyDroid; /* This is a workaround to allow the MAVLink backend access to the @@ -129,6 +130,9 @@ class AP_Mount #if HAL_MOUNT_XFROBOT_ENABLED XFRobot = 14, /// XFRobot gimbal using a custom serial protocol #endif +#if HAL_MOUNT_SKYDROID_ENABLED + SkyDroid = 15, /// SkyDroid gimbal using a custom serial protocol +#endif // HAL_MOUNT_SKYDROID_ENABLED }; // init - detect and initialise all mounts diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 653981f64c759..cc9e9524db182 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -1119,6 +1119,16 @@ void AP_Mount_Backend::update_angle_target_from_rate(const MountRateTarget& rate } } +// simple P-controller converting an angle error to a rate command - see this +// function's declaration for the full explanation +float AP_Mount_Backend::angle_error_to_rate(float error, float gain, float rate_max, float deadzone) +{ + if (fabsf(error) <= deadzone) { + return 0.0f; + } + return constrain_float(error * gain, -rate_max, rate_max); +} + // helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message uint16_t AP_Mount_Backend::get_gimbal_device_flags() const { diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 2cc192fe4c684..f1c4a693ab3ef 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -407,6 +407,18 @@ class AP_Mount_Backend // assumes a 50hz update rate void update_angle_target_from_rate(const MountRateTarget& rate_rad, MountAngleTarget& angle_rad) const; + // simple P-controller converting an angle error to a rate command: multiply + // by gain, then constrain to +/- rate_max. A non-zero deadzone forces a + // clean zero output for small errors instead of tapering to an + // ever-smaller command - useful for a rate actuator with a coarse + // resolution floor, where a command below that floor is simply rounded + // away by the wire encoding anyway, so continuing to send one just adds + // dither with no actual effect. Unit-agnostic: error, deadzone and + // rate_max must all share one consistent unit (e.g. all in deg, all in + // rad/s, or all as a normalised -100..100 scalar) - gain converts between + // error's unit and the returned rate's + static float angle_error_to_rate(float error, float gain, float rate_max, float deadzone = 0.0f); + // helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message uint16_t get_gimbal_device_flags() const; diff --git a/libraries/AP_Mount/AP_Mount_Backend_TPFrame.cpp b/libraries/AP_Mount/AP_Mount_Backend_TPFrame.cpp new file mode 100644 index 0000000000000..a789d43aa0358 --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_Backend_TPFrame.cpp @@ -0,0 +1,277 @@ +#include "AP_Mount_config.h" + +#if HAL_MOUNT_TOPOTEK_ENABLED || HAL_MOUNT_SKYDROID_ENABLED + +#include "AP_Mount_Backend_TPFrame.h" + +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +#define AP_MOUNT_TPFRAME_DEBUG 0 +#define debug(fmt, args ...) do { if (AP_MOUNT_TPFRAME_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mount: " fmt, ## args); } } while (0) + +// reading incoming packets from gimbal and confirm they are of the correct format +void AP_Mount_Backend_TPFrame::read_incoming_packets() +{ + // check for bytes on the serial port + const uint16_t nbytes = MIN(_uart->available(), 1024U); + if (nbytes == 0) { + return; + } + + // flag to allow cases below to reset parser state + bool reset_parser = false; + + // process bytes received + for (uint16_t i = 0; i < nbytes; i++) { + uint8_t b; + if (!_uart->read(b)) { + continue; + } + + // add latest byte to buffer + _msg_buff[_msg_buff_len++] = b; + + // protect against overly long messages + if (_msg_buff_len >= packetlen_max()) { + reset_parser = true; + } + + // process byte depending upon current state + switch (_parser.state) { + + case ParseState::WAITING_FOR_HEADER1: + if (b == '#') { + _parser.state = ParseState::WAITING_FOR_HEADER2; + break; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_HEADER2: + // 't'/'T' (and 'p'/'P' below) distinguish HeaderType::VARIABLE_LEN from + // FIXED_LEN - meaningful on transmit (see send_variablelen_packet()), but + // deliberately not tracked here on receive: the packet's own Data_Len + // nibble is authoritative regardless of which header case was used to + // send it, so the parser has no need to remember which one it saw + if (b == 't' || b == 'T') { + _parser.state = ParseState::WAITING_FOR_HEADER3; + break; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_HEADER3: + if (b == 'p' || b == 'P') { + _parser.state = ParseState::WAITING_FOR_ADDR1; + break; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_ADDR1: + case ParseState::WAITING_FOR_ADDR2: + if (is_valid_address_byte(b)) { + // advance to next state + _parser.state = (ParseState)((uint8_t)_parser.state+1); + break; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_DATALEN: { + // sanity check data length + uint8_t data_len; + if (hex_char_to_nibble(b, data_len) && data_len <= datalen_max()) { + _parser.data_len = data_len; + _parser.state = ParseState::WAITING_FOR_CONTROL; + break; + } + reset_parser = true; + break; + } + + case ParseState::WAITING_FOR_CONTROL: + // r or w + if (b == 'r' || b == 'w') { + _parser.state = ParseState::WAITING_FOR_ID1; + break; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_ID1: + case ParseState::WAITING_FOR_ID2: + // check all uppercase letters and numbers. eg 'GAC' + if ((b >= 'A' && b <= 'Z') || (b >= '0' && b <= '9')) { + // advance to next state + _parser.state = (ParseState)((uint8_t)_parser.state+1); + break; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_ID3: + if ((b >= 'A' && b <= 'Z') || (b >= '0' && b <= '9')) { + // a zero-length data segment has no data bytes to wait for - the + // WAITING_FOR_DATA case below can only advance once + // data_bytes_received (which starts at 1 on the very next byte and + // only increases) equals _parser.data_len, which can never happen + // for data_len==0, so go straight to the CRC instead of getting + // stuck consuming the CRC (and then the next packet) as fake data + // until datalen_max() overflows and fires an avoidable INTERNAL_ERROR + _parser.state = (_parser.data_len == 0) ? ParseState::WAITING_FOR_CRC_LOW : ParseState::WAITING_FOR_DATA; + break; + } + reset_parser = true; + break; + + case ParseState::WAITING_FOR_DATA: { + // normally hex numbers in char form (e.g. '0A') + const uint8_t data_bytes_received = _msg_buff_len - (AP_MOUNT_TPFRAME_PACKETLEN_MIN - 2); + + // sanity check to protect against programming errors + if (data_bytes_received > datalen_max()) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + reset_parser = true; + break; + } + + // advance parser state once expected number of bytes have been received + if (data_bytes_received == _parser.data_len) { + _parser.state = ParseState::WAITING_FOR_CRC_LOW; + } + break; + } + + case ParseState::WAITING_FOR_CRC_LOW: + _parser.state = ParseState::WAITING_FOR_CRC_HIGH; + break; + + case ParseState::WAITING_FOR_CRC_HIGH: + // this is the last byte in the message so reset the parser + reset_parser = true; + + // sanity check to protect against programming errors + if (_msg_buff_len < AP_MOUNT_TPFRAME_PACKETLEN_MIN) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + break; + } + + // calculate and check CRC + const uint8_t crc_value = calculate_crc(_msg_buff, _msg_buff_len - 2); + const char crc_char1 = hex2char((crc_value >> 4) & 0x0f); + const char crc_char2 = hex2char((crc_value) & 0x0f); + if (crc_char1 != _msg_buff[_msg_buff_len - 2] || crc_char2 != _msg_buff[_msg_buff_len-1]) { + debug("CRC expected:%x got:%c%c", (int)crc_value, crc_char1, crc_char2); + break; + } + + // CRC is OK, dispatch on the 3-character command ID to the subclass + handle_message((const char*)&_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_ID]); + } + + // handle reset of parser + if (reset_parser) { + _parser.state = ParseState::WAITING_FOR_HEADER1; + _msg_buff_len = 0; + reset_parser = false; + } + } +} + +// calculate checksum +uint8_t AP_Mount_Backend_TPFrame::calculate_crc(const uint8_t *cmd, uint8_t len) const +{ + uint8_t crc = 0; + for (uint16_t i = 0; i= data)) { + return (data + '0'); + } else { + return (data - 10 + 'A'); + } +} + +// send a fixed length packet +bool AP_Mount_Backend_TPFrame::send_fixedlen_packet(uint8_t address, const Identifier id, bool write, uint8_t value) +{ + uint8_t databuff[3]; + hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02X", value); + return send_variablelen_packet(HeaderType::FIXED_LEN, address, id, write, databuff, ARRAY_SIZE(databuff)-1); +} + +// send variable length packet +bool AP_Mount_Backend_TPFrame::send_variablelen_packet(HeaderType header, uint8_t address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len) +{ + // exit immediately if not initialised + if (!_initialised) { + return false; + } + + // calculate and sanity check packet size + const uint16_t packet_size = AP_MOUNT_TPFRAME_PACKETLEN_MIN + databuff_len; + if (packet_size > packetlen_max()) { + debug("send_packet data buff too large"); + return false; + } + + // check for sufficient space in outgoing buffer + if (_uart->txspace() < packet_size) { + debug("tx buffer full"); + return false; + } + + // create buffer for holding outgoing packet + uint8_t send_buff[packet_size]; + uint8_t send_buff_ofs = 0; + + // packet header (bytes 0 ~ 2) + send_buff[send_buff_ofs++] = '#'; + send_buff[send_buff_ofs++] = (header == HeaderType::FIXED_LEN) ? 'T' : 't'; + send_buff[send_buff_ofs++] = (header == HeaderType::FIXED_LEN) ? 'P' : 'p'; + + // address (bytes 3, 4) + send_buff[send_buff_ofs++] = source_address_byte(); + send_buff[send_buff_ofs++] = address; + + // data length (byte 5) + send_buff[send_buff_ofs++] = hex2char(databuff_len); + + // control byte (byte 6) + send_buff[send_buff_ofs++] = write ? (uint8_t)ControlByte::WRITE : (uint8_t)ControlByte::READ; + + // identifier (bytes 7 ~ 9) + send_buff[send_buff_ofs++] = id[0]; + send_buff[send_buff_ofs++] = id[1]; + send_buff[send_buff_ofs++] = id[2]; + + // data + if (databuff_len != 0) { + memcpy(&send_buff[send_buff_ofs], databuff, databuff_len); + send_buff_ofs += databuff_len; + } + + // crc + uint8_t crc = calculate_crc(send_buff, send_buff_ofs); + send_buff[send_buff_ofs++] = hex2char((crc >> 4) & 0x0f); + send_buff[send_buff_ofs++] = hex2char(crc & 0x0f); + + // send packet. txspace() was already confirmed sufficient above, but + // callers (e.g. set_gimbal_lock()) latch success permanently on a true + // return, so a short write must be reported as failure rather than + // silently dropping the unsent tail of the packet + return _uart->write(send_buff, send_buff_ofs) == send_buff_ofs; +} + +#endif // HAL_MOUNT_TOPOTEK_ENABLED || HAL_MOUNT_SKYDROID_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Backend_TPFrame.h b/libraries/AP_Mount/AP_Mount_Backend_TPFrame.h new file mode 100644 index 0000000000000..2be0c84110eb4 --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_Backend_TPFrame.h @@ -0,0 +1,142 @@ +/* + Shared base for gimbal backends speaking the "#TP"/"#tp" wire framing used + by at least two independent products: Topotek's own gimbal line (see + AP_Mount_Topotek) and SkyDroid's OEM'd gimbal family (see AP_Mount_SkyDroid). + Neither product's protocol document ever names or expands what "TP" stands + for - it appears only as the literal 3-byte frame marker itself. This + class (and its name) is therefore built around that marker, not around + either company's name, since the marker is shared by multiple products + regardless of which one originated it. + + Packet format common to both products (each subclass documents its own + command-identifier set and AddressByte values, which do differ): + + ------------------------------------------------------------------------------------------- + Field Index Bytes Description + ------------------------------------------------------------------------------------------- + Frame Header 0 3 #TP (fixed length) or #tp (variable length) + Address Bit 3 2 source address first, destination address second + Data_Len 5 1 data length (hex nibble, max 0x0F) + Control Bit 6 1 r -> query w -> set/control + Identification Bit 7 3 3 character command identifier + Data 10 Data_Len + Check Bit 2 sum of all preceding bytes, output as 2 ASCII hex + characters (high nibble first) + */ + +#pragma once + +#include "AP_Mount_config.h" + +#if HAL_MOUNT_TOPOTEK_ENABLED || HAL_MOUNT_SKYDROID_ENABLED + +#include "AP_Mount_Backend_Serial.h" + +// preamble layout is fixed by the protocol and identical for every product - +// see the packet-format table above +#define AP_MOUNT_TPFRAME_PACKETLEN_MIN 12 // packet length not including the data segment +#define AP_MOUNT_TPFRAME_MSGOFS_DATALEN 5 // data length, 1 ASCII hex nibble +#define AP_MOUNT_TPFRAME_MSGOFS_ID 7 // 3-character command identifier +#define AP_MOUNT_TPFRAME_MSGOFS_DATA 10 // start of the command-specific data segment +// large enough for the bigger of the two products' own PACKETLEN_MAX (Topotek's 36); +// each subclass's packetlen_max() enforces its own, possibly smaller, real limit +#define AP_MOUNT_TPFRAME_PACKETLEN_MAX 36 + +class AP_Mount_Backend_TPFrame : public AP_Mount_Backend_Serial +{ + +public: + // inherit constructor + using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial; + + // Do not allow copies + CLASS_NO_COPY(AP_Mount_Backend_TPFrame); + +protected: + + // header type (fixed or variable length) + // first three bytes of packet determined by this value + enum class HeaderType : uint8_t { + FIXED_LEN = 0x00, // #TP will be sent + VARIABLE_LEN = 0x01, // #tp will be sent + }; + + // control byte (read or write) + // sent as 7th byte of packet + enum class ControlByte : uint8_t { + READ = 114, // 'r' + WRITE = 119, // 'w' + }; + + // parsing state. Preamble states are shared by both products; a product + // needing extra states beyond WAITING_FOR_DATA would extend this, though + // neither does today + enum class ParseState : uint8_t { + WAITING_FOR_HEADER1 = 0,// # + WAITING_FOR_HEADER2, // T or t + WAITING_FOR_HEADER3, // P or p + WAITING_FOR_ADDR1, // source address + WAITING_FOR_ADDR2, // destination address + WAITING_FOR_DATALEN, + WAITING_FOR_CONTROL, // r or w + WAITING_FOR_ID1, // e.g. 'G' + WAITING_FOR_ID2, // e.g. 'A' + WAITING_FOR_ID3, // e.g. 'C' + WAITING_FOR_DATA, // normally hex numbers in char form (e.g. '0A') + WAITING_FOR_CRC_LOW, + WAITING_FOR_CRC_HIGH, + }; + + // identifier bytes + typedef char Identifier[3]; + + // reading incoming packets from gimbal and confirm they are of the correct + // format. Calls handle_message() once a packet's CRC has been verified + void read_incoming_packets(); + + // called once a complete, CRC-verified packet has been received. + // _msg_buff/_msg_buff_len describe it; msg_id points at its 3-character + // command ID (AP_MOUNT_TPFRAME_MSGOFS_ID into _msg_buff) for convenience + virtual void handle_message(const char* msg_id) = 0; + + // maximum number of bytes in a packet sent to or received from the gimbal. + // Must not exceed AP_MOUNT_TPFRAME_PACKETLEN_MAX (the size of _msg_buff) + virtual uint8_t packetlen_max() const = 0; + + // data segment length can be no more than this + uint8_t datalen_max() const { return packetlen_max() - AP_MOUNT_TPFRAME_PACKETLEN_MIN; } + + // true if b is a valid destination/source address byte for this product - + // each product's own AddressByte enum defines its actual address set + virtual bool is_valid_address_byte(uint8_t b) const = 0; + + // return the address byte to send as the source of our own outgoing + // packets. Each product's protocol document defines its own rule for + // this (e.g. whether UART vs network-attached connections use different + // source addresses), so there is no shared default + virtual uint8_t source_address_byte() const = 0; + + // calculate checksum + uint8_t calculate_crc(const uint8_t *cmd, uint8_t len) const; + + // hexadecimal to character conversion + uint8_t hex2char(uint8_t data) const; + + // send a fixed length packet to gimbal + // returns true on success, false if serial port initialization failed + bool send_fixedlen_packet(uint8_t address, const Identifier id, bool write, uint8_t value); + + // send a variable length packet to gimbal + // returns true on success, false if serial port initialization failed + bool send_variablelen_packet(HeaderType header, uint8_t address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len); + + // members + uint8_t _msg_buff[AP_MOUNT_TPFRAME_PACKETLEN_MAX]; // buffer holding bytes from latest packet received. only used to calculate crc + uint8_t _msg_buff_len; // number of bytes in the msg buffer + struct { + ParseState state; // parser state + uint8_t data_len; // expected number of data bytes + } _parser; +}; + +#endif // HAL_MOUNT_TOPOTEK_ENABLED || HAL_MOUNT_SKYDROID_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Params.cpp b/libraries/AP_Mount/AP_Mount_Params.cpp index 317999855a791..5ed2be0985ed2 100644 --- a/libraries/AP_Mount/AP_Mount_Params.cpp +++ b/libraries/AP_Mount/AP_Mount_Params.cpp @@ -10,7 +10,7 @@ const AP_Param::GroupInfo AP_Mount_Params::var_info[] = { // @DisplayName: Mount Type // @Description: Mount Type // @SortValues: AlphabeticalZeroAtTop - // @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial, 6:MAVLink (Gremsy/AVT), 7:BrushlessPWM, 8:Siyi, 9:Scripting, 10:Xacti, 11:Viewpro, 12:Topotek, 13:CADDX, 14:XFRobot + // @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial, 6:MAVLink (Gremsy/AVT), 7:BrushlessPWM, 8:Siyi, 9:Scripting, 10:Xacti, 11:Viewpro, 12:Topotek, 13:CADDX, 14:XFRobot, 15:SkyDroid // @RebootRequired: True // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Mount_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 4def266dc727e..9c66abb27c84b 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -665,7 +665,7 @@ void AP_Mount_Siyi::send_target_angles(const MountAngleTarget &angle_rad) // use simple P controller to convert pitch angle error (in radians) to a target rate scalar (-100 to +100) const float pitch_err_rad = (pitch_rad - current_angle_transformed.y); - const float pitch_rate_scalar = constrain_float(100.0 * pitch_err_rad * AP_MOUNT_SIYI_PITCH_P / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100); + const float pitch_rate_scalar = angle_error_to_rate(pitch_err_rad, 100.0 * AP_MOUNT_SIYI_PITCH_P / AP_MOUNT_SIYI_RATE_MAX_RADS, 100.0f); // convert yaw angle to body-frame float yaw_bf_rad = yaw_is_ef ? wrap_PI(yaw_rad - AP::ahrs().get_yaw_rad()) : yaw_rad; @@ -680,7 +680,7 @@ void AP_Mount_Siyi::send_target_angles(const MountAngleTarget &angle_rad) // use simple P controller to convert yaw angle error to a target rate scalar (-100 to +100) const float yaw_err_rad = (yaw_bf_rad - current_angle_transformed.z); - const float yaw_rate_scalar = constrain_float(100.0 * yaw_err_rad * AP_MOUNT_SIYI_YAW_P / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100); + const float yaw_rate_scalar = angle_error_to_rate(yaw_err_rad, 100.0 * AP_MOUNT_SIYI_YAW_P / AP_MOUNT_SIYI_RATE_MAX_RADS, 100.0f); // rotate gimbal. pitch_rate and yaw_rate are scalars in the range -100 ~ +100 rotate_gimbal(pitch_rate_scalar, yaw_rate_scalar, yaw_is_ef); diff --git a/libraries/AP_Mount/AP_Mount_SkyDroid.cpp b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp new file mode 100644 index 0000000000000..4866c354b6abf --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_SkyDroid.cpp @@ -0,0 +1,631 @@ +#include "AP_Mount_config.h" + +#if HAL_MOUNT_SKYDROID_ENABLED + +#include "AP_Mount_SkyDroid.h" + +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +#define AP_MOUNT_SKYDROID_UPDATE_INTERVAL_MS 100 // push our attitude to the gimbal, and step the 1hz request loop, at this interval - target angles/rates are sent every update() call instead, at the full 50hz (see update()'s comment) +#define AP_MOUNT_SKYDROID_HEALTH_TIMEOUT_MS 1000 // timeout for health (based on attitude reports from gimbal) +#define AP_MOUNT_SKYDROID_ATTITUDE_RATE_HZ 50 // rate we ask the gimbal to stream its attitude to us (matches the 50hz rate AP_Mount::update() is actually called at; doc allows up to 100hz) + +// 3 character identifiers +#define AP_MOUNT_SKYDROID_ID3CHAR_GIMBAL_MODE "PTZ" // discrete gimbal control, data bytes: 00:stop, 01:up, 02:down, 03:left, 04:right, 05:center, 06:follow, 07:lock head. Only the follow/lock codes (06/07 - see set_gimbal_lock()) and center (05 - see send_center_command()) are used +#define AP_MOUNT_SKYDROID_ID3CHAR_SPEED_YAW "GSY" // individual-axis yaw rate control, data bytes: signed 8bit hex. The only command confirmed to move yaw on real hardware (GAM/GSM/GAY all silently ignored); its sign is also inverted vs the doc - see send_target_rates() +#define AP_MOUNT_SKYDROID_ID3CHAR_SPEED_PITCH "GSP" // individual-axis pitch rate control, data bytes: signed 8bit hex. Confirmed functional on real hardware, sign matches the doc +// matches the protocol doc's and SkyDroid's own RCSDK's documented 0.5deg/s per LSB +// (max speed +/-63.5 deg/s == 127 * 0.5). An earlier real-hardware measurement of +// this driver had it at 1/16th of this (0.03125) - that measurement was wrong, not +// the documentation: a real-C11 dataflash log of a sustained full-deflection GSY +// rate-mode command (MNT1_RC_RATE=90, so comfortably saturating to the max LSB +// value of 127) measured yaw moving at a clean, consistent ~64deg/s across three +// independent full-speed sweeps (e.g. a 179deg sweep in exactly 2.80s = 63.97deg/s) +// - matching 0.5deg/s/LSB almost exactly, not 0.03125 +#define AP_MOUNT_SKYDROID_AXIS_DPS_PER_LSB 0.5f +#define AP_MOUNT_SKYDROID_AXIS_MAX_DPS (127 * AP_MOUNT_SKYDROID_AXIS_DPS_PER_LSB) +// every other 3-character command ID is used in exactly one place, so is a plain +// string literal at its own call site instead of a macro defined here + +#define AP_MOUNT_SKYDROID_DEBUG 0 +#define debug(fmt, args ...) do { if (AP_MOUNT_SKYDROID_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SkyDroid: " fmt, ## args); } } while (0) + +const char* AP_Mount_SkyDroid::send_message_prefix = "Mount: SkyDroid"; + +// update mount position - should be called periodically +void AP_Mount_SkyDroid::update() +{ + AP_Mount_Backend::update(); + + // exit immediately if not initialised + if (!_initialised) { + return; + } + + // reading incoming packets from gimbal + read_incoming_packets(); + + // update based on mount mode, and send target angles or rates depending on the + // target type. Deliberately NOT gated by the 10hz throttle below - AP_Mount::update() + // is actually called at 50hz (see ArduPlane/ArduCopter's SCHED_TASK entry), and the + // closed-loop angle control in send_target_angles() benefits from running its + // P-controller at the full rate rather than being throttled down further + update_mnt_target(); + send_target_to_gimbal(); + + // everything below updates at 10hz + uint32_t now_ms = AP_HAL::millis(); + if ((now_ms - _last_req_current_info_ms) < AP_MOUNT_SKYDROID_UPDATE_INTERVAL_MS) { + return; + } + _last_req_current_info_ms = now_ms; + + // push our own attitude to the gimbal + send_attitude_to_gimbal(); + + // calls below here called at 1hz + _last_req_step++; + if (_last_req_step >= (uint8_t)ReqStep::NUM_STEPS) { + _last_req_step = 0; + } + switch ((ReqStep)_last_req_step) { + case ReqStep::VERSION: + // get gimbal firmware version. Worth retrying until answered rather than + // asking once: the version determines which command sets the gimbal actually + // implements (SkyDroid's RCSDK gates its combined yaw+pitch call on firmware + // >= 0.5, and we've found the combined/absolute-angle commands dead on the + // firmware we have), so it's the single most useful thing to know when + // diagnosing a gimbal that connects but won't move + if (!_got_gimbal_version) { + request_gimbal_version(); + } + break; + case ReqStep::TIME_SYNC: + // (re)send current UTC time so photos/videos are timestamped correctly - see + // send_time_sync() for why this is needed and why it's resent periodically + send_time_sync(); + break; + case ReqStep::ATTITUDE_ENABLE: + // (re)request gimbal attitude streaming. harmless to resend if already enabled, + // and guards against the enable packet being lost over UDP + request_gimbal_attitude(); + break; + case ReqStep::MODEL: + // get the model name. Purely informational (reported to the GCS via + // CAMERA_INFORMATION) - no control decision depends on it, since SkyDroid have + // confirmed the gimbal-control commands are identical across models. That's + // why a slow answer here is harmless and 1hz is plenty: "MOD" has been seen to + // take minutes to reply on real hardware, and control works throughout + if (!_got_model_name) { + request_gimbal_model(); + } + break; + case ReqStep::SDCARD: + // request memory card information + request_gimbal_sdcard_info(); + break; + case ReqStep::ATTITUDE_ACCEPT: + // (re)enable gimbal to accept our attitude pushes + send_attitude_enable(); + break; + default: + // spare steps (5, 7, 8, 9) - nothing to do + break; + } +} + +// return true if healthy +bool AP_Mount_SkyDroid::healthy() const +{ + // exit immediately if not initialised + if (!_initialised) { + return false; + } + + // unhealthy until we've heard at least one "GAC" attitude report, and if + // attitude information has not been received recently since then + const uint32_t last_current_angle_ms = _last_current_angle_ms; + if (last_current_angle_ms == 0) { + return false; + } + return (AP_HAL::millis() - last_current_angle_ms < AP_MOUNT_SKYDROID_HEALTH_TIMEOUT_MS); +} + +// take a picture. returns true on success +bool AP_Mount_SkyDroid::take_picture() +{ + // exit immediately if not initialised + if (!_initialised) { + return false; + } + + // exit immediately if the memory card is confirmed absent - UNKNOWN (no SDC + // reply yet) is allowed through, since that reply can take minutes (see + // SDCardState's comment) and we'd rather attempt the capture than silently + // refuse every request until it arrives + if (_sdcard_state == SDCardState::ABSENT) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s SD card error", send_message_prefix); + return false; + } + + // "CAP": take picture, data bytes: 01. sample command: #TPUD2wCAP01 + return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, "CAP", true, 1); +} + +// start or stop video recording. returns true on success +// set start_recording = true to start record, false to stop recording +bool AP_Mount_SkyDroid::record_video(bool start_recording) +{ + // exit immediately if not initialised + if (!_initialised) { + return false; + } + + // exit immediately if the memory card is confirmed absent (see take_picture()) + if (_sdcard_state == SDCardState::ABSENT) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s SD card error", send_message_prefix); + return false; + } + + // "REC": record video, data bytes: 00:stop, 01:start. sample command: #TPUD2wREC01 + if (send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, "REC", true, start_recording ? 1 : 0)) { + // SkyDroid does not push unsolicited recording-state changes to us so track our own request locally + _recording = start_recording; + return true; + } + return false; +} + +// set zoom specified as a rate. SkyDroid's digital zoom is stepped, not continuous: +// there is no "stop" data value, only single-shot zoom-in/zoom-out pulses +bool AP_Mount_SkyDroid::set_zoom(ZoomType zoom_type, float zoom_value) +{ + // exit immediately if not initialised + if (!_initialised) { + return false; + } + + // only rate based zoom is supported + if (zoom_type != ZoomType::RATE) { + return false; + } + + // zero rate has no corresponding command so treat as a successful no-op + if (is_zero(zoom_value)) { + return true; + } + + // "DZM": digital zoom, data bytes: 0A:zoom+ (single step), 0B:zoom- (single step). + // sample command: #TPUM2wDZM0A65 + const uint8_t zoom_cmd = (zoom_value < 0) ? 0x0B : 0x0A; // 0x0B: zoom-, 0x0A: zoom+ + return send_fixedlen_packet(AddressByte::LENS, "DZM", true, zoom_cmd); +} + +// send camera settings message to GCS +void AP_Mount_SkyDroid::send_camera_settings(mavlink_channel_t chan) const +{ + // exit immediately if not initialised + if (!_initialised) { + return; + } + + // send CAMERA_SETTINGS message + mavlink_msg_camera_settings_send( + chan, + AP_HAL::millis(), // time_boot_ms + _recording ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey) + NaNf, // zoomLevel float, percentage from 0 to 100, NaN if unknown + NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown +} + +// get attitude as a quaternion. returns true on success +bool AP_Mount_SkyDroid::get_attitude_quaternion(Quaternion& att_quat) +{ + // fail while we've never actually received a "GAC" attitude report - otherwise + // callers (e.g. GIMBAL_DEVICE_ATTITUDE_STATUS reporting) would be given a + // fabricated (0,0,0) attitude as if it were real data. _last_current_angle_ms + // is zero-initialised and only ever set by gimbal_angle_analyse() on receipt of + // a real "GAC" packet, so this is the same "have we ever heard from the gimbal" + // signal healthy() uses for its own timeout check + if (_last_current_angle_ms == 0) { + return false; + } + // x=roll (always zero on models with no roll axis, e.g. C11), y=pitch, z=yaw + att_quat.from_euler(_current_angle_rad.x, _current_angle_rad.y, _current_angle_rad.z); + return true; +} + +// dispatch on the 3-character command ID to the function that consumes that +// message - called by AP_Mount_Backend_TPFrame::read_incoming_packets() once a +// packet's CRC has been verified +void AP_Mount_SkyDroid::handle_message(const char* msg_id) +{ + if (strncmp(msg_id, "GAC", 3) == 0) { + gimbal_angle_analyse(); + } else if (strncmp(msg_id, "REC", 3) == 0) { + gimbal_record_analyse(); + } else if (strncmp(msg_id, "SDC", 3) == 0) { + gimbal_sdcard_analyse(); + } else if (strncmp(msg_id, "VER", 3) == 0) { + gimbal_version_analyse(); + } else if (strncmp(msg_id, "MOD", 3) == 0) { + gimbal_model_analyse(); + } +} + +// request gimbal to (re)start sending us attitude at AP_MOUNT_SKYDROID_ATTITUDE_RATE_HZ +void AP_Mount_SkyDroid::request_gimbal_attitude() +{ + // "GAA": enable/disable gimbal->us attitude streaming, data bytes: 00:off, + // 01-64:rate in Hz. sample command: #TPUG2wGAA0A + send_fixedlen_packet(AddressByte::GIMBAL, "GAA", true, AP_MOUNT_SKYDROID_ATTITUDE_RATE_HZ); +} + +// request gimbal memory card information +void AP_Mount_SkyDroid::request_gimbal_sdcard_info() +{ + // "SDC": get SD card state, data bytes: 00 to query. sample command: #TPUD2rSDC00 + send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, "SDC", false, 0); +} + +// request gimbal version +void AP_Mount_SkyDroid::request_gimbal_version() +{ + // "VER": get firmware version, data bytes always 00. sample command: #TPUD2rVER00 + send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, "VER", false, 0); +} + +// request gimbal model name (e.g. "C11", "C13") +void AP_Mount_SkyDroid::request_gimbal_model() +{ + // "MOD": get model name (e.g. "C11"), data bytes always 00. sample command: #TPUD2rMOD00 + send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, "MOD", false, 0); +} + +// send current UTC date/time to the gimbal so photos/videos are timestamped correctly. +// Confirmed on real hardware that the camera has no RTC of its own and defaults to +// 1970-01-01 without this. Uses UTC since ArduPilot has no local timezone concept - +// this means driver-triggered captures' timestamps will differ from ones taken via +// SkyDroid's own app (which uses the connected device's local time) by the local UTC +// offset. Returns false (without sending) if the vehicle doesn't yet have a valid +// time source (e.g. GPS not locked) +bool AP_Mount_SkyDroid::send_time_sync() +{ + uint16_t year, ms; + uint8_t month, day, hour, min, sec; + if (!AP::rtc().get_date_and_time_utc(year, month, day, hour, min, sec, ms)) { + return false; + } + + // "TIM": set current time, data bytes: hhmmss.ccDDMMYY (15 ASCII chars, + // cc=hundredths of a second). Confirmed on real hardware that the camera has no + // RTC of its own and defaults to 1970-01-01 without this. + // sample command: #tpUDFwTIM142832.00031218 (2018-12-03 14:28:32.00) - data is 15 + // ASCII chars: hhmmss.ccDDMMYY, cc=hundredths of a second, YY=2-digit year + uint8_t databuff[16]; + hal.util->snprintf((char*)databuff, ARRAY_SIZE(databuff), "%02u%02u%02u.%02u%02u%02u%02u", + hour, min, sec, (unsigned)((ms / 10) % 100), + day, month + 1, (unsigned)(year % 100)); + return send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::SYSTEM_AND_IMAGE, "TIM", true, databuff, ARRAY_SIZE(databuff)-1); +} + +// (re)enable the gimbal to accept our attitude pushes +bool AP_Mount_SkyDroid::send_attitude_enable() +{ + // "FAE": enable/disable us->gimbal attitude streaming, data bytes: 00:off, 01:on. + // sample command: #TPUG2wFAE01 + return send_fixedlen_packet(AddressByte::GIMBAL, "FAE", true, 1); +} + +// send our current attitude to the gimbal +bool AP_Mount_SkyDroid::send_attitude_to_gimbal() +{ + const int16_t yaw_cd = wrap_180_cd((int32_t)(AP::ahrs().get_yaw_deg() * 100)); + const int16_t pitch_cd = (int16_t)(AP::ahrs().get_pitch_deg() * 100); + const int16_t roll_cd = (int16_t)(AP::ahrs().get_roll_deg() * 100); + + // 1: fixed-wing, 0: copter/hover - this is SkyDroid's own documented field for + // this command, and APM_BUILD_TYPE(APM_BUILD_ArduPlane) is the obvious compile-time + // proxy for it, but we don't actually know what it changes in the gimbal's own + // firmware, or whether that proxy is the right one (e.g. a Plane holding/loitering + // isn't continuously moving forward either). Raised with SkyDroid to find out what + // this bit actually affects before assuming a "more correct" runtime check would + // really be better rather than just differently wrong + const bool fixed_wing = APM_BUILD_TYPE(APM_BUILD_ArduPlane); + + // "FAI": our attitude sent to gimbal, data bytes: yaw+pitch+roll (4hex each, + // 0.01deg) + mode (1:fixed-wing, 0:hover). sample command: #tpUG0EwFAI + uint8_t databuff[15]; + hal.util->snprintf((char*)databuff, ARRAY_SIZE(databuff), "%04X%04X%04X%02X", + (uint16_t)yaw_cd, (uint16_t)pitch_cd, (uint16_t)roll_cd, fixed_wing ? 1 : 0); + return send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::GIMBAL, "FAI", true, databuff, ARRAY_SIZE(databuff)-1); +} + +// send angle target in radians to gimbal, by closing the loop ourselves using the +// gimbal's own "GAC" attitude feedback and driving GSY/GSP as the rate actuator. +// There is no absolute-angle command that works on this hardware - GAM/GAY/GAP are +// all silently ignored (see this file's header comment) +void AP_Mount_SkyDroid::send_target_angles(const MountAngleTarget& angle_rad) +{ + // set gimbal's lock state (follow the body-frame target) + set_gimbal_lock(false); + + // clamp to the configured MNT1_YAW/PITCH_MIN/MAX range (also in degrees) - + // AP_Mount's frontend does not clamp the target itself before calling us. + // Roll is deliberately absent: the gimbal self-stabilizes roll and offers no + // way to command it (see has_roll_control()). Everything below stays in + // degrees from here on - the only unit conversions in this function are the + // unavoidable ones at its boundaries: angle_rad (radians, AP_Mount's own + // target-type convention) coming in, and _current_angle_rad (radians, kept + // that way for get_attitude_quaternion()'s benefit) read via degrees() below + const float yaw_target_deg = constrain_float(degrees(angle_rad.get_bf_yaw()), + _params.yaw_angle_min, _params.yaw_angle_max); + const float pitch_target_deg = constrain_float(degrees(angle_rad.pitch), + _params.pitch_angle_min, _params.pitch_angle_max); + + // if GAC attitude reports have stopped arriving, _current_angle_rad is stale - + // the error computed against it below would be wrong, and (since it's held + // fixed once the feed drops) could keep commanding a nonzero rate indefinitely + // rather than converging. Command an explicit stop instead of just skipping + // the send, since a resumed-later GAC feed is the only thing that would + // otherwise correct a stale outstanding rate command + if (!healthy()) { + send_axis_rate(AP_MOUNT_SKYDROID_ID3CHAR_SPEED_YAW, 0); + send_axis_rate(AP_MOUNT_SKYDROID_ID3CHAR_SPEED_PITCH, 0); + return; + } + + // simple P-controller driving GSY/GSP as the rate actuator, using the GAC + // attitude feedback already parsed by gimbal_angle_analyse(). + // + // Confirmed on real C11 hardware that kP=2.0 sustains a continuous limit-cycle + // oscillation on both axes: at that gain, full-scale rate is reached by just + // ~2deg of error, so the actuator was being driven at max speed for almost any + // real excursion - combined with the real feedback's lag (GAC round-trip plus + // the gimbal's own mechanical response), a fast-reacting/early-saturating + // P-controller overshoots and re-corrects indefinitely instead of settling. + // + // kP=1.0 was chosen (before AP_MOUNT_SKYDROID_AXIS_DPS_PER_LSB's real value was + // known - see that constant's comment) to saturate at ~4deg of error + // (AP_MOUNT_SKYDROID_AXIS_MAX_DPS/kP), staying clear of the ~2deg real-hardware + // oscillation zone confirmed below while converging fast enough to meet + // mount_test_body's tightest check (test_mount_rc_targetting()'s hardcoded + // 0.1deg tolerance). Now that AXIS_MAX_DPS is 16x larger (0.5, not 0.03125 + // deg/s per LSB), this saturates at ~63.5deg instead - even further from the + // 2deg danger zone, so no less safe, but the "~4deg" figure below is no longer + // accurate to the number, just the reasoning. NOT YET RE-VALIDATED ON REAL + // HARDWARE - the original oscillation was only ever found via real-hardware + // testing, not SITL, so confirm this still settles cleanly (no hunting/dither) + // on the real C11 before relying on this + constexpr float kP = 1.0; // (deg/s of rate command) per (deg of angle error) + // + // The deadzone below (stop correcting entirely once close, rather than tapering + // to an ever-smaller command) guarantees a clean stop rather than dither once + // within range - its WIDTH is a separate knob from kP, and was originally set to + // 2.0deg for margin without much thought, wider than mount_test_body's autotest + // tolerances and hence a real bug, not a hardware ceiling. 0.05deg keeps margin + // under the 0.1deg check while staying a "stop dead" cutoff (not tapering). + // + // IMPORTANT: this deadzone is no longer the binding constraint it was designed + // to be. GSY/GSP's wire value is a quantized 8-bit signed LSB - see + // AP_MOUNT_SKYDROID_AXIS_DPS_PER_LSB, now known to be 0.5deg/s per LSB (not the + // 0.03125 this was tuned against) - so kP*error now rounds to 0 LSB at + // send_axis_rate() for any error below ~0.25deg (half an LSB step at this kP), + // which is COARSER than the 0.05deg deadzone below and than the 0.1deg test + // tolerance this was tuned to meet. In other words: the actuator's real, + // confirmed resolution floor may no longer be fine enough to pass + // test_mount_rc_targetting() at all via this fixed-gain approach, regardless of + // deadzone or kP tuning - re-run the autotest and see before assuming this still + // passes + constexpr float deadzone_deg = 0.05; // stop correcting once within this many degrees + const float yaw_error_deg = wrap_180(yaw_target_deg - degrees(_current_angle_rad.z)); + const float pitch_error_deg = pitch_target_deg - degrees(_current_angle_rad.y); + const float yaw_rate_dps = angle_error_to_rate(yaw_error_deg, kP, AP_MOUNT_SKYDROID_AXIS_MAX_DPS, deadzone_deg); + const float pitch_rate_dps = angle_error_to_rate(pitch_error_deg, kP, AP_MOUNT_SKYDROID_AXIS_MAX_DPS, deadzone_deg); + + // GSY's sign is inverted vs AP_Mount's convention (see send_axis_rate below) + send_axis_rate(AP_MOUNT_SKYDROID_ID3CHAR_SPEED_YAW, -yaw_rate_dps); + send_axis_rate(AP_MOUNT_SKYDROID_ID3CHAR_SPEED_PITCH, pitch_rate_dps); +} + +// send rate target in rad/s to gimbal, directly via GSY/GSP - the only commands +// confirmed to move this hardware (GSM is silently ignored) +void AP_Mount_SkyDroid::send_target_rates(const MountRateTarget& rate_rads) +{ + // set gimbal's lock state if it has changed + set_gimbal_lock(rate_rads.yaw_is_ef); + + // GSY's sign is inverted vs AP_Mount's convention (confirmed on real hardware: a + // positive GSY value moves yaw LEFT, not right) - negate here so callers of this + // function keep using AP_Mount's normal yaw-right-positive convention. GSP's + // sign matches AP_Mount's convention (pitch-up-positive) so is passed straight + // through. rate_rads.roll is deliberately ignored - the gimbal self-stabilizes + // roll and offers no way to command it (see has_roll_control()) + send_axis_rate(AP_MOUNT_SKYDROID_ID3CHAR_SPEED_YAW, -degrees(rate_rads.yaw)); + send_axis_rate(AP_MOUNT_SKYDROID_ID3CHAR_SPEED_PITCH, degrees(rate_rads.pitch)); +} + +// send a single-axis rate command (GSY or GSP) for rate_dps, converted to the wire's +// signed 8bit LSB units using the real-world calibrated scale. Caller is responsible +// for any axis-specific sign compensation (see send_target_rates() above) +void AP_Mount_SkyDroid::send_axis_rate(const Identifier id, float rate_dps) +{ + const int8_t rate_lsb = constrain_int16(roundf(rate_dps / AP_MOUNT_SKYDROID_AXIS_DPS_PER_LSB), -127, 127); + send_fixedlen_packet(AddressByte::GIMBAL, id, true, (uint8_t)rate_lsb); +} + +// attitude information analysis of gimbal (arrives as "GAC" in response to our "GAA" enable request) +void AP_Mount_SkyDroid::gimbal_angle_analyse() +{ + // consume current angles. data is yaw, pitch, roll in that order, each 4 hex chars, 0.01deg units + if (_parser.data_len < 12) { + return; + } + uint32_t yaw_raw, pitch_raw, roll_raw; + if (!hex_chars_to_uint32((const char*)&_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA], 4, yaw_raw) || + !hex_chars_to_uint32((const char*)&_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA + 4], 4, pitch_raw) || + !hex_chars_to_uint32((const char*)&_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA + 8], 4, roll_raw)) { + return; + } + const int16_t yaw_angle_cd = wrap_180_cd((int16_t)yaw_raw); + const int16_t pitch_angle_cd = (int16_t)pitch_raw; + // roll comes from the gimbal's own self-stabilization - we report it for telemetry + // but cannot command it (see has_roll_control()) + const int16_t roll_angle_cd = (int16_t)roll_raw; + + // convert cd to radians + _current_angle_rad.x = cd_to_rad(roll_angle_cd); + _current_angle_rad.y = cd_to_rad(pitch_angle_cd); + _current_angle_rad.z = cd_to_rad(yaw_angle_cd); + _last_current_angle_ms = AP_HAL::millis(); + + // announce gimbal connection to the user on the first attitude report received. + // this does not depend on the "VER" command (whose model support is undocumented + // for some SkyDroid models) so it is a more reliable connection signal + if (!_announced_connected) { + _announced_connected = true; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s connected", send_message_prefix); + } +} + +// gimbal video information analysis +void AP_Mount_SkyDroid::gimbal_record_analyse() +{ + // data is 2 ASCII chars ("00" or "01") - only the low digit is ever non-zero, so + // that's the only one we need to check + if (_parser.data_len < 2) { + return; + } + _recording = (_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA + 1] == '1'); +} + +// information analysis of gimbal storage card +void AP_Mount_SkyDroid::gimbal_sdcard_analyse() +{ + // data is 10 hex chars: 5 for remaining capacity, 5 for total capacity (units MB) + // all zeros means no card inserted. Gate on _parser.data_len (the actual number + // of data bytes), not _msg_buff_len (which also counts the 2 trailing CRC chars) - + // otherwise a data_len of 8 or 9 would read 1-2 CRC characters as if they were + // card-capacity data, and a nonzero CRC char would falsely read as "card present" + if (_parser.data_len < 10) { + return; + } + static const uint8_t all_zero_chars[10] = {'0','0','0','0','0','0','0','0','0','0'}; + const bool all_zero = (memcmp(&_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA], all_zero_chars, ARRAY_SIZE(all_zero_chars)) == 0); + _sdcard_state = all_zero ? SDCardState::ABSENT : SDCardState::PRESENT; +} + +// gimbal basic information analysis. response data is of the form "VX.X.X" (e.g. "V1.0.78") +void AP_Mount_SkyDroid::gimbal_version_analyse() +{ + uint8_t data_buf_len; + if (!hex_char_to_nibble(_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATALEN], data_buf_len) || data_buf_len == 0 || + _msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA] != 'V') { + return; + } + + // version array with index 0=major, 1=minor, 2=patch + uint8_t version[3] {}; + uint8_t ver_count = 0; + uint32_t ver_num = 0; + for (uint8_t i = 1; i < data_buf_len && ver_count < ARRAY_SIZE(version); i++) { + const uint8_t c = _msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATA + i]; + if (c == '.') { + version[ver_count++] = ver_num; + ver_num = 0; + continue; + } + uint8_t digit; + if (!hex_char_to_nibble(c, digit)) { + return; + } + ver_num = ver_num * 10 + digit; + } + if (ver_count < ARRAY_SIZE(version)) { + version[ver_count] = ver_num; + } + _firmware_ver = (version[2] << 16) | (version[1] << 8) | (version[0]); + + // display gimbal firmware version to user. Worth reporting prominently: which + // command sets this gimbal actually implements appears to depend on it (SkyDroid's + // RCSDK gates its combined yaw+pitch call on firmware >= 0.5, and we've found the + // combined/absolute-angle commands dead on the firmware we have), so this is the + // first thing to check when a gimbal connects but won't move + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s firmware v%u.%u.%u", + send_message_prefix, + version[0], // major version + version[1], // minor version + version[2]); // patch version + + _got_gimbal_version = true; +} + +// gimbal model name analysis. response data is raw ASCII text, e.g. "C13" +void AP_Mount_SkyDroid::gimbal_model_analyse() +{ + uint8_t data_buf_len; + if (!hex_char_to_nibble(_msg_buff[AP_MOUNT_TPFRAME_MSGOFS_DATALEN], data_buf_len) || data_buf_len == 0) { + return; + } + memset(_model_name, 0, sizeof(_model_name)); + memcpy(_model_name, _msg_buff + AP_MOUNT_TPFRAME_MSGOFS_DATA, MIN((uint8_t)(sizeof(_model_name)-1), data_buf_len)); + + // display gimbal model name to user. Informational only - no control decision + // depends on it (see this driver's header comment) + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s model %s", send_message_prefix, _model_name); + + _got_model_name = true; +} + +// set gimbal's lock vs follow mode +// lock should be true if gimbal should maintain an earth-frame target +// lock is false to follow / maintain a body-frame target +bool AP_Mount_SkyDroid::set_gimbal_lock(bool lock) +{ + // _last_lock defaults false, which is indistinguishable from "we've already + // confirmed the gimbal is in follow mode" unless we also track whether a mode + // has actually been sent yet - without _lock_sent, the very first call (always + // requesting follow, false) would silently no-op instead of sending anything + if (_lock_sent && _last_lock == lock) { + return true; + } + + // send message and update lock state. PTZ data: 0x06 = follow, 0x07 = lock head + if (send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_SKYDROID_ID3CHAR_GIMBAL_MODE, true, lock ? 0x07 : 0x06)) { + _last_lock = lock; + _lock_sent = true; + return true; + } + return false; +} + +// send the gimbal's own one-shot "center" command. Deliberately not deduped like +// set_gimbal_lock() above - unlike lock/follow (a persistent mode we don't want to +// keep re-sending), center is a one-shot action that should fire every time the +// mode is (re)selected, same as every other RETRACT/NEUTRAL backend's behaviour +bool AP_Mount_SkyDroid::send_center_command() +{ + return send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_SKYDROID_ID3CHAR_GIMBAL_MODE, true, 0x05); +} + +// move to a "retracted" position - see this file's header declaration for why this +// uses the gimbal's own "center" command rather than falling through to the +// angle-based conversion (which needs GAC attitude feedback to converge) +void AP_Mount_SkyDroid::send_target_retracted() +{ + send_center_command(); +} + +// move to a neutral (forward-pointing) position - see this file's header declaration +void AP_Mount_SkyDroid::send_target_neutral() +{ + send_center_command(); +} + +#endif // HAL_MOUNT_SKYDROID_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_SkyDroid.h b/libraries/AP_Mount/AP_Mount_SkyDroid.h new file mode 100644 index 0000000000000..9613a2b1d2a80 --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_SkyDroid.h @@ -0,0 +1,316 @@ +/* + SkyDroid gimbal driver using custom serial protocol (usually run over UDP) + + This is the same "#TP"/"#tp" wire framing used by SkyDroid's OEM supplier + for the Topotek driver (see AP_Mount_Topotek) - both derive from + AP_Mount_Backend_TPFrame, which implements the shared framing/CRC/packet- + send layer. Neither protocol document ever names or expands what "TP" + stands for. The address bytes, command identifiers and units used by + SkyDroid's own firmware differ from Topotek's, so everything past that + shared layer is a separate, independent implementation. + + ------------------------------------------------------------------------------------------- + Field Index Bytes Description + ------------------------------------------------------------------------------------------- + Frame Header 0 3 #TP (fixed length) or #tp (variable length) + Address Bit 3 2 source address first, destination address second + Data_Len 5 1 data length (hex nibble, max 0x0F) + Control Bit 6 1 r -> query w -> set/control + Identification Bit 7 3 3 character command identifier + Data 10 Data_Len + Check Bit 2 sum of all preceding bytes, output as 2 ASCII hex + characters (high nibble first) + + This one driver covers every model in SkyDroid's "TOP protocol" gimbal camera + family: + - control is over UDP only (no direct UART), source address is always 'U' + - confirmed directly with SkyDroid: the gimbal-control commands are IDENTICAL + across models (C11, C13, ...). There is no model-specific control path, and + this driver deliberately has no model-dependent behaviour. The C13's extra + features over the C11 are infrared thermal imaging and laser ranging, neither + of which this driver currently uses. Do not reintroduce per-model dispatch + without new information from SkyDroid - an earlier version of this driver had + it, and it was wrong + - confirmed directly with SkyDroid: ROLL IS SELF-STABILIZED BY THE GIMBAL AND HAS + NO CONTROL COMMAND AT ALL, on any model. The protocol document does describe + roll commands ("GAR" angle, "GSR" rate) but the firmware does not implement + them, so this driver does not send them and reports has_roll_control() == false. + Roll is still parsed from the gimbal's own attitude reports and passed through + for telemetry, and our vehicle roll is still pushed to the gimbal ("FAI") to + feed its stabilizer + - pitch/yaw ranges are configured via MNT1_PITCH/YAW_MIN/MAX (e.g. the C11 has + pitch -90 to +10 deg, yaw -90 to +90 deg). This driver does not hardcode any + model's limits itself, so it stays correct across the family + - SkyDroid's documented sign convention is yaw-right-positive, pitch-up-positive, which + matches AP_Mount's own convention (no sign flip needed, unlike Topotek's protocol) + - the connected model (e.g. "C11", "C13") is queried at runtime via the "MOD" + command and reported through CAMERA_INFORMATION. This is INFORMATIONAL ONLY - + no control decision depends on it, which matters because "MOD" has been observed + on real hardware to take anywhere from under a second to 8+ minutes to answer + (SkyDroid's own SDK documents that camera-side commands are only effective once + the camera is producing video frames - "需要在出图后设置才有效" - which would + explain it). Gimbal-addressed commands (GSY/GSP/PTZ) have usually been observed + to work well before "MOD" answers - but at least once, on real hardware, NOTHING + worked (no RC, no GAC, no GCS messages of any kind) for over 5 minutes, so this + is not a guarantee - root cause of that particular case is still unknown (raised + with SkyDroid, response pending). Whatever gates it, this driver deliberately + does not wait on it: gimbal-addressed commands are always sent unconditionally, + for whenever the link does come up + - confirmed on real C11 hardware: the combined and absolute-angle commands (GAM, + GSM, GAY, GAP) are all silently ignored - only the individual-axis speed + commands (GSY, GSP) actually move the gimbal, so both rate and (closed-loop) + angle control are driven through those. This is believed to be a FIRMWARE + limitation rather than a model one: SkyDroid's SDK documents its combined + yaw+pitch call as requiring gimbal firmware >= 0.5, so newer firmware may well + accept the commands we found dead here. The gimbal's reported firmware version + is logged at startup (see gimbal_version_analyse()) to make that checkable + */ + +#pragma once + +#include "AP_Mount_config.h" + +#if HAL_MOUNT_SKYDROID_ENABLED + +#include "AP_Mount_Backend_TPFrame.h" +#include +#include +#include + +#define AP_MOUNT_SKYDROID_PACKETLEN_MAX 28 // maximum number of bytes in a packet sent to or received from the gimbal + +class AP_Mount_SkyDroid : public AP_Mount_Backend_TPFrame +{ + +public: + // Constructor + using AP_Mount_Backend_TPFrame::AP_Mount_Backend_TPFrame; + + // Do not allow copies + CLASS_NO_COPY(AP_Mount_SkyDroid); + + // update mount position - should be called periodically + void update() override; + + // return true if healthy + bool healthy() const override; + + // has_pan_control - returns true if this mount can control its pan (required for multicopters) + bool has_pan_control() const override { return yaw_range_valid(); }; + + // has_roll_control - always false: confirmed directly with SkyDroid that roll is + // self-stabilized by the gimbal and has no control command on any model in this + // family, regardless of what MNT1_ROLL_MIN/MAX is set to + bool has_roll_control() const override { return false; }; + + // + // camera controls + // + + // take a picture. returns true on success + bool take_picture() override; + + // start or stop video recording + // set start_recording = true to start record, false to stop recording + bool record_video(bool start_recording) override; + + // set zoom specified as a rate. SkyDroid's zoom is stepped (not continuous) so + // each non-zero call sends a single zoom-in/zoom-out pulse + bool set_zoom(ZoomType zoom_type, float zoom_value) override; + + bool has_camera_information() const override { return true; } + // return camera vendor name + void get_camera_vendor_name(char *buf, uint8_t buflen) const override { strncpy(buf, "SkyDroid", buflen); } + // return camera model name (e.g. "C11", "C13"), queried from the gimbal via the "MOD" command. + // this same driver supports every model in SkyDroid's "TOP protocol" gimbal camera family; + // the model name lets the GCS show which one is actually connected + void get_camera_model_name(char *buf, uint8_t buflen) const override { + if (!_got_model_name) { + return; + } + strncpy(buf, _model_name, buflen); + } + // return camera firmware version + uint32_t get_camera_firmware_version() const override { return _firmware_ver; } + // return camera capability flags + uint32_t get_camera_cap_flags() const override { + return (CAMERA_CAP_FLAGS_CAPTURE_VIDEO | + CAMERA_CAP_FLAGS_CAPTURE_IMAGE | + CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM); + } + + // send camera settings message to GCS + void send_camera_settings(mavlink_channel_t chan) const override; + +protected: + + // get attitude as a quaternion. returns true on success + bool get_attitude_quaternion(Quaternion& att_quat) override; + + // SkyDroid can send either rates or angles, and also has a dedicated one-shot + // "center" command for retract/neutral (see send_target_retracted()/ + // send_target_neutral()) rather than falling through to the angle-based + // conversion every other target type uses + uint8_t natively_supported_mount_target_types() const override { + return NATIVE_ANGLES_AND_RATES_ONLY | + (1U << uint8_t(MountTargetType::RETRACTED)) | + (1U << uint8_t(MountTargetType::NEUTRAL)); + }; + + // move to a "retracted" position: SkyDroid has no separate stow position, so + // this uses the same one-shot "center" command as send_target_neutral() + void send_target_retracted() override; + + // move to a neutral (forward-pointing) position using the gimbal's own one-shot + // "center" command (PTZ data byte 0x05), rather than the angle-based conversion + // every other target type falls through to (which would drive our own P-controller + // toward _params.neutral_angles, requiring GAC attitude feedback to converge - see + // send_target_angles()). This is deliberately independent of that feedback loop: + // a switch mapped to RC_TARGETING->NEUTRAL should reliably point the gimbal + // forward using the gimbal's own logic, not depend on our closed loop ever + // having received a GAC packet + void send_target_neutral() override; + +private: + + // address (2nd and 3rd bytes of packet) + // first byte is always U (external control unit, whether connected over + // UART or UDP - SkyDroid's protocol doesn't distinguish the two at the + // address-byte level) for our outgoing packets + enum class AddressByte : uint8_t { + SYSTEM_AND_IMAGE = 68, // 'D' + GIMBAL = 71, // 'G' + LENS = 77, // 'M' + UDP = 85, // 'U' + }; + + // steps of the 1hz round-robin housekeeping loop in update() - not every value is + // used every cycle (some requests only fire until answered once), and a few + // numbers are deliberately left spare for anything added later without needing + // to renumber the rest + enum class ReqStep : uint8_t { + VERSION = 0, // request_gimbal_version(), until _got_gimbal_version + TIME_SYNC = 1, // send_time_sync() + ATTITUDE_ENABLE = 2, // request_gimbal_attitude() + MODEL = 3, // request_gimbal_model(), until _got_model_name + SDCARD = 4, // request_gimbal_sdcard_info() + ATTITUDE_ACCEPT = 6, // send_attitude_enable() - note the gap at 5, spare + NUM_STEPS = 10, // wraps back to VERSION after this - note the gap at 7-9, spare + }; + + // memory card state, as last reported by "SDC" (see gimbal_sdcard_analyse()). + // UNKNOWN until the first reply arrives - camera-addressed replies like this one + // can take anywhere from under a second to 8+ minutes (see this file's header + // comment), so capture attempts must not be blocked on a card that may simply not + // have reported in yet + enum class SDCardState : uint8_t { + UNKNOWN = 0, + PRESENT = 1, + ABSENT = 2, + }; + + // send text prefix string + static const char* send_message_prefix; + + // AP_Mount_Backend_TPFrame overrides - see that class for what each means + void handle_message(const char* msg_id) override; + uint8_t packetlen_max() const override { return AP_MOUNT_SKYDROID_PACKETLEN_MAX; } + bool is_valid_address_byte(uint8_t b) const override { + return b == (uint8_t)AddressByte::UDP || b == (uint8_t)AddressByte::LENS || + b == (uint8_t)AddressByte::SYSTEM_AND_IMAGE || b == (uint8_t)AddressByte::GIMBAL; + } + uint8_t source_address_byte() const override { return (uint8_t)AddressByte::UDP; } + + // request gimbal to start sending attitude at AP_MOUNT_SKYDROID_ATTITUDE_RATE_HZ + void request_gimbal_attitude(); + + // request gimbal memory card information + void request_gimbal_sdcard_info(); + + // request gimbal version + void request_gimbal_version(); + + // request gimbal model name (e.g. "C11", "C13") + void request_gimbal_model(); + + // send current UTC date/time to the gimbal (TIM command) so it can correctly + // timestamp photos/videos - the camera has no RTC of its own and defaults to + // 1970-01-01 without this (confirmed on real hardware). Resent periodically, + // same as request_gimbal_attitude()/send_attitude_enable(), both as a guard + // against UDP packet loss and to recover if the camera reboots independently + // of the flight controller (also confirmed to happen on real hardware) + bool send_time_sync(); + + // enable the gimbal to receive our attitude (FAE) and send it to us (GAA) + bool send_attitude_enable(); + + // send our current attitude to the gimbal (FAI) + bool send_attitude_to_gimbal(); + + // send angle target in radians to gimbal. Closes the loop ourselves with a + // P-controller over the gimbal's own "GAC" attitude feedback, driving GSY/GSP as + // the rate actuator - there is no absolute-angle command that works on this + // hardware (see this file's header comment) + void send_target_angles(const MountAngleTarget& angle_rad) override; + + // send rate target in rad/s to gimbal, directly via GSY/GSP + void send_target_rates(const MountRateTarget& rate_rads) override; + + // send a single-axis rate command (GSY or GSP) for rate_dps, converted to the + // wire's signed 8bit LSB units using the real-world calibrated scale (see + // AP_MOUNT_SKYDROID_AXIS_DPS_PER_LSB). Caller is responsible for any + // axis-specific sign compensation (GSY's sign is inverted vs AP_Mount's + // convention - see send_target_rates()) + void send_axis_rate(const Identifier id, float rate_dps); + + // attitude information analysis of gimbal (response to GAA, arrives as "GAC") + void gimbal_angle_analyse(); + + // gimbal video information analysis + void gimbal_record_analyse(); + + // information analysis of gimbal storage card + void gimbal_sdcard_analyse(); + + // gimbal basic information analysis + void gimbal_version_analyse(); + + // gimbal model name analysis (raw ASCII text, e.g. "C13") + void gimbal_model_analyse(); + + // thin wrappers keeping call sites typed on our own AddressByte rather than + // the base class's raw uint8_t (which must accommodate every product's own, + // differently-valued AddressByte enum) + bool send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value) { + return AP_Mount_Backend_TPFrame::send_fixedlen_packet((uint8_t)address, id, write, value); + } + bool send_variablelen_packet(HeaderType header, AddressByte address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len) { + return AP_Mount_Backend_TPFrame::send_variablelen_packet(header, (uint8_t)address, id, write, databuff, databuff_len); + } + + // set gimbal's lock vs follow mode + // lock should be true if gimbal should maintain an earth-frame target + // lock is false to follow / maintain a body-frame target + bool set_gimbal_lock(bool lock); + + // send the gimbal's own one-shot "center" command (PTZ data byte 0x05). Used by + // both send_target_retracted() and send_target_neutral() - see their comments + bool send_center_command(); + + // members + bool _recording; // recording status, tracked locally from commands we've sent + SDCardState _sdcard_state = SDCardState::UNKNOWN; // memory card state, as last reported by the gimbal (see SDCardState) + bool _last_lock; // last lock mode sent to gimbal, only meaningful once _lock_sent + bool _lock_sent; // true once set_gimbal_lock() has sent a mode at least once + bool _got_gimbal_version; // true if gimbal's version has been received + bool _got_model_name; // true if gimbal's model name has been received + bool _announced_connected; // true once we've told the user the gimbal is connected + uint32_t _firmware_ver; // firmware version + char _model_name[8]; // gimbal model name (e.g. "C11", "C13"), always null-terminated + Vector3f _current_angle_rad; // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw). roll is reported by the gimbal's own self-stabilization and is not controllable - see has_roll_control() + uint32_t _last_current_angle_ms; // system time (in milliseconds) that angle information received from the gimbal + uint32_t _last_req_current_info_ms; // system time that this driver last requested current gimbal information + uint8_t _last_req_step; // 10hz request loop step (different requests are sent at various steps) +}; + +#endif // HAL_MOUNT_SKYDROID_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp index 95b3831377c9d..1040b7776a3e1 100644 --- a/libraries/AP_Mount/AP_Mount_Topotek.cpp +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -17,8 +17,6 @@ extern const AP_HAL::HAL& hal; #define TRACK_RANGE 60 // the size of the image at point tracking #define AP_MOUNT_TOPOTEK_UPDATE_INTERVAL_MS 100 // resend angle or rate targets to gimbal at this interval #define AP_MOUNT_TOPOTEK_HEALTH_TIMEOUT_MS 1000 // timeout for health and rangefinder readings -#define AP_MOUNT_TOPOTEK_PACKETLEN_MIN 12 // packet length not including the data segment -#define AP_MOUNT_TOPOTEK_DATALEN_MAX (AP_MOUNT_TOPOTEK_PACKETLEN_MAX - AP_MOUNT_TOPOTEK_PACKETLEN_MIN) // data segment lens can be no more tha this // 3 character identifiers # define AP_MOUNT_TOPOTEK_ID3CHAR_CAPTURE "CAP" // take picture, data bytes: 01:RGB + thermal, 02:RGB, 03:thermal, 05:RGB + thermal (with temp measurement) @@ -49,9 +47,6 @@ extern const AP_HAL::HAL& hal; # define AP_MOUNT_TOPOTEK_ID3CHAR_SET_ALT "ALT" // set the gimbal's altitude # define AP_MOUNT_TOPOTEK_ID3CHAR_SET_AZIMUTH "AZI" // set the gimbal's yaw (aka azimuth) -#define AP_MOUNT_TOPOTEK_DEBUG 0 -#define debug(fmt, args ...) do { if (AP_MOUNT_TOPOTEK_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Topotek: " fmt, ## args); } } while (0) - const char* AP_Mount_Topotek::send_message_prefix = "Mount: Topotek"; // update mount position - should be called periodically @@ -495,158 +490,15 @@ bool AP_Mount_Topotek::get_attitude_quaternion(Quaternion& att_quat) return true; } -// reading incoming packets from gimbal and confirm they are of the correct format -void AP_Mount_Topotek::read_incoming_packets() +// dispatch on the 3-character command ID to the function that consumes that +// message - called by AP_Mount_Backend_TPFrame::read_incoming_packets() once a +// packet's CRC has been verified +void AP_Mount_Topotek::handle_message(const char* msg_id) { - // check for bytes on the serial port - int16_t nbytes = MIN(_uart->available(), 1024U); - if (nbytes <= 0 ) { - return; - } - - // flag to allow cases below to reset parser state - bool reset_parser = false; - - // process bytes received - for (int16_t i = 0; i < nbytes; i++) { - uint8_t b; - if (!_uart->read(b)) { - continue; - } - - // add latest byte to buffer - _msg_buff[_msg_buff_len++] = b; - - // protect against overly long messages - if (_msg_buff_len >= AP_MOUNT_TOPOTEK_PACKETLEN_MAX) { - reset_parser = true; - } - - // process byte depending upon current state - switch (_parser.state) { - - case ParseState::WAITING_FOR_HEADER1: - if (b == '#') { - _parser.state = ParseState::WAITING_FOR_HEADER2; - break; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_HEADER2: - if (b == 't' || b == 'T') { - _parser.state = ParseState::WAITING_FOR_HEADER3; - break; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_HEADER3: - if (b == 'p' || b == 'P') { - _parser.state = ParseState::WAITING_FOR_ADDR1; - break; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_ADDR1: - case ParseState::WAITING_FOR_ADDR2: - if (b == 'U' || b =='M' || b == 'D' || b =='E' || b =='P' || b =='G') { - // advance to next state - _parser.state = (ParseState)((uint8_t)_parser.state+1); - break; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_DATALEN: { - // sanity check data length - uint8_t data_len; - if (hex_char_to_nibble(b, data_len) && data_len <= AP_MOUNT_TOPOTEK_DATALEN_MAX) { - _parser.data_len = data_len; - _parser.state = ParseState::WAITING_FOR_CONTROL; - break; - } - reset_parser = true; - break; - } - - case ParseState::WAITING_FOR_CONTROL: - // r or w - if (b == 'r' || b == 'w') { - _parser.state = ParseState::WAITING_FOR_ID1; - break; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_ID1: - case ParseState::WAITING_FOR_ID2: - case ParseState::WAITING_FOR_ID3: - // check all uppercase letters and numbers. eg 'GAC' - if ((b >= 'A' && b <= 'Z') || (b >= '0' && b <= '9')) { - // advance to next state - _parser.state = (ParseState)((uint8_t)_parser.state+1); - break; - } - reset_parser = true; - break; - - case ParseState::WAITING_FOR_DATA: { - // normally hex numbers in char form (e.g. '0A') - const uint8_t data_bytes_received = _msg_buff_len - (AP_MOUNT_TOPOTEK_PACKETLEN_MIN - 2); - - // sanity check to protect against programming errors - if (data_bytes_received > AP_MOUNT_TOPOTEK_DATALEN_MAX) { - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - reset_parser = true; - break; - } - - // advance parser state once expected number of bytes have been received - if (data_bytes_received == _parser.data_len) { - _parser.state = ParseState::WAITING_FOR_CRC_LOW; - } - break; - } - - case ParseState::WAITING_FOR_CRC_LOW: - _parser.state = ParseState::WAITING_FOR_CRC_HIGH; + for (uint8_t count = 0; count < AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM; count++) { + if (strncmp(msg_id, (const char*)(uart_recv_cmd_compare_list[count].uart_cmd_key), 3) == 0) { + (this->*(uart_recv_cmd_compare_list[count].func))(); break; - - case ParseState::WAITING_FOR_CRC_HIGH: - // this is the last byte in the message so reset the parser - reset_parser = true; - - // sanity check to protect against programming errors - if (_msg_buff_len < AP_MOUNT_TOPOTEK_PACKETLEN_MIN) { - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - break; - } - - // calculate and check CRC - const uint8_t crc_value = calculate_crc(_msg_buff, _msg_buff_len - 2); - const char crc_char1 = hex2char((crc_value >> 4) & 0x0f); - const char crc_char2 = hex2char((crc_value) & 0x0f); - if (crc_char1 != _msg_buff[_msg_buff_len - 2] || crc_char2 != _msg_buff[_msg_buff_len-1]) { - debug("CRC expected:%x got:%c%c", (int)crc_value, crc_char1, crc_char2); - break; - } - - // CRC is OK, call function to process the message - for (uint8_t count = 0; count < AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM; count++) { - if (strncmp((const char*)_msg_buff + 7, (const char*)(uart_recv_cmd_compare_list[count].uart_cmd_key), 3) == 0) { - (this->*(uart_recv_cmd_compare_list[count].func))(); - break; - } - } - } - - // handle reset of parser - if (reset_parser) { - _parser.state = ParseState::WAITING_FOR_HEADER1; - _msg_buff_len = 0; - reset_parser = false; } } } @@ -1032,108 +884,23 @@ void AP_Mount_Topotek::gimbal_model_name_analyse() _got_gimbal_model_name = true; } -// calculate checksum -uint8_t AP_Mount_Topotek::calculate_crc(const uint8_t *cmd, uint8_t len) const -{ - uint8_t crc = 0; - for (uint16_t i = 0; i= data)) { - return (data + '0'); - } else { - return (data - 10 + 'A'); - } -} - - -// send a fixed length packet -bool AP_Mount_Topotek::send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value) -{ - uint8_t databuff[3]; - hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02X", value); - return send_variablelen_packet(HeaderType::FIXED_LEN, address, id, write, databuff, ARRAY_SIZE(databuff)-1); -} - -// send variable length packet -bool AP_Mount_Topotek::send_variablelen_packet(HeaderType header, AddressByte address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len) -{ - // exit immediately if not initialised - if (!_initialised) { - return false; - } - - // calculate and sanity check packet size - const uint16_t packet_size = AP_MOUNT_TOPOTEK_PACKETLEN_MIN + databuff_len; - if (packet_size > AP_MOUNT_TOPOTEK_PACKETLEN_MAX) { - debug("send_packet data buff too large"); - return false; - } - - // check for sufficient space in outgoing buffer - if (_uart->txspace() < packet_size) { - debug("tx buffer full"); - return false; - } - - // create buffer for holding outgoing packet - uint8_t send_buff[packet_size]; - uint8_t send_buff_ofs = 0; - - // packet header (bytes 0 ~ 2) - send_buff[send_buff_ofs++] = '#'; - send_buff[send_buff_ofs++] = (header == HeaderType::FIXED_LEN) ? 'T' : 't'; - send_buff[send_buff_ofs++] = (header == HeaderType::FIXED_LEN) ? 'P' : 'p'; - - // address (bytes 3, 4) - send_buff[send_buff_ofs++] = (uint8_t)source_address(); - send_buff[send_buff_ofs++] = (uint8_t)address; - - // data length (byte 5) - send_buff[send_buff_ofs++] = hex2char(databuff_len); - - // control byte (byte 6) - send_buff[send_buff_ofs++] = write ? (uint8_t)ControlByte::WRITE : (uint8_t)ControlByte::READ; - - // identified (bytes 7 ~ 9) - send_buff[send_buff_ofs++] = id[0]; - send_buff[send_buff_ofs++] = id[1]; - send_buff[send_buff_ofs++] = id[2]; - - // data - if (databuff_len != 0) { - memcpy(&send_buff[send_buff_ofs], databuff, databuff_len); - send_buff_ofs += databuff_len; - } - - // crc - uint8_t crc = calculate_crc(send_buff, send_buff_ofs); - send_buff[send_buff_ofs++] = hex2char((crc >> 4) & 0x0f); - send_buff[send_buff_ofs++] = hex2char(crc & 0x0f); - - // send packet - _uart->write(send_buff, send_buff_ofs); - return true; -} - // set gimbal's lock vs follow mode // lock should be true if gimbal should maintain an earth-frame target // lock is false to follow / maintain a body-frame target bool AP_Mount_Topotek::set_gimbal_lock(bool lock) { - if (_last_lock == lock) { + // _last_lock defaults false, which is indistinguishable from "we've already + // confirmed the gimbal is in follow mode" unless we also track whether a mode + // has actually been sent yet - without _lock_sent, the very first call (often + // requesting follow, false) would silently no-op instead of sending anything + if (_lock_sent && _last_lock == lock) { return true; } // send message and update lock state if (send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_MODE, true, lock ? 6 : 7)) { _last_lock = lock; + _lock_sent = true; return true; } return false; diff --git a/libraries/AP_Mount/AP_Mount_Topotek.h b/libraries/AP_Mount/AP_Mount_Topotek.h index adead6799e4e6..b2ee60d064d33 100644 --- a/libraries/AP_Mount/AP_Mount_Topotek.h +++ b/libraries/AP_Mount/AP_Mount_Topotek.h @@ -1,6 +1,10 @@ /* Topotek gimbal driver using custom serial protocol + This "#TP"/"#tp" wire framing is also used by SkyDroid's OEM'd gimbal + family (see AP_Mount_SkyDroid) - both derive from AP_Mount_Backend_TPFrame, + which implements the shared framing/CRC/packet-send layer. + Packet format (courtesy of Topotek's SDK document) ------------------------------------------------------------------------------------------- @@ -22,7 +26,7 @@ #if HAL_MOUNT_TOPOTEK_ENABLED -#include "AP_Mount_Backend_Serial.h" +#include "AP_Mount_Backend_TPFrame.h" #include #include #include @@ -30,12 +34,12 @@ #define AP_MOUNT_TOPOTEK_PACKETLEN_MAX 36 // maximum number of bytes in a packet sent to or received from the gimbal #define AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM 7 // parse the number of gimbal command types -class AP_Mount_Topotek : public AP_Mount_Backend_Serial +class AP_Mount_Topotek : public AP_Mount_Backend_TPFrame { public: // Constructor - using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial; + using AP_Mount_Backend_TPFrame::AP_Mount_Backend_TPFrame; // Do not allow copies CLASS_NO_COPY(AP_Mount_Topotek); @@ -132,13 +136,6 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial private: - // header type (fixed or variable length) - // first three bytes of packet determined by this value - enum class HeaderType : uint8_t { - FIXED_LEN = 0x00, // #TP will be sent - VARIABLE_LEN = 0x01, // #tp will be sent - }; - // address (2nd and 3rd bytes of packet) // first byte is always U followed by one of the other options enum class AddressByte : uint8_t { @@ -150,30 +147,6 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial UART = 85, // 'U' }; - // control byte (read or write) - // sent as 7th byte of packet - enum class ControlByte : uint8_t { - READ = 114, // 'r' - WRITE = 119, // 'w' - }; - - // parsing state - enum class ParseState : uint8_t { - WAITING_FOR_HEADER1 = 0,// # - WAITING_FOR_HEADER2, // T or t - WAITING_FOR_HEADER3, // P or p - WAITING_FOR_ADDR1, // normally U - WAITING_FOR_ADDR2, // M, D, E, P, G - WAITING_FOR_DATALEN, - WAITING_FOR_CONTROL, // r or w - WAITING_FOR_ID1, // e.g. 'G' - WAITING_FOR_ID2, // e.g. 'A' - WAITING_FOR_ID3, // e.g. 'C' - WAITING_FOR_DATA, // normally hex numbers in char form (e.g. '0A') - WAITING_FOR_CRC_LOW, - WAITING_FOR_CRC_HIGH, - }; - // tracking status enum class TrackingStatus : uint8_t { STOPPED_TRACKING = 0x30, // not tracking @@ -182,14 +155,17 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial LENS_UNSUPPORT_TRACK = 0x34, // this lens does not support tracking }; - // identifier bytes - typedef char Identifier[3]; - // send text prefix string static const char* send_message_prefix; - // reading incoming packets from gimbal and confirm they are of the correct format - void read_incoming_packets(); + // AP_Mount_Backend_TPFrame overrides - see that class for what each means + void handle_message(const char* msg_id) override; + uint8_t packetlen_max() const override { return AP_MOUNT_TOPOTEK_PACKETLEN_MAX; } + bool is_valid_address_byte(uint8_t b) const override { + return b == (uint8_t)AddressByte::UART || b == (uint8_t)AddressByte::LENS || + b == (uint8_t)AddressByte::SYSTEM_AND_IMAGE || b == (uint8_t)AddressByte::AUXILIARY_EQUIPMENT || + b == (uint8_t)AddressByte::NETWORK || b == (uint8_t)AddressByte::GIMBAL; + } // request gimbal attitude void request_gimbal_attitude(); @@ -239,28 +215,24 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial // gimbal distance information analysis void gimbal_dist_info_analyse(); - // return the address to send as the source of our packets. The + // return the address byte to send as the source of our packets. The // gimbal sends its replies out of the interface named here, so this // must be the interface we are actually connected to. This is one // of the few places it is legitimate to ask a port how it is // connected; the protocol itself carries that information - AddressByte source_address() const { - return _uart->is_network_port() ? AddressByte::NETWORK : AddressByte::UART; + uint8_t source_address_byte() const override { + return (uint8_t)(_uart->is_network_port() ? AddressByte::NETWORK : AddressByte::UART); } - // calculate checksum - uint8_t calculate_crc(const uint8_t *cmd, uint8_t len) const; - - // hexadecimal to character conversion - uint8_t hex2char(uint8_t data) const; - - // send a fixed length packet to gimbal - // returns true on success, false if serial port initialization failed - bool send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value); - - // send a variable length packet to gimbal - // returns true on success, false if serial port initialization failed - bool send_variablelen_packet(HeaderType header, AddressByte address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len); + // thin wrappers keeping call sites typed on our own AddressByte rather than + // the base class's raw uint8_t (which must accommodate every product's own, + // differently-valued AddressByte enum) + bool send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value) { + return AP_Mount_Backend_TPFrame::send_fixedlen_packet((uint8_t)address, id, write, value); + } + bool send_variablelen_packet(HeaderType header, AddressByte address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len) { + return AP_Mount_Backend_TPFrame::send_variablelen_packet(header, (uint8_t)address, id, write, databuff, databuff_len); + } // set gimbal's lock vs follow mode // lock should be true if gimbal should maintain an earth-frame target @@ -273,7 +245,8 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial TrackingStatus _last_tracking_state = TrackingStatus::STOPPED_TRACKING; // last tracking state received from gimbal uint8_t _last_mode; // mode during latest update, used to detect mode changes and cancel tracking bool _sdcard_status; // memory card status (received from gimbal) - bool _last_lock; // last lock mode sent to gimbal + bool _last_lock; // last lock mode sent to gimbal, only meaningful once _lock_sent + bool _lock_sent; // true once set_gimbal_lock() has sent a mode at least once bool _got_gimbal_version; // true if gimbal's version has been received bool _got_gimbal_model_name; // true if gimbal's model name has been received bool _last_zoom_stop; // true if zoom has been stopped (used to re-send in order to handle lost packets) @@ -287,12 +260,6 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial uint8_t _last_req_step; // 10hz request loop step (different requests are sent at various steps) uint8_t _stop_order_count; // number of stop commands sent since target rates became zero float _measure_dist_m = -1.0f; // latest rangefinder distance (in meters) - uint8_t _msg_buff[AP_MOUNT_TOPOTEK_PACKETLEN_MAX]; // buffer holding bytes from latest packet received. only used to calculate crc - uint8_t _msg_buff_len; // number of bytes in the msg buffer - struct { - ParseState state; // parser state - uint8_t data_len; // expected number of data bytes - } _parser; // mapping from received message key to member function pointer to consume the message typedef struct { diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index fcc4239292115..6b91c5c7c957a 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/libraries/AP_Mount/AP_Mount_config.h @@ -74,6 +74,10 @@ #define HAL_MOUNT_TOPOTEK_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED #endif +#ifndef HAL_MOUNT_SKYDROID_ENABLED +#define HAL_MOUNT_SKYDROID_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED && HAL_PROGRAM_SIZE_LIMIT_KB > 1024 +#endif + // set camera source is supported on gimbals that may have more than one lens #ifndef HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED #define HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED HAL_MOUNT_SIYI_ENABLED || HAL_MOUNT_XACTI_ENABLED || HAL_MOUNT_VIEWPRO_ENABLED diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h index 3be7a5539b575..9d19fa4423959 100644 --- a/libraries/AP_Networking/AP_Networking.h +++ b/libraries/AP_Networking/AP_Networking.h @@ -277,6 +277,13 @@ class AP_Networking bool init_buffers(const uint32_t size_rx, const uint32_t size_tx); void thread_create(AP_HAL::MemberProc); + // true if addr (host byte order, as returned by AP_Networking_IPV4::get_uint32() + // or SocketAPM::last_recv_address()) is the broadcast or a multicast address + static bool is_broadcast_or_multicast(uint32_t addr) { + const uint8_t first_octet = (addr >> 24) & 0xFF; + return addr == 0xFFFFFFFF || (first_octet >= 224 && first_octet <= 239); + } + uint32_t txspace() override; void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override; size_t _write(const uint8_t *buffer, size_t size) override; @@ -299,6 +306,7 @@ class AP_Networking uint32_t last_size_rx; bool packetise; bool connected; + bool is_udp_client_unicast; // only meaningful when type == UDP_CLIENT, set in udp_client_loop() uint32_t last_udp_connect_address; uint16_t last_udp_connect_port; bool have_received; diff --git a/libraries/AP_Networking/AP_Networking_port.cpp b/libraries/AP_Networking/AP_Networking_port.cpp index 18bc64592b607..fb58f5754e1b4 100644 --- a/libraries/AP_Networking/AP_Networking_port.cpp +++ b/libraries/AP_Networking/AP_Networking_port.cpp @@ -181,11 +181,26 @@ void AP_Networking::Port::udp_client_loop(void) AP::network().startup_wait(); const char *dest = ip.get_str(); - if (!sock->connect(dest, port.get())) { - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UDP[%u]: Failed to connect to %s", (unsigned)state.idx, dest); - delete sock; - sock = nullptr; - return; + is_udp_client_unicast = !is_broadcast_or_multicast(ip.get_uint32()); + + if (is_udp_client_unicast) { + // deliberately not calling sock->connect(): a connect()'d UDP socket + // has its incoming packets filtered by the kernel to the exact + // address *and port* it connected to, but some devices reply from a + // different source port than the one they were queried on. + // send_receive() uses sendto() for our fixed destination instead, + // and checks the source IP itself (ignoring port) on receive + } else { + // broadcast/multicast: connect() also joins the multicast group + // (IP_ADD_MEMBERSHIP) when the destination is one, which we must + // not skip - and neither of these targets are point-to-point, so + // they don't have the mismatched-reply-port problem above + if (!sock->connect(dest, port.get())) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UDP[%u]: Failed to connect to %s", (unsigned)state.idx, dest); + delete sock; + sock = nullptr; + return; + } } GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UDP[%u]: connected to %s:%u", (unsigned)state.idx, dest, unsigned(port.get())); @@ -339,15 +354,28 @@ bool AP_Networking::Port::send_receive(void) return false; } if (ret > 0) { - WITH_SEMAPHORE(sem); - readbuffer->write(buf, ret); + bool accept = true; + if (type == NetworkPortType::UDP_CLIENT && is_udp_client_unicast) { + // our socket isn't connect()'d for a unicast destination (see + // udp_client_loop()), so the kernel doesn't filter incoming + // packets for us - check the source IP ourselves. Deliberately + // not checking the source port: some devices reply from a + // different port than the one they were queried on + uint32_t src_addr = 0; + uint16_t src_port = 0; + accept = sock->last_recv_address(src_addr, src_port) && (src_addr == ip.get_uint32()); + } + if (accept) { + WITH_SEMAPHORE(sem); + readbuffer->write(buf, ret); - // Cant track dropped read packets because we only read in what there is space for - // The socket buffer becomes full and data is lost there - rx_stats_bytes += ret; + // Cant track dropped read packets because we only read in what there is space for + // The socket buffer becomes full and data is lost there + rx_stats_bytes += ret; - active = true; - have_received = true; + active = true; + have_received = true; + } } } @@ -410,8 +438,14 @@ bool AP_Networking::Port::send_receive(void) if(last_udp_connect_address != 0 && last_udp_connect_port != 0) { ret = sock->sendto(buf, n, last_udp_connect_address, last_udp_connect_port); } + } else if (type == NetworkPortType::UDP_CLIENT && is_udp_client_unicast) { + // a unicast UDP Client also uses sendto rather than a connect()'d + // send() - see udp_client_loop() and the receive-side comment + // above for why + ret = sock->sendto(buf, n, ip.get_uint32(), port.get()); } else { - // TCP Server and Client and UDP Client use send + // TCP Server and Client, and a broadcast/multicast UDP Client + // (which is connect()'d - see udp_client_loop()), use send() ret = sock->send(buf, n); } diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 9721834303ac4..6d2ec642213ba 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -39,6 +39,7 @@ #include "SIM_GPIO_LED_RGB.h" #include "SIM_Siyi.h" #include "SIM_Topotek.h" +#include "SIM_SkyDroid.h" #include "SIM_Viewpro.h" #include "SIM_Mount.h" diff --git a/libraries/SITL/SIM_SerialDevice.cpp b/libraries/SITL/SIM_SerialDevice.cpp index 6f26c22f4cf10..2856d7b340fd3 100644 --- a/libraries/SITL/SIM_SerialDevice.cpp +++ b/libraries/SITL/SIM_SerialDevice.cpp @@ -159,10 +159,40 @@ bool SerialDevice::listen_on_tcp_port(const uint16_t port) listener = nullptr; return false; } + listener_is_udp = false; ::printf("SIM: device listening for autopilot on TCP port %u\n", unsigned(port)); return true; } +/* + attach this device to a UDP socket. The autopilot sends datagrams to + this socket (e.g. with a NET_Pn port configured as a UDP client) + instead of talking to the device over a simulated serial port. + Unlike TCP there is no connection to accept; the autopilot's address + is simply learned from whichever packet it last sent us, and replies + are sent back to that address + */ +bool SerialDevice::listen_on_udp_port(const uint16_t port) +{ + listener = NEW_NOTHROW SocketAPM_native(true); + if (listener == nullptr) { + return false; + } + listener->reuseaddress(); + if (!listener->bind("127.0.0.1", port) || + !listener->set_blocking(false)) { + ::fprintf(stderr, "SIM: failed to bind UDP port %u: %m\n", unsigned(port)); + delete listener; + listener = nullptr; + return false; + } + listener_is_udp = true; + udp_peer_addr = 0; + udp_peer_port = 0; + ::printf("SIM: device listening for autopilot on UDP port %u\n", unsigned(port)); + return true; +} + /* move bytes between the network socket and this device. This performs the same role the SITL UART driver performs for serially-attached @@ -175,6 +205,11 @@ void SerialDevice::network_update() return; } + if (listener_is_udp) { + network_update_udp(); + return; + } + if (sock == nullptr) { sock = listener->accept(0); if (sock == nullptr) { @@ -212,6 +247,52 @@ void SerialDevice::network_update() write_to_device(buffer, nread); } } + +/* + UDP variant of network_update(). There is no connection to accept; + we simply learn the autopilot's address from whichever datagram it + last sent us and reply to that address. Until a first datagram has + arrived we have nowhere to send device->autopilot traffic, so it is + dropped (matching how a real UDP device behaves before its peer has + said anything) + */ +void SerialDevice::network_update_udp() +{ + char buffer[512]; + + // autopilot to device: + while (true) { + const ssize_t nread = listener->recv(buffer, sizeof(buffer), 0); + if (nread <= 0) { + break; + } + listener->last_recv_address(udp_peer_addr, udp_peer_port); + write_to_device(buffer, nread); + } + + // device to autopilot: + if (udp_peer_addr == 0) { + // haven't heard from the autopilot yet, nowhere to send + return; + } + while (true) { + // AP_Networking_port::run() (AP_Networking_port.cpp) reads at + // most 300 bytes per recv() call. UDP is datagram-based, not + // a byte stream like TCP, so a datagram larger than that isn't + // queued for a later read -- the kernel silently discards + // whatever didn't fit. Cap what we send to what the far end + // can actually receive in one call, or a device that bursts + // more than 300 bytes at once would have its frame truncated. + const ssize_t nread = read_from_device(buffer, MIN(sizeof(buffer), (size_t)300)); + if (nread <= 0) { + break; + } + if (listener->sendto(buffer, nread, udp_peer_addr, udp_peer_port) != nread) { + // if the autopilot is not keeping up we simply drop the data + break; + } + } +} #endif // AP_SIM_SERIALDEVICE_NETWORK_ENABLED /** diff --git a/libraries/SITL/SIM_SerialDevice.h b/libraries/SITL/SIM_SerialDevice.h index 8a7bc5f9c2edc..f5cb70f43235e 100644 --- a/libraries/SITL/SIM_SerialDevice.h +++ b/libraries/SITL/SIM_SerialDevice.h @@ -48,10 +48,18 @@ class SerialDevice { #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED // attach this device to a TCP server socket rather than to a // simulated serial port. This simulates a device which the - // autopilot reaches over the network (e.g. via a NET_Pn port) - // rather than over one of its serial ports. Returns true on success + // autopilot reaches over the network (e.g. via a NET_Pn port + // configured as a TCP client) rather than over one of its serial + // ports. Returns true on success bool listen_on_tcp_port(uint16_t port) WARN_IF_UNUSED; + // attach this device to a UDP socket rather than to a simulated + // serial port. This simulates a device which the autopilot + // reaches over the network (e.g. via a NET_Pn port configured as + // a UDP client) rather than over one of its serial ports. + // Returns true on success + bool listen_on_udp_port(uint16_t port) WARN_IF_UNUSED; + // true if this device is attached to the autopilot via a network // socket rather than via a simulated serial port bool is_network_attached() const { return listener != nullptr; } @@ -76,13 +84,21 @@ class SerialDevice { bool is_match_baud(void) const; +#if AP_SIM_SERIALDEVICE_NETWORK_ENABLED + // UDP variant of network_update() + void network_update_udp(); +#endif + // baudrate the autopilot has this device open at; zero if the // device is not attached to a simulated serial port uint32_t autopilot_baud; #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED SocketAPM_native *listener = nullptr; // socket the autopilot connects to, nullptr if serially attached - SocketAPM_native *sock = nullptr; // socket to the connected autopilot, nullptr if not connected + SocketAPM_native *sock = nullptr; // TCP: socket to the connected autopilot, nullptr if not connected. unused for UDP + bool listener_is_udp; // true if listener is a UDP socket rather than a TCP listening socket + uint32_t udp_peer_addr; // UDP: IP address of the autopilot, learned from the last packet received. 0 if no packet received yet + uint16_t udp_peer_port; // UDP: port of the autopilot, learned from the last packet received #endif // AP_SIM_SERIALDEVICE_NETWORK_ENABLED ssize_t corrupt_transfer(char *buffer, const ssize_t ret, const size_t size) const; diff --git a/libraries/SITL/SIM_SkyDroid.cpp b/libraries/SITL/SIM_SkyDroid.cpp new file mode 100644 index 0000000000000..4a048913423e9 --- /dev/null +++ b/libraries/SITL/SIM_SkyDroid.cpp @@ -0,0 +1,301 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + Simulator for SkyDroid gimbal +*/ + +#include "SIM_config.h" +#include + +#if AP_SIM_SKYDROID_ENABLED + +#include "SIM_SkyDroid.h" +#include "SIM_Aircraft.h" +#include +#include + +using namespace SITL; + +void SkyDroid::update(const Aircraft &aircraft) +{ + Matrix3f gimbal_dcm; + gimbal.get_dcm(gimbal_dcm); + const Vector3f vehicle_rate_gimbal = gimbal_dcm.transposed() * aircraft.get_dcm() * aircraft.get_gyro(); + + // Confirmed on real hardware: GAM/GSM/GAY/GAP are silently ignored, only the + // individual-axis GSY/GSP speed commands actually move the gimbal, and they are + // genuinely proportional (unlike PTZ's fixed-speed jog, which was tried first and + // found not to move yaw at all). Real-world calibration: see + // AP_MOUNT_SKYDROID_AXIS_DPS_PER_LSB in AP_Mount_SkyDroid.cpp for the + // 0.5deg/s-per-LSB scale this mirrors, matching the protocol doc and SkyDroid's + // own RCSDK. GSY's sign is also inverted vs the doc on real hardware; this + // simulation reproduces that same inversion so it cancels out correctly against + // the driver's compensating negation in send_target_rates(), exactly like the + // real gimbal does + const float dps_per_lsb = 0.5f; + Vector3f ja; + gimbal.get_joint_angles(ja); + float pitch_rate; + float yaw_rate; + if (_centering) { + // simulate the gimbal's own one-shot "center" response to "PTZ" 0x05 - drive + // pitch/yaw toward zero using the same simple P-controller approach the real + // driver uses for closed-loop control elsewhere, since we have no real + // hardware data on how a real gimbal's own centering actually moves + constexpr float gain = 10.0f; + pitch_rate = -ja.y * gain; + yaw_rate = -ja.z * gain; + } else { + pitch_rate = radians(_commanded_pitch_speed_lsb * dps_per_lsb); + yaw_rate = -radians(_commanded_yaw_speed_lsb * dps_per_lsb); + } + + // roll is left entirely to the simulated gimbal's own stabilization, matching the + // real hardware: SkyDroid have confirmed roll is self-stabilized with no control + // command on any model in this family + gimbal.set_demanded_rates(Vector3f( + vehicle_rate_gimbal.x, + vehicle_rate_gimbal.y + pitch_rate, + vehicle_rate_gimbal.z + yaw_rate)); + + gimbal.update(aircraft); + update_input(); + + // send attitude at 10 Hz + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - _last_attitude_ms >= 100) { + _last_attitude_ms = now_ms; + send_attitude(); + } +} + +void SkyDroid::send_attitude() +{ + // Report actual GimbalSim joint angles. + // joint_angles.y = pitch (negative = down), .z = azimuth. + // Wire encoding: pitch_cd = pitch_deg * 100, yaw_cd = yaw_deg * 100 (no sign flip) + Vector3f ja; + gimbal.get_joint_angles(ja); + const int16_t yaw_cd = (int16_t)(degrees(ja.z) * 100.0f); + const int16_t pitch_cd = (int16_t)(degrees(ja.y) * 100.0f); + const int16_t roll_cd = (int16_t)(degrees(ja.x) * 100.0f); + + uint8_t data[12]; + uint16_to_hex4((uint16_t)yaw_cd, &data[0]); + uint16_to_hex4((uint16_t)pitch_cd, &data[4]); + uint16_to_hex4((uint16_t)roll_cd, &data[8]); + + // attitude data is always sent with identifier "GAC", distinct from the + // "GAA" enable/rate-request command the driver sends to ask for it + send_packet('G', "GAC", false, data, sizeof(data)); +} + +/* + read bytes from autopilot into _buf, then scan for complete packets. + Packet format: + [0] '#' + [1] 'T' or 't' + [2] 'P' or 'p' + [3] 'U' (addr1, always UDP/external-control for SkyDroid) + [4] addr2 ('G','D','M') + [5] data_len as a single ASCII hex char + [6] 'r' or 'w' + [7..9] 3-char command ID + [10..10+data_len-1] data bytes + [10+data_len..11+data_len] 2-byte CRC + Total packet length = 12 + data_len +*/ +void SkyDroid::move_preamble_in_buffer(uint8_t search_start_pos) +{ + uint8_t i; + for (i = search_start_pos; i < _buflen; i++) { + if (_buf[i] == '#') { + break; + } + } + if (i == 0) { + return; + } + memmove(_buf, &_buf[i], _buflen - i); + _buflen -= i; +} + +void SkyDroid::update_input() +{ + const ssize_t n = read_from_autopilot((char*)&_buf[_buflen], ARRAY_SIZE(_buf) - _buflen - 1); + if (n < 0) { + if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) { + AP_HAL::panic("Failed to read from autopilot"); + } + return; + } + _buflen += n; + + while (_buflen >= 3) { + // search for '#' at the start + if (_buf[0] != '#') { + move_preamble_in_buffer(1); + continue; + } + if (_buf[1] != 'T' && _buf[1] != 't') { + move_preamble_in_buffer(1); + continue; + } + if (_buf[2] != 'P' && _buf[2] != 'p') { + move_preamble_in_buffer(1); + continue; + } + + // need at least 6 bytes to read the data_len field + if (_buflen < 6) { + break; + } + + // parse data length from ASCII hex char at [5] + uint8_t data_len; + if (!hex_char_to_nibble(_buf[5], data_len)) { + // invalid data length — discard '#' + move_preamble_in_buffer(1); + continue; + } + + const uint8_t pkt_len = 12 + data_len; + if (pkt_len > PACKETLEN_MAX) { + move_preamble_in_buffer(1); + continue; + } + + // wait for the full packet + if (_buflen < pkt_len) { + break; + } + + // verify and dispatch the packet + handle_packet(data_len); + move_preamble_in_buffer(pkt_len); + } +} + +void SkyDroid::handle_packet(uint8_t data_len) +{ + // verify CRC + const uint8_t pkt_len = 12 + data_len; + const uint8_t crc = crc_sum_of_bytes(_buf, pkt_len - 2); + const uint8_t expected_hi = hex2char((crc >> 4) & 0x0f); + const uint8_t expected_lo = hex2char(crc & 0x0f); + if (_buf[pkt_len - 2] != expected_hi || _buf[pkt_len - 1] != expected_lo) { + return; + } + + // ID is at bytes [7..9] + const char *id = (const char*)&_buf[7]; + + if (strncmp(id, "GAA", 3) == 0) { + // attitude streaming enable/rate request + send_attitude(); + + } else if (strncmp(id, "VER", 3) == 0) { + // version response begins with a literal 'V' (see AP_Mount_SkyDroid::gimbal_version_analyse) + const uint8_t data[] { 'V', '1', '.', '0', '.', '0' }; + send_packet('D', "VER", false, data, sizeof(data)); + + } else if (strncmp(id, "SDC", 3) == 0) { + // card present: 5 hex chars remaining, 5 hex chars total, not all zero + const uint8_t data[] { '0','0','0','1','0', '0','0','0','2','0' }; + send_packet('D', "SDC", false, data, sizeof(data)); + + } else if (strncmp(id, "MOD", 3) == 0) { + // model name response is raw ASCII text, e.g. "C13" + send_packet('D', "MOD", false, (const uint8_t*)_model_name, strlen(_model_name)); + + } else if (strncmp(id, "GSY", 3) == 0 && data_len >= 2) { + // individual-axis yaw speed command - confirmed on real hardware to be the + // only thing that actually moves yaw: signed 8bit hex value, LSB units + // calibrated in update() above. A real speed command supersedes any + // in-progress centering - see _centering's comment + _centering = false; + uint32_t tmp; + if (hex_chars_to_uint32((const char*)&_buf[10], 2, tmp)) { + _commanded_yaw_speed_lsb = (int8_t)tmp; + } + + } else if (strncmp(id, "GSP", 3) == 0 && data_len >= 2) { + // individual-axis pitch speed command, same as GSY above + _centering = false; + uint32_t tmp; + if (hex_chars_to_uint32((const char*)&_buf[10], 2, tmp)) { + _commanded_pitch_speed_lsb = (int8_t)tmp; + } + + } else if (strncmp(id, "PTZ", 3) == 0 && data_len >= 2) { + // discrete gimbal control. Only 0x05 ("center") is simulated - see + // _centering's comment and update()'s use of it. Follow/lock (0x06/0x07) + // and the jog codes (0x00-0x04) are not simulated: this driver only ever + // sends follow/lock (see AP_Mount_SkyDroid::set_gimbal_lock()), which has no + // observable effect on the simulated gimbal's motion either way + uint32_t tmp; + if (hex_chars_to_uint32((const char*)&_buf[10], 2, tmp) && tmp == 0x05) { + _centering = true; + } + } + // Everything else is absorbed silently, deliberately reproducing the real + // hardware's behaviour: the combined and absolute-angle commands (GAM, GSM, GAY, + // GAP) are confirmed silently ignored on real hardware, and roll (GAR, GSR) has + // no control command at all - SkyDroid have confirmed roll is self-stabilized. + // FAE, FAI, CAP, REC, DZM and TIM are also absorbed here +} + +void SkyDroid::send_packet(char addr2, const char id[3], bool write, const uint8_t *data, uint8_t len) +{ + const uint8_t total = 12 + len; + if (total > PACKETLEN_MAX) { + return; + } + + uint8_t pkt[PACKETLEN_MAX]; + uint8_t ofs = 0; + + pkt[ofs++] = '#'; + pkt[ofs++] = 'T'; + pkt[ofs++] = 'P'; + pkt[ofs++] = 'U'; // SkyDroid always replies to the 'U' (UDP/external-control) address + pkt[ofs++] = (uint8_t)addr2; + pkt[ofs++] = hex2char(len & 0x0f); // data length as single ASCII hex char + pkt[ofs++] = write ? 'w' : 'r'; + pkt[ofs++] = (uint8_t)id[0]; + pkt[ofs++] = (uint8_t)id[1]; + pkt[ofs++] = (uint8_t)id[2]; + + for (uint8_t i = 0; i < len; i++) { + pkt[ofs++] = data[i]; + } + + // checksum: byte sum of all preceding bytes, encoded as 2 uppercase ASCII hex chars + const uint8_t crc = crc_sum_of_bytes(pkt, ofs); + pkt[ofs++] = hex2char((crc >> 4) & 0x0f); + pkt[ofs++] = hex2char(crc & 0x0f); + + write_to_autopilot((const char*)pkt, ofs); +} + +void SkyDroid::uint16_to_hex4(uint16_t val, uint8_t buf[4]) +{ + buf[0] = hex2char((val >> 12) & 0x0f); + buf[1] = hex2char((val >> 8) & 0x0f); + buf[2] = hex2char((val >> 4) & 0x0f); + buf[3] = hex2char((val ) & 0x0f); +} + +#endif // AP_SIM_SKYDROID_ENABLED diff --git a/libraries/SITL/SIM_SkyDroid.h b/libraries/SITL/SIM_SkyDroid.h new file mode 100644 index 0000000000000..fc8727073a102 --- /dev/null +++ b/libraries/SITL/SIM_SkyDroid.h @@ -0,0 +1,116 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + Simulator for SkyDroid gimbal. This one class simulates every model in + SkyDroid's "TOP protocol" gimbal camera family: SkyDroid have confirmed the + gimbal-control commands are identical across models, so the ONLY thing the + simulated model name changes is the "MOD" response. Registered under two + device names (see SITL_State_common.cpp) purely so a test can check the + driver behaves identically regardless of which model answers. + + Roll is deliberately not controllable here, matching the real hardware: + SkyDroid have confirmed roll is self-stabilized with no control command on + any model, so GAR/GSR are absorbed and ignored like any other unimplemented + command. + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:skydroid --speedup=1 + +param set MNT1_TYPE 15 # skydroid +param set SERIAL5_PROTOCOL 8 # gimbal +reboot + +Use --serial5=sim:skydroid_c13 to have the gimbal report itself as a "C13" +instead of a "C11". Its control behaviour is identical. + +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_SKYDROID_ENABLED + +#include "SIM_Mount.h" +#include "SIM_Gimbal.h" + +namespace SITL { + +class SkyDroid : public Mount { +public: + + // model_name is returned verbatim in response to the "MOD" command (e.g. "C11", + // "C13"). It affects nothing else - control behaviour is model-independent + SkyDroid(const char *model_name) : + _model_name(model_name) {} + + void update(const Aircraft &aircraft) override; + +private: + + // the physical gimbal: + Gimbal gimbal; + + const char *_model_name; + + // input accumulation buffer; also used as working buffer by handle_packet() + static constexpr uint8_t PACKETLEN_MAX = 28; + uint8_t _buf[PACKETLEN_MAX]; + uint8_t _buflen; + + uint32_t _last_attitude_ms; // time of last attitude packet sent + + // last GSY/GSP speed values received (signed 8bit wire units). These + // individual-axis speed commands are the only ones that actually move the real + // gimbal (GAM/GSM/GAY/GAP are all confirmed silently ignored on real hardware), + // so they're the only ones simulated here + int8_t _commanded_yaw_speed_lsb; + int8_t _commanded_pitch_speed_lsb; + + // true from the moment a "PTZ" 0x05 ("center") command is received until the + // next GSY/GSP speed command arrives - see update()'s use of this to simulate + // the gimbal's own one-shot centering behaviour (AP_Mount_SkyDroid:: + // send_target_neutral()/send_target_retracted() send this instead of driving + // GSY/GSP themselves, so without this the simulated gimbal would never move + // for RETRACT/NEUTRAL and the autotest's neutral-position check would fail) + bool _centering; + + // read and dispatch incoming packets from autopilot + void update_input(); + + // scan forward from search_start_pos for '#' and move it to _buf[0] + void move_preamble_in_buffer(uint8_t search_start_pos); + + // send gimbal attitude packet to the driver + void send_attitude(); + + // dispatch a complete packet beginning at _buf[0], data_len data bytes + void handle_packet(uint8_t data_len); + + // build and send a response packet. SkyDroid's control address is always 'U' + // (UDP-only device, no separate UART/network interface ambiguity) + void send_packet(char addr2, const char id[3], bool write, const uint8_t *data, uint8_t len); + + // encode a uint16 as 4 uppercase ASCII hex chars + static void uint16_to_hex4(uint16_t val, uint8_t buf[4]); + + // convert a nibble (0-15) to an uppercase ASCII hex character + static uint8_t hex2char(uint8_t nibble) { + return nibble < 10 ? ('0' + nibble) : ('A' + nibble - 10); + } +}; + +} // namespace SITL + +#endif // AP_SIM_SKYDROID_ENABLED diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index f9f3c84435485..0e5bf261a4cd7 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -227,6 +227,10 @@ #define AP_SIM_VIEWPRO_ENABLED 1 #endif +#ifndef AP_SIM_SKYDROID_ENABLED +#define AP_SIM_SKYDROID_ENABLED 1 +#endif + #ifndef AP_SIM_AVT_CM62_ENABLED #define AP_SIM_AVT_CM62_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL && HAL_MAVLINK_BINDINGS_ENABLED) #endif @@ -243,7 +247,7 @@ // base class for all simulated gimbal backends: #ifndef AP_SIM_MOUNT_ENABLED -#define AP_SIM_MOUNT_ENABLED (AP_SIM_SIYI_ENABLED || AP_SIM_TOPOTEK_ENABLED || AP_SIM_VIEWPRO_ENABLED || AP_SIM_MAVLINKGIMBALV2_ENABLED) +#define AP_SIM_MOUNT_ENABLED (AP_SIM_SIYI_ENABLED || AP_SIM_TOPOTEK_ENABLED || AP_SIM_VIEWPRO_ENABLED || AP_SIM_MAVLINKGIMBALV2_ENABLED || AP_SIM_SKYDROID_ENABLED) #endif #ifndef AP_SIM_AIRSPEED_DLVR_ENABLED