From 70443011487705a1e56dda5fde5b22c6c8054857 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 8 Feb 2023 13:15:33 +1300 Subject: [PATCH] mavlink: use command to set SiK ID This removes the param MAV_SIK_RADIO_ID and replaces it with a MAVLink command to set the radio's net ID. Using a command has the benefit that we get feedback whether the change has been accepted. The code now also reads back the bytes after doing the config in order to check for the radio's OK feedback. Previously, the commands were just sent blindly and there is the chance they did not actually get through. Signed-off-by: Julian Oes --- msg/VehicleCommand.msg | 1 + src/modules/mavlink/mavlink_main.cpp | 220 ++++++++++++++--------- src/modules/mavlink/mavlink_main.h | 10 +- src/modules/mavlink/mavlink_params.c | 14 -- src/modules/mavlink/mavlink_receiver.cpp | 66 +++++++ src/modules/mavlink/mavlink_receiver.h | 14 ++ 6 files changed, 217 insertions(+), 108 deletions(-) diff --git a/msg/VehicleCommand.msg b/msg/VehicleCommand.msg index 13ac224582..b41c109c31 100644 --- a/msg/VehicleCommand.msg +++ b/msg/VehicleCommand.msg @@ -84,6 +84,7 @@ uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512 # Request to send a single instanc uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530 # Set camera capture mode (photo, video, etc.) uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531 # Set camera zoom uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532 +uint16 VEHICLE_CMD_SET_AT_PARAM = 550 # Set AT command for SiK radio uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 # Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 # Gimbal configuration to set which sysid/compid is in primary and secondary control uint16 VEHICLE_CMD_IMAGE_START_CAPTURE = 2000 # Start image capture sequence. diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index bfff5180a1..e89f857a8b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -2299,8 +2299,6 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_update_parameters(); } - configure_sik_radio(); - if (_vehicle_status_sub.updated()) { vehicle_status_s vehicle_status; @@ -2329,47 +2327,77 @@ Mavlink::task_main(int argc, char *argv[]) } - // MAVLINK_MODE_IRIDIUM: handle VEHICLE_CMD_CONTROL_HIGH_LATENCY - if (_mode == MAVLINK_MODE_IRIDIUM) { - int vehicle_command_updates = 0; + int vehicle_command_updates = 0; - while (_vehicle_command_sub.updated() && (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) { - vehicle_command_updates++; - const unsigned last_generation = _vehicle_command_sub.get_last_generation(); - vehicle_command_s vehicle_cmd; + while (_vehicle_command_sub.updated() && (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) { + vehicle_command_updates++; + const unsigned last_generation = _vehicle_command_sub.get_last_generation(); + vehicle_command_s vehicle_cmd; - if (_vehicle_command_sub.update(&vehicle_cmd)) { - if (_vehicle_command_sub.get_last_generation() != last_generation + 1) { - PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation()); - } + if (_vehicle_command_sub.update(&vehicle_cmd)) { + if (_vehicle_command_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation()); + } - if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) && - _mode == MAVLINK_MODE_IRIDIUM) { + if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) && + _mode == MAVLINK_MODE_IRIDIUM) { - if (vehicle_cmd.param1 > 0.5f) { - if (!_transmitting_enabled) { - mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command\t", - _device_name); - events::send(events::ID("mavlink_iridium_enable_cmd"), events::Log::Info, - "Enabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id); - } - - _transmitting_enabled = true; - _transmitting_enabled_commanded = true; - - } else { - if (_transmitting_enabled) { - mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command\t", - _device_name); - events::send(events::ID("mavlink_iridium_disable_cmd"), events::Log::Info, - "Disabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id); - } - - _transmitting_enabled = false; - _transmitting_enabled_commanded = false; + if (vehicle_cmd.param1 > 0.5f) { + if (!_transmitting_enabled) { + mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command\t", + _device_name); + events::send(events::ID("mavlink_iridium_enable_cmd"), events::Log::Info, + "Enabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id); } - // send positive command ack + _transmitting_enabled = true; + _transmitting_enabled_commanded = true; + + } else { + if (_transmitting_enabled) { + mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command\t", + _device_name); + events::send(events::ID("mavlink_iridium_disable_cmd"), events::Log::Info, + "Disabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id); + } + + _transmitting_enabled = false; + _transmitting_enabled_commanded = false; + } + + // send positive command ack + vehicle_command_ack_s command_ack{}; + command_ack.command = vehicle_cmd.command; + command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + command_ack.from_external = !vehicle_cmd.from_external; + command_ack.target_system = vehicle_cmd.source_system; + command_ack.target_component = vehicle_cmd.source_component; + command_ack.timestamp = vehicle_cmd.timestamp; + _vehicle_command_ack_pub.publish(command_ack); + + } else if (vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_SET_AT_PARAM) { + + + // We only support ATS3 to set NET_ID and setting it for all radios. + if ((int)(vehicle_cmd.param2 + 0.5f) != 3 && (int)(vehicle_cmd.param1 + 0.5f) == 0) { + vehicle_command_ack_s command_ack{}; + command_ack.command = vehicle_cmd.command; + command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + command_ack.from_external = !vehicle_cmd.from_external; + command_ack.target_system = vehicle_cmd.source_system; + command_ack.target_component = vehicle_cmd.source_component; + command_ack.timestamp = vehicle_cmd.timestamp; + _vehicle_command_ack_pub.publish(command_ack); + + } else if (!_radio_status_available) { + // Only respond if we have an SiK radio detected, otherwise just stay silent. + // If we nacked here with DENIED or UNSUPPORTED we would cause multiple + // acks/nacks to be sent back which would be confusing. + + } else { + // Since this command might take several seconds, we need to send an + // IN_PROGRESS ack immediately, and the final result later. + vehicle_command_ack_s command_ack{}; command_ack.command = vehicle_cmd.command; command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -2378,6 +2406,10 @@ Mavlink::task_main(int argc, char *argv[]) command_ack.target_component = vehicle_cmd.source_component; command_ack.timestamp = vehicle_cmd.timestamp; _vehicle_command_ack_pub.publish(command_ack); + + command_ack.result = configure_sik_radio((uint16_t)(vehicle_cmd.param3 + 0.5f)); + command_ack.timestamp = vehicle_cmd.timestamp; + _vehicle_command_ack_pub.publish(command_ack); } } } @@ -2744,56 +2776,56 @@ void Mavlink::publish_telemetry_status() _tstatus_updated = false; } -void Mavlink::configure_sik_radio() +uint8_t Mavlink::configure_sik_radio(uint16_t netid) { - /* radio config check */ - if (_uart_fd >= 0 && _param_sik_radio_id.get() != 0) { - /* request to configure radio and radio is present */ - FILE *fs = fdopen(_uart_fd, "w"); - - if (fs) { - /* switch to AT command mode */ - px4_usleep(1200000); - fprintf(fs, "+++"); - fflush(fs); - px4_usleep(1200000); - - if (_param_sik_radio_id.get() > 0) { - /* set channel */ - fprintf(fs, "ATS3=%" PRIu32 "\r\n", _param_sik_radio_id.get()); - px4_usleep(200000); - - } else { - /* reset to factory defaults */ - fprintf(fs, "AT&F\r\n"); - px4_usleep(200000); - } - - /* write config */ - fprintf(fs, "AT&W\r\n"); - px4_usleep(200000); - - /* reboot */ - fprintf(fs, "ATZ\r\n"); - px4_usleep(200000); - - // XXX NuttX suffers from a bug where - // fclose() also closes the fd, not just - // the file stream. Since this is a one-time - // config thing, we leave the file struct - // allocated. -#ifndef __PX4_NUTTX - fclose(fs); -#endif - - } else { - PX4_WARN("open fd %d failed", _uart_fd); - } - - /* reset param and save */ - _param_sik_radio_id.set(0); - _param_sik_radio_id.commit_no_notification(); + if (_uart_fd < 0) { + return vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED; } + + LockGuard lg{_send_mutex}; + + // it doesn't seem to switch without this wait + px4_usleep(1200000); + // switch to AT command mode + const char at_command_mode[] = "+++"; + _receiver.wait_for_ok(); + // we don't write the 0 termination, otherwise it doesn't seem to work. + write(_uart_fd, at_command_mode, 3); + fsync(_uart_fd); + + if (!wait_for_ok(2000000)) { + return vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED; + } + + // set channel + char id_setting[20] {}; + snprintf(id_setting, sizeof(id_setting), "ATS3=%" PRIu16 "\r\n", netid); + _receiver.wait_for_ok(); + write(_uart_fd, id_setting, sizeof(id_setting)); + fsync(_uart_fd); + + if (!wait_for_ok(500000)) { + return vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED; + } + + // write config + const char write_config[] = "AT&W\r\n\0"; + _receiver.wait_for_ok(); + write(_uart_fd, write_config, sizeof(write_config)); + fsync(_uart_fd); + px4_usleep(200000); + + if (!wait_for_ok(200000)) { + return vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED; + } + + // reboot + const char reboot[] = "ATZ\r\n\0"; + _receiver.wait_for_ok(); + write(_uart_fd, reboot, sizeof(reboot)); + fsync(_uart_fd); + + return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } int Mavlink::start_helper(int argc, char *argv[]) @@ -3282,6 +3314,22 @@ Mavlink::set_boot_complete() } +bool Mavlink::wait_for_ok(unsigned timeout_us) +{ + hrt_abstime now = hrt_absolute_time(); + + while (hrt_elapsed_time(&now) < timeout_us) { + + if (_receiver.got_ok()) { + return true; + } + + px4_usleep(10000); + } + + return false; +} + static void usage() { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 85c0361de3..510f9590ea 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -668,7 +668,6 @@ private: (ParamInt) _param_mav_sys_id, (ParamInt) _param_mav_comp_id, (ParamInt) _param_mav_proto_ver, - (ParamInt) _param_sik_radio_id, (ParamInt) _param_mav_type, (ParamBool) _param_mav_usehilgps, (ParamBool) _param_mav_fwdextsp, @@ -729,13 +728,8 @@ private: void check_requested_subscriptions(); - /** - * Reconfigure a SiK radio if requested by MAV_SIK_RADIO_ID - * - * This convenience function allows to re-configure a connected - * SiK radio without removing it from the main system harness. - */ - void configure_sik_radio(); + uint8_t configure_sik_radio(uint16_t netid); + bool wait_for_ok(unsigned timeout_us); /** * Update rate mult so total bitrate will be equal to _datarate. diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c index 8a793f5e5f..37be2af012 100644 --- a/src/modules/mavlink/mavlink_params.c +++ b/src/modules/mavlink/mavlink_params.c @@ -58,20 +58,6 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 1); */ PARAM_DEFINE_INT32(MAV_PROTO_VER, 0); -/** - * MAVLink SiK Radio ID - * - * When non-zero the MAVLink app will attempt to configure the - * SiK radio to this ID and re-set the parameter to 0. If the value - * is negative it will reset the complete radio config to - * factory defaults. Only applies if this mavlink instance is going through a SiK radio - * - * @group MAVLink - * @min -1 - * @max 240 - */ -PARAM_DEFINE_INT32(MAV_SIK_RADIO_ID, 0); - /** * MAVLink airframe type * diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 047d84b49f..025418e907 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -3213,6 +3213,13 @@ MavlinkReceiver::run() /* non-blocking read. read may return negative values */ nread = ::read(fds[0].fd, buf, sizeof(buf)); + parse_for_ok(nread, (const char *)buf); + + if (waiting_for_ok()) { + // Don't bother with the rest at the moment, we only wait for an ok. + continue; + } + if (nread == -1 && errno == ENOTCONN) { // Not connected (can happen for USB) usleep(100000); } @@ -3579,3 +3586,62 @@ void MavlinkReceiver::stop() _should_exit.store(true); pthread_join(_thread, nullptr); } + +void MavlinkReceiver::parse_for_ok(int nread, const char *buf) +{ + if (nread < 1) { + return; + } + + for (int i = 0; i < nread; ++i) { + switch (_parse_state) { + case OkParseState::None: + // Nothing to do. + break; + + case OkParseState::Waiting: + if (buf[i] == 'O') { + _parse_state = OkParseState::GotO; + } + + break; + + case OkParseState::GotO: + if (buf[i] == 'K') { + _parse_state = OkParseState::GotK; + + } else { + _parse_state = OkParseState::None; + } + + break; + + case OkParseState::GotK: + break; + + default: + break; + + + }; + } +} + +void MavlinkReceiver::wait_for_ok() +{ + _parse_state = OkParseState::Waiting; +} + +bool MavlinkReceiver::got_ok() +{ + if (_parse_state == OkParseState::GotK) { + _parse_state = OkParseState::None; + return true; + } + + return false; +} +bool MavlinkReceiver::waiting_for_ok() const +{ + return _parse_state == OkParseState::Waiting; +} diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index f239b2b4ef..d63c57f80f 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -138,6 +138,10 @@ public: void request_stop() { _should_exit.store(true); } + void wait_for_ok(); + bool got_ok(); + bool waiting_for_ok() const; + private: static void *start_trampoline(void *context); void run(); @@ -243,6 +247,8 @@ private: */ void updateParams() override; + void parse_for_ok(int nread, const char *buf); + Mavlink *_mavlink; MavlinkFTP _mavlink_ftp; @@ -396,6 +402,14 @@ private: hrt_abstime _heartbeat_component_udp_bridge{0}; hrt_abstime _heartbeat_component_uart_bridge{0}; + enum class OkParseState { + None, + Waiting, + GotO, + GotK, + } _parse_state = OkParseState::None; + + DEFINE_PARAMETERS( (ParamFloat) _param_bat_crit_thr, (ParamFloat) _param_bat_emergen_thr,