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,