From 7cfec996a2fddf6533cb40d349421e860fa0a44d Mon Sep 17 00:00:00 2001 From: olliw42 Date: Thu, 10 Aug 2023 10:05:18 +0200 Subject: [PATCH 001/125] cc1 --- ArduCopter/GCS_Mavlink.cpp | 9 + ArduCopter/version.h | 6 + libraries/GCS_MAVLink/GCS.h | 17 ++ libraries/GCS_MAVLink/GCS_Common.cpp | 177 ++++++++++++++++++++ libraries/bp_version.h | 240 +++++++++++++++++++++++++++ 5 files changed, 449 insertions(+) create mode 100644 libraries/bp_version.h diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 2ed967c5196cf..f83dab79cd675 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1402,6 +1402,15 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) handle_radio_status(msg, copter.should_log(MASK_LOG_PM)); break; } +//OW RADIOLINK + case MAVLINK_MSG_ID_RADIO_LINK_STATS: + handle_radio_link_stats(msg, copter.should_log(MASK_LOG_PM)); + break; + + case MAVLINK_MSG_ID_RADIO_LINK_FLOW_CONTROL: + handle_radio_link_flow_control(msg, copter.should_log(MASK_LOG_PM)); + break; +//OWEND case MAVLINK_MSG_ID_TERRAIN_DATA: case MAVLINK_MSG_ID_TERRAIN_CHECK: diff --git a/ArduCopter/version.h b/ArduCopter/version.h index aca789abdc19f..c671c4555dc04 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -8,6 +8,12 @@ #define THISFIRMWARE "ArduCopter V4.5.0-dev" +//OW +#undef THISFIRMWARE +#include "../libraries/bp_version.h" +#define THISFIRMWARE "BetaCopter V4.5.0-dev" BETAPILOTVERSION " 20230810" +//OWEND + // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 26e9371b03d7e..1fc4c5d3ece9f 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -477,8 +477,20 @@ class GCS_MAVLINK MAV_RESULT set_message_interval(uint32_t msg_id, int32_t interval_us); +//OW + static void send_to_all(uint32_t msgid, const char *pkt) { routing.send_to_all(msgid, pkt); } +//OWEND + protected: +//OW RADIOLINK + // called from vehicle class handler + void handle_radio_link_stats(const mavlink_message_t &msg, bool log_radio); + void handle_radio_link_flow_control(const mavlink_message_t &msg, bool log_radio); + // called from common handler + void handle_radio_rc_channels(const mavlink_message_t &msg); +//OWEND + bool mavlink_coordinate_frame_to_location_alt_frame(MAV_FRAME coordinate_frame, Location::AltFrame &frame); @@ -1244,6 +1256,11 @@ class GCS virtual uint8_t sysid_this_mav() const = 0; +//OW + uint8_t get_landed_state(void) const { return num_gcs() > 0 ? chan(0)->landed_state() : MAV_LANDED_STATE_UNDEFINED; } + uint8_t sysid_my_gcs() { return num_gcs() > 0 ? chan(0)->sysid_my_gcs() : 0; } +//OWEND + protected: virtual GCS_MAVLINK *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters ¶ms, diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index a63774f91253c..3a10a3feabf78 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1707,6 +1707,12 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status, // e.g. enforce-sysid says we shouldn't look at this packet return; } +//OW +#if HAL_MOUNT_ENABLED + AP_Mount *mount = AP::mount(); + if (mount != nullptr) mount->handle_msg(chan, msg); +#endif +//OWEND handleMessage(msg); } @@ -4114,6 +4120,14 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) } break; #endif + +//OW RADIOLINK +// like with old RADIO_STATUS, the messages +// MAVLINK_MSG_ID_RADIO_LINK_STATS, MAVLINK_MSG_ID_RADIO_LINK_FLOW_CONTROL are handled in the vehicle's GCS_Mavlink.cpp + case MAVLINK_MSG_ID_RADIO_RC_CHANNELS: + handle_radio_rc_channels(msg); + break; +//OWEND } } @@ -4222,6 +4236,13 @@ void GCS_MAVLINK::send_banner() } } #endif + +//OW +#if HAL_MOUNT_ENABLED + AP_Mount *mount = AP::mount(); + if (mount != nullptr) mount->send_banner(); +#endif +//OWEND } @@ -6344,6 +6365,16 @@ bool GCS_MAVLINK::accept_packet(const mavlink_status_t &status, return true; } +//OW RADIOLINK + // we want to handle messages from a radio + // we currently narrow it down to "ours" to play it safe + if ((msg.compid == MAV_COMP_ID_TELEMETRY_RADIO) && + (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS || + msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_FLOW_CONTROL || msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_STATS)) { + return true; + } +//OWEND + if (!sysid_enforce()) { return true; } @@ -6716,3 +6747,149 @@ MAV_RESULT GCS_MAVLINK::handle_control_high_latency(const mavlink_command_long_t return MAV_RESULT_ACCEPTED; } #endif // HAL_HIGH_LATENCY2_ENABLED + +//OW RADIOLINK +static mavlink_radio_t ow_mavlink_radio_packet; + +void GCS_MAVLINK::handle_radio_link_stats(const mavlink_message_t &msg, bool log_radio) +{ + mavlink_radio_link_stats_t packet; + mavlink_msg_radio_link_stats_decode(&msg, &packet); + + // let the AP_RCProtocol do it's thing + // is doing a thing if AP_RCProtocol::MAVLINK_RADIO is selected by the user + // will provide rssi if also AP_RSSI::RssiType::RECEIVER is selected by the user + AP::RC().handle_radio_link_stats(&packet); + + // handle the rssi info + // is doing a thing if AP_RSSI::RssiType::TELEMETRY_RADIO_RSSI is selected by the user + AP_RSSI* rssi = AP::rssi(); + if ((rssi != nullptr) && !rssi->enabled(AP_RSSI::RssiType::TELEMETRY_RADIO_RSSI)) return; + + // convert it to what we would get from RADIO_STATUS + uint8_t _rssi = packet.rx_rssi1; + uint8_t _remrssi = (packet.tx_rssi1 == UINT8_MAX) ? 0 : packet.tx_rssi1; + uint8_t _noise = 0; + uint8_t _remnoise = 0; + + if (packet.flags & RADIO_LINK_STATS_FLAGS_RSSI_DBM) { + // the rssi values are not in MAVLink standard, but negative dBm + // so we convert using ArduPilot's CRSF conversion, see AP_RCProtocol_CRSF::process_link_stats_frame() + if (_rssi < 50) { + _rssi = 254; + } else if (_rssi > 120) { + _rssi = 0; + } else { + _rssi = int16_t(roundf((1.0f - (_rssi - 50.0f) / 70.0f) * 254.0f)); + } + if (_remrssi < 50) { + _remrssi = 254; + } else if (_remrssi > 120) { + _remrssi = 0; + } else { + _remrssi = int16_t(roundf((1.0f - (_remrssi - 50.0f) / 70.0f) * 254.0f)); + } + } + + // now do what it does for RADIO_STATUS + const uint32_t now = AP_HAL::millis(); + + last_radio_status.received_ms = now; + last_radio_status.rssi = _rssi; + + // record if the GCS has been receiving radio messages from the vehicle + if (_remrssi != 0) { + last_radio_status.remrssi_ms = now; + } + + // we fake it for logging + ow_mavlink_radio_packet.rssi = _rssi; + ow_mavlink_radio_packet.remrssi = _remrssi; + ow_mavlink_radio_packet.noise = _noise; + ow_mavlink_radio_packet.remnoise = _remnoise; + + // log rssi, noise, etc if logging Performance monitoring data + if (log_radio) { + AP::logger().Write_Radio(ow_mavlink_radio_packet); + } +} + +void GCS_MAVLINK::handle_radio_link_flow_control(const mavlink_message_t &msg, bool log_radio) +{ + mavlink_radio_link_flow_control_t packet; + mavlink_msg_radio_link_flow_control_decode(&msg, &packet); + + if (packet.txbuf == UINT8_MAX) return; + + // convert it to what we would get from RADIO_STATUS + uint8_t _txbuf = packet.txbuf; + + // now what it would do for RADIO_STATUS + last_txbuf = _txbuf; + + // use the state of the transmit buffer in the radio to + // control the stream rate, giving us adaptive software + // flow control + if (_txbuf < 20 && stream_slowdown_ms < 2000) { + // we are very low on space - slow down a lot + stream_slowdown_ms += 60; + } else if (_txbuf < 50 && stream_slowdown_ms < 2000) { + // we are a bit low on space, slow down slightly + stream_slowdown_ms += 20; + } else if (_txbuf > 95 && stream_slowdown_ms > 200) { + // the buffer has plenty of space, speed up a lot + stream_slowdown_ms -= 40; + } else if (_txbuf > 90 && stream_slowdown_ms != 0) { + // the buffer has enough space, speed up a bit + stream_slowdown_ms -= 20; + } + +#if GCS_DEBUG_SEND_MESSAGE_TIMINGS + if (stream_slowdown_ms > max_slowdown_ms) { + max_slowdown_ms = stream_slowdown_ms; + } +#endif + + // we fake it for logging + ow_mavlink_radio_packet.txbuf = _txbuf; + + // log rssi, noise, etc if logging Performance monitoring data + if (log_radio) { + AP::logger().Write_Radio(ow_mavlink_radio_packet); + } +} + +void GCS_MAVLINK::handle_radio_rc_channels(const mavlink_message_t &msg) +{ + mavlink_radio_rc_channels_t packet; + mavlink_msg_radio_rc_channels_decode(&msg, &packet); + + AP::RC().handle_radio_rc_channels(&packet); +} +//OWEND +//OW FRPT +// this is tentative, just demo +// we probably want some timing, some packets do not need to be send so often +// maybe we also want to make which packets are send dependent on which streams are enabled +// or vice versa, modify the streams depending on whether frsky passthorugh is send +// one also could make it dependent on which rate is higher + +void GCS_MAVLINK::send_frsky_passthrough_array() +{ + AP_Frsky_SPort_Protocol* pt = AP::frsky_sport_protocol(); + if (pt == nullptr) return; + + uint8_t count = 0; + uint8_t packet_buf[MAVLINK_MSG_FRSKY_PASSTHROUGH_ARRAY_FIELD_PACKET_BUF_LEN] = {0}; // max 40 packets! + + pt->assemble_array(packet_buf, &count, 21, AP_HAL::millis()); + + if (count == 0) return; // nothing to send + + mavlink_msg_frsky_passthrough_array_send( + chan, + AP_HAL::millis(), // time since system boot + count, + packet_buf); +} +//OWEND diff --git a/libraries/bp_version.h b/libraries/bp_version.h new file mode 100644 index 0000000000000..d77984b8a8b32 --- /dev/null +++ b/libraries/bp_version.h @@ -0,0 +1,240 @@ +#pragma once + +#define BETAPILOTVERSION "v058" + +/* +search for //OW to find all changes + +2023.03.10: v058 + upgraded to Copter4.3.6 stable +2023.03.10: v057.3 + upgraded to Copter4.3.5-rc1 +2023.02.26: v057.2 + make it compile for MatekF405-CAN + upgraded to Copter4.3.4-rc1 +2023.02.11: v057.1 + upgraded to Copter4.3.4-rc1, has get_rate_ef_targets(), does not yet have corrected 1/3 param download +2023.01.20: v056 + upgraded to Copter4.3.3 stable +2023.01.16: v055 + aux functions + AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_EXT + revise handling of RADIO_LINL_STATS a bit, add some comments + do not send rc channels if RADIO_RC_CHANNELS is detected + put mLRS/RADLIO_LINK_xxx messages under the hood of storm32.xml + upgraded to Copter4.3.2 + replace COMPONENT_PREARM_STATUS by EVENT msg + upgraded to Copter4.3.1 + migrate to gimbal device v2 + undo mavtype_is_on_channel() + undo mavftp per serial port option + upgraded to Copter4.3.1-rc1 + no mavftp per serial port option, (1U<<13) = 8192 + upgraded to Copter4.3.0 + frsky_passthrough_array support, new param SRx_FRPT added to set stream rate + radio_rc_channels, radio_link_flow_control, radio_link_stats support added + upgraded to Copter4.3.0-beta1 +2022.09.19: v054 + upgraded to Copter4.2.3 + upgraded to Copter4.2.1 + upgraded to Copter4.2.1-rc1 + upgraded to Copter4.2.0 + upgraded to Plane4.2.1 + Ardu is unfortunately still on storm32.xml v 4. Feb. 2021 +2021.10.08: v052 + upgraded to Copter4.1.0 + upgraded to storm32.xml v 6. Okt. 2021 + more zflags, logging + reverted correction to CMD_DO_MOUNT_CONTROL bug, it's not going to change and it's acknowledged +2021.04.05: + upgraded to Copter4.0.7 +2021.02.05: + upgraded to Copter4.0.6 + upgraded to storm32.xml v 4. Feb. 2021, 600xx ranges + dual mount support if enabled +2020.11.25: + upgraded to Copter4.0.5 + upgraded to storm32.xml v 25. Nov. 2020 + upgraded to storm32_lib.h v 17. Nov. 2020 + +ArduCopter specific +- GCS_Mavlink.cpp: (2 comments, no change) +- version.h: 1x + +ArduPlane specific +- ArduPlane.cpp: 1x +- version.h: 1x + +Libraries: +- AP_Mount_Backend.cpp: 4x (+3 comments) +- AP_Mount_Backend.h: 1x (+1 comment) +- AP_Mount.cpp: 7x (+1 comment) +- AP_Mount.h: 6x +- GCS_Common.cpp: 2x +- GCS.h: 2x + +Additional Files in library: +- bp_version.h +- AP_Mount/BP_Mount_STorM32_MAVLink.cpp +- AP_Mount/BP_Mount_STorM32_MAVLink.h +- AP_Mount/STorM32_lib.h + + +Effect of SYSID_MYGCS parameter value: + +rc_channels_override (all) +manual_contol (copter) +these require sysid = SYSID_MYGCS + +accept_packet +only true if sysid = SYSID_MYGCS, if SYSID_ENFORCE = 1 (per default it is 0, so all sysid's are accepted) + +failsafe_gcs_check +reseted by heartbeat from SYSID_MYGCS + +send_rc_channels( + +------- + +SET_POSITION_TARGET_GLOBAL_INT Waiting for 3D fix +EKF_STATUS_REPORT + +MSG_LOCATION +MSG_POSITION_TARGET_GLOBAL_INT + +WPNAV_SPEED + +ACCEL_Z CAPACITY TYPE + +RC_CHANNELS_OVERRIDE +RC_CHANNELS +RC_CHANNELS_RAW +RADIO_STATUS +MANUAL_CONTROL + +SYSTEM_TIME + +STATUSTEXT + +ESTIMATOR_STATUS +EXTENDED_SYS_STATE + +MAV_CMD_DO_SET_ROI +MAV_CMD_DO_SET_SYSID +MAV_CMD_DO_MOUNT_CONTROL + +BUTTON_CHANGE MANUAL_CONTROL + +MAV_CMD_DO_SEND_BANNER +EXTENDED_SYS_STATE + +FOLLOW +copter.g2.follow.handle_msg(msg); +GCS_MAVLINK_Copter::handle_command_int_packet() -> MAV_CMD_DO_FOLLOW -> copter.g2.follow.set_target_sysid((uint8_t)packet.param1); +GCS_MAVLINK_Copter::handle_command_long_packet()-> MAV_CMD_DO_FOLLOW -> copter.g2.follow.set_target_sysid((uint8_t)packet.param1); + +GLOBAL_POSITION_INT +FOLLOW_TARGET + +DIGICAM_CONTROL +MOUNT_CONFIGURE +MOUNT_CONTROL +MAV_CMD_SET_CAMERA_MODE, MAV_CMD_SET_CAMERA_ZOOM, MAV_CMD_SET_CAMERA_FOCUS ?? +MAV_CMD_IMAGE_START_CAPTURE, MAV_CMD_IMAGE_STOP_CAPTURE, MAV_CMD_VIDEO_START_CAPTURE, MAV_CMD_VIDEO_STOP_CAPTURE + +WIND_COV HIGH_LATENCY2 WIND, GPS_RAW_INT VFR_HUD GPS2_RAW OPEN_DRONE_ID_LOCATION + +MAV_TYPE_ "GUID" +*/ + +/* +MissionPlanner +PayloadControl: sends MOUNT_CONTROL messages with target of the autopilot +AuxFunction: sends CMD_LONG with cmd MAV_CMD_DO_AUX_FUNCTION = 218 +TriggerCameraHere: sends CMD_LONG with cmd DO_DIGICAM_CONTROL = 203, param5 = 1, with target of the autopilot + +*/ + +/* aux handling +input: RC, script, mavlink +MAV_CMD_DO_AUX_FUNCTION = 218 for the Aux Buttons in MissionPlanner, MissionPlanner sends it with targets 1,1 +handle_command_do_aux_function(packet) +MAV_CMD_DO_AUX_FUNCTION_SWITCH_LEVEL is defined in xml but not used + + CAMERA_TRIGGER = 9, // trigger camera servo or relay + RETRACT_MOUNT1 = 27, // Retract Mount1 + MOUNT_LOCK = 163, // Mount yaw lock vs follow + CAMERA_REC_VIDEO = 166, // start recording on high, stop recording on low + CAMERA_ZOOM = 167, // camera zoom high = zoom in, middle = hold, low = zoom out + CAMERA_MANUAL_FOCUS = 168,// camera manual focus. high = long shot, middle = stop focus, low = close shot + CAMERA_AUTO_FOCUS = 169, // camera auto focus + + // inputs from 200 will eventually used to replace RCMAP + //OW: have no effect currently in RC_Channel::do_aux_function() + MOUNT1_ROLL = 212, // mount1 roll input + MOUNT1_PITCH = 213, // mount1 pitch input + MOUNT1_YAW = 214, // mount1 yaw input + MOUNT2_ROLL = 215, // mount2 roll input + MOUNT2_PITCH = 216, // mount3 pitch input + MOUNT2_YAW = 217, // mount4 yaw input + + // entries from 100-150 are expected to be developer options used for testing + CAM_MODE_TOGGLE = 102, // Momentary switch to cycle camera modes + + +AUX_FUNC::CAMERA_TRIGGER: +-> do_aux_function_camera_trigger(ch_flag); + -> if (ch_flag == AuxSwitchPos::HIGH) camera->take_picture(); + +AUX_FUNC::CAMERA_REC_VIDEO: +-> do_aux_function_record_video(ch_flag); + -> camera->record_video(ch_flag == AuxSwitchPos::HIGH); + +AUX_FUNC::CAMERA_ZOOM: +-> do_aux_function_camera_zoom(ch_flag); + -> camera->set_zoom_step(zoom_step); + +AUX_FUNC::CAMERA_MANUAL_FOCUS: +-> do_aux_function_camera_manual_focus(ch_flag); + -> camera->set_manual_focus_step(focus_step); + +AUX_FUNC::CAMERA_AUTO_FOCUS: +-> do_aux_function_camera_auto_focus(ch_flag); + -> if (ch_flag == AuxSwitchPos::HIGH) camera->set_auto_focus(); + +AUX_FUNC::CAM_MODE_TOGGLE: +-> case AuxSwitchPos::HIGH: camera->cam_mode_toggle(); + + +AUX_FUNC::RETRACT_MOUNT1: +-> case AuxSwitchPos::HIGH: mount->set_mode(0,MAV_MOUNT_MODE_RETRACT); +-> case AuxSwitchPos::LOW: mount->set_mode_to_default(0); + +AUX_FUNC::MOUNT_LOCK: +-> mount->set_yaw_lock(ch_flag == AuxSwitchPos::HIGH); + + +camera: + +AP_Camera::take_picture() +-> trigger_pic() + -> case CamTrigType::mount: mount->take_picture(0); +-> send to components: MAVLINK_MSG_ID_COMMAND_LONG:MAV_CMD_DO_DIGICAM_CONTROL: param5 = 1; + +AP_Camera::record_video(bool start_recording) +-> if (get_trigger_type() == CamTrigType::mount) mount->record_video(0, start_recording); + +AP_Camera::set_zoom_step(int8_t zoom_step) +-> if (get_trigger_type() == CamTrigType::mount) mount->set_zoom_step(0, zoom_step); + +AP_Camera::set_manual_focus_step(int8_t focus_step) +-> if (get_trigger_type() == CamTrigType::mount) mount->set_manual_focus_step(0, focus_step); + +AP_Camera::set_auto_focus() +-> if (get_trigger_type() == CamTrigType::mount) mount->set_auto_focus(0); + +AP_Camera::cam_mode_toggle() +-> does NOTHING !!! + + +*/ From 5f824ccf3a34e5e8388f0e1bd3e5a9adaca03dd3 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Thu, 10 Aug 2023 12:23:47 +0200 Subject: [PATCH 002/125] compiles --- bp_mavlink/ASLUAV.xml | 294 + bp_mavlink/AVSSUAS.xml | 198 + bp_mavlink/all.xml | 43 + bp_mavlink/ardupilotmega.xml | 1877 +++++ bp_mavlink/betapilot.xml | 7 + bp_mavlink/betapilot_missing_common.xml | 60 + bp_mavlink/common.xml | 6448 +++++++++++++++++ bp_mavlink/cubepilot.xml | 48 + bp_mavlink/development.xml | 42 + bp_mavlink/icarous.xml | 48 + bp_mavlink/loweheiser.xml | 55 + bp_mavlink/matrixpilot.xml | 349 + bp_mavlink/minimal.xml | 709 ++ bp_mavlink/olliw_dev.xml | 64 + bp_mavlink/paparazzi.xml | 38 + bp_mavlink/python_array_test.xml | 67 + bp_mavlink/standard.xml | 10 + bp_mavlink/storm32.xml | 433 ++ bp_mavlink/test.xml | 31 + bp_mavlink/uAvionix.xml | 210 + bp_mavlink/ualberta.xml | 76 + libraries/AP_Camera/AP_Camera.cpp | 32 + libraries/AP_Camera/AP_Camera.h | 5 + libraries/AP_Mount/AP_Mount.cpp | 50 + libraries/AP_Mount/AP_Mount.h | 24 + libraries/AP_Mount/AP_Mount_Backend.h | 32 +- libraries/AP_Mount/AP_Mount_Params.cpp | 4 + libraries/AP_Mount/AP_Mount_Params.h | 4 + .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 1534 ++++ libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 258 + libraries/AP_RCProtocol/AP_RCProtocol.cpp | 65 + libraries/AP_RCProtocol/AP_RCProtocol.h | 21 + .../AP_RCProtocol_MavlinkRadio.cpp | 49 + .../AP_RCProtocol_MavlinkRadio.h | 22 + libraries/AP_RSSI/AP_RSSI.h | 3 + libraries/GCS_MAVLink/GCS_Common.cpp | 38 +- libraries/GCS_MAVLink/GCS_MAVLink.h | 10 +- libraries/GCS_MAVLink/MAVLink_routing.cpp | 40 + libraries/GCS_MAVLink/MAVLink_routing.h | 4 + libraries/RC_Channel/RC_Channel.cpp | 17 + libraries/bp_version.h | 3 + wscript | 5 +- 42 files changed, 13290 insertions(+), 37 deletions(-) create mode 100644 bp_mavlink/ASLUAV.xml create mode 100644 bp_mavlink/AVSSUAS.xml create mode 100644 bp_mavlink/all.xml create mode 100644 bp_mavlink/ardupilotmega.xml create mode 100644 bp_mavlink/betapilot.xml create mode 100644 bp_mavlink/betapilot_missing_common.xml create mode 100644 bp_mavlink/common.xml create mode 100644 bp_mavlink/cubepilot.xml create mode 100644 bp_mavlink/development.xml create mode 100644 bp_mavlink/icarous.xml create mode 100644 bp_mavlink/loweheiser.xml create mode 100644 bp_mavlink/matrixpilot.xml create mode 100644 bp_mavlink/minimal.xml create mode 100644 bp_mavlink/olliw_dev.xml create mode 100644 bp_mavlink/paparazzi.xml create mode 100644 bp_mavlink/python_array_test.xml create mode 100644 bp_mavlink/standard.xml create mode 100644 bp_mavlink/storm32.xml create mode 100644 bp_mavlink/test.xml create mode 100644 bp_mavlink/uAvionix.xml create mode 100644 bp_mavlink/ualberta.xml create mode 100644 libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp create mode 100644 libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h diff --git a/bp_mavlink/ASLUAV.xml b/bp_mavlink/ASLUAV.xml new file mode 100644 index 0000000000000..4e80a5ac7827a --- /dev/null +++ b/bp_mavlink/ASLUAV.xml @@ -0,0 +1,294 @@ + + + + + common.xml + + + + + Mission command to reset Maximum Power Point Tracker (MPPT) + MPPT number + Empty + Empty + Empty + Empty + Empty + Empty + + + Mission command to perform a power cycle on payload + Complete power cycle + VISensor power cycle + Empty + Empty + Empty + Empty + Empty + + + + + no service + + + link type unknown + + + 2G (GSM/GRPS/EDGE) link + + + 3G link (WCDMA/HSDPA/HSPA) + + + 4G link (LTE) + + + + + not specified + + + HUAWEI LTE USB Stick E3372 + + + + + + Message encoding a command with parameters as scaled integers and additional metadata. Scaling depends on the actual command value. + UTC time, seconds elapsed since 01.01.1970 + Microseconds elapsed since vehicle boot + System ID + Component ID + The coordinate system of the COMMAND, as defined by MAV_FRAME enum + The scheduled action for the mission item, as defined by MAV_CMD enum + false:0, true:1 + autocontinue to next wp + PARAM1, see MAV_CMD enum + PARAM2, see MAV_CMD enum + PARAM3, see MAV_CMD enum + PARAM4, see MAV_CMD enum + PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + PARAM6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7 + PARAM7 / z position: global: altitude in meters (MSL, WGS84, AGL or relative to home - depending on frame). + + + Send a command with up to seven parameters to the MAV and additional metadata + UTC time, seconds elapsed since 01.01.1970 + Microseconds elapsed since vehicle boot + System which should execute the command + Component which should execute the command, 0 for all components + Command ID, as defined by MAV_CMD enum. + 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + Parameter 1, as defined by MAV_CMD enum. + Parameter 2, as defined by MAV_CMD enum. + Parameter 3, as defined by MAV_CMD enum. + Parameter 4, as defined by MAV_CMD enum. + Parameter 5, as defined by MAV_CMD enum. + Parameter 6, as defined by MAV_CMD enum. + Parameter 7, as defined by MAV_CMD enum. + + + Voltage and current sensor data + Power board voltage sensor reading + Power board current sensor reading + Board current sensor 1 reading + Board current sensor 2 reading + + + Maximum Power Point Tracker (MPPT) sensor data for solar module power performance tracking + MPPT last timestamp + MPPT1 voltage + MPPT1 current + MPPT1 pwm + MPPT1 status + MPPT2 voltage + MPPT2 current + MPPT2 pwm + MPPT2 status + MPPT3 voltage + MPPT3 current + MPPT3 pwm + MPPT3 status + + + ASL-fixed-wing controller data + Timestamp + ASLCTRL control-mode (manual, stabilized, auto, etc...) + See sourcecode for a description of these values... + + + Pitch angle + Pitch angle reference + + + + + + + Airspeed reference + + Yaw angle + Yaw angle reference + Roll angle + Roll angle reference + + + + + + + + + ASL-fixed-wing controller debug data + Debug data + Debug data + Debug data + Debug data + Debug data + Debug data + Debug data + Debug data + Debug data + Debug data + Debug data + + + Extended state information for ASLUAVs + Status of the position-indicator LEDs + Status of the IRIDIUM satellite communication system + Status vector for up to 8 servos + Motor RPM + + + + Extended EKF state estimates for ASLUAVs + Time since system start + Magnitude of wind velocity (in lateral inertial plane) + Wind heading angle from North + Z (Down) component of inertial wind velocity + Magnitude of air velocity + Sideslip angle + Angle of attack + + + Off-board controls/commands for ASLUAVs + Time since system start + Elevator command [~] + Throttle command [~] + Throttle 2 command [~] + Left aileron command [~] + Right aileron command [~] + Rudder command [~] + Off-board computer status + + + Atmospheric sensors (temperature, humidity, ...) + Time since system boot + Ambient temperature + Relative humidity + + + Battery pack monitoring data for Li-Ion batteries + Time since system start + Battery pack temperature + Battery pack voltage + Battery pack current + Battery pack state-of-charge + Battery monitor status report bits in Hex + Battery monitor serial number in Hex + Battery monitor safetystatus report bits in Hex + Battery monitor operation status report bits in Hex + Battery pack cell 1 voltage + Battery pack cell 2 voltage + Battery pack cell 3 voltage + Battery pack cell 4 voltage + Battery pack cell 5 voltage + Battery pack cell 6 voltage + + + Fixed-wing soaring (i.e. thermal seeking) data + Timestamp + Timestamp since last mode change + Thermal core updraft strength + Thermal radius + Thermal center latitude + Thermal center longitude + Variance W + Variance R + Variance Lat + Variance Lon + Suggested loiter radius + Suggested loiter direction + Distance to soar point + Expected sink rate at current airspeed, roll and throttle + Measurement / updraft speed at current/local airplane position + Measurement / roll angle tracking error + Expected measurement 1 + Expected measurement 2 + Thermal drift (from estimator prediction step only) + Thermal drift (from estimator prediction step only) + Total specific energy change (filtered) + Debug variable 1 + Debug variable 2 + Control Mode [-] + Data valid [-] + + + Monitoring of sensorpod status + Timestamp in linuxtime (since 1.1.1970) + Rate of ROS topic 1 + Rate of ROS topic 2 + Rate of ROS topic 3 + Rate of ROS topic 4 + Number of recording nodes + Temperature of sensorpod CPU in + Free space available in recordings directory in [Gb] * 1e2 + + + Monitoring of power board status + Timestamp + Power board status register + Power board leds status + Power board system voltage + Power board servo voltage + Power board digital voltage + Power board left motor current sensor + Power board right motor current sensor + Power board analog current sensor + Power board digital current sensor + Power board extension current sensor + Power board aux current sensor + + + Status of GSM modem (connected to onboard computer) + Timestamp (of OBC) + GSM modem used + GSM link type + RSSI as reported by modem (unconverted) + RSRP (LTE) or RSCP (WCDMA) as reported by modem (unconverted) + SINR (LTE) or ECIO (WCDMA) as reported by modem (unconverted) + RSRQ (LTE only) as reported by modem (unconverted) + + + + Status of the SatCom link + Timestamp + Timestamp of the last successful sbd session + Number of failed sessions + Number of successful sessions + Signal quality + Ring call pending + Transmission session pending + Receiving session pending + + + Calibrated airflow angle measurements + Timestamp + Angle of attack + Angle of attack measurement valid + Sideslip angle + Sideslip angle measurement valid + + + diff --git a/bp_mavlink/AVSSUAS.xml b/bp_mavlink/AVSSUAS.xml new file mode 100644 index 0000000000000..7138758a4c3be --- /dev/null +++ b/bp_mavlink/AVSSUAS.xml @@ -0,0 +1,198 @@ + + + + + + + + + common.xml + 2 + 1 + + + + + AVSS defined command. Set PRS arm statuses. + PRS arm statuses + User defined + User defined + User defined + User defined + User defined + User defined + + + AVSS defined command. Gets PRS arm statuses + User defined + User defined + User defined + User defined + User defined + User defined + User defined + + + AVSS defined command. Get the PRS battery voltage in millivolts + User defined + User defined + User defined + User defined + User defined + User defined + User defined + + + AVSS defined command. Get the PRS error statuses. + User defined + User defined + User defined + User defined + User defined + User defined + User defined + + + AVSS defined command. Set the ATS arming altitude in meters. + ATS arming altitude + User defined + User defined + User defined + User defined + User defined + User defined + + + AVSS defined command. Get the ATS arming altitude in meters. + User defined + User defined + User defined + User defined + User defined + User defined + User defined + + + AVSS defined command. Shuts down the PRS system. + User defined + User defined + User defined + User defined + User defined + User defined + User defined + + + + + AVSS defined command failure reason. PRS not steady. + + + AVSS defined command failure reason. PRS DTM not armed. + + + AVSS defined command failure reason. PRS OTM not armed. + + + + + In manual control mode + + + In attitude mode + + + In GPS mode + + + In hotpoint mode + + + In assisted takeoff mode + + + In auto takeoff mode + + + In auto landing mode + + + In go home mode + + + In sdk control mode + + + In sport mode + + + In force auto landing mode + + + In tripod mode + + + In search mode + + + In engine mode + + + + + In manual control mode + + + In auto takeoff mode + + + In auto landing mode + + + In go home mode + + + In drop mode + + + + + + AVSS PRS system status. + Timestamp (time since PRS boot). + PRS error statuses + Estimated battery run-time without a remote connection and PRS battery voltage + PRS arm statuses + PRS battery charge statuses + + + Drone position. + Timestamp (time since FC boot). + Latitude, expressed + Longitude, expressed + Altitude (MSL). Note that virtually all GPS modules provide both WGS84 and MSL. + Altitude above ground, This altitude is measured by a ultrasound, Laser rangefinder or millimeter-wave radar + This altitude is measured by a barometer + + + Drone IMU data. Quaternion order is w, x, y, z and a zero rotation would be expressed as (1 0 0 0). + Timestamp (time since FC boot). + Quaternion component 1, w (1 in null-rotation) + Quaternion component 2, x (0 in null-rotation) + Quaternion component 3, y (0 in null-rotation) + Quaternion component 4, z (0 in null-rotation) + X acceleration + Y acceleration + Z acceleration + Angular speed around X axis + Angular speed around Y axis + Angular speed around Z axis + + + Drone operation mode. + Timestamp (time since FC boot). + DJI M300 operation mode + horsefly operation mode + + + diff --git a/bp_mavlink/all.xml b/bp_mavlink/all.xml new file mode 100644 index 0000000000000..0dd38915a7caf --- /dev/null +++ b/bp_mavlink/all.xml @@ -0,0 +1,43 @@ + + + ardupilotmega.xml + + ASLUAV.xml + common.xml + development.xml + icarous.xml + + + minimal.xml + + + python_array_test.xml + standard.xml + test.xml + ualberta.xml + uAvionix.xml + + loweheiser.xml + + storm32.xml + + AVSSUAS.xml + + cubepilot.xml + + diff --git a/bp_mavlink/ardupilotmega.xml b/bp_mavlink/ardupilotmega.xml new file mode 100644 index 0000000000000..46fe2d721bbfb --- /dev/null +++ b/bp_mavlink/ardupilotmega.xml @@ -0,0 +1,1877 @@ + + + common.xml + + uAvionix.xml + icarous.xml + loweheiser.xml + cubepilot.xml + 2 + + + + + + + + + + + + + + + + + + + + + + + + + Set the distance to be repeated on mission resume + Distance. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + + + Control attached liquid sprayer + 0: disable sprayer. 1: enable sprayer. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + + + Pass instructions onto scripting, a script should be checking for a new command + uint16 ID value to be passed to scripting + float value to be passed to scripting + float value to be passed to scripting + float value to be passed to scripting + Empty. + Empty. + Empty. + + + Execute auxiliary function + Auxiliary Function. + Switch Level. + Empty. + Empty. + Empty. + Empty. + Empty. + + + Mission command to wait for an altitude or downwards vertical speed. This is meant for high altitude balloon launches, allowing the aircraft to be idle until either an altitude is reached or a negative vertical speed is reached (indicating early balloon burst). The wiggle time is how often to wiggle the control surfaces to prevent them seizing up. + Altitude. + Descent speed. + How long to wiggle the control surfaces to prevent them seizing up. + Empty. + Empty. + Empty. + Empty. + + + A system wide power-off event has been initiated. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + + + + FLY button has been clicked. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + + + FLY button has been held for 1.5 seconds. + Takeoff altitude. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + + + PAUSE button has been clicked. + 1 if Solo is in a shot mode, 0 otherwise. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + + + Magnetometer calibration based on fixed position + in earth field given by inclination, declination and intensity. + Magnetic declination. + Magnetic inclination. + Magnetic intensity. + Yaw. + Empty. + Empty. + Empty. + + + Magnetometer calibration based on fixed expected field values. + Field strength X. + Field strength Y. + Field strength Z. + Empty. + Empty. + Empty. + Empty. + + + + Set EKF sensor source set. + Source Set Id. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + + + Initiate a magnetometer calibration. + Bitmask of magnetometers to calibrate. Use 0 to calibrate all sensors that can be started (sensors may not start if disabled, unhealthy, etc.). The command will NACK if calibration does not start for a sensor explicitly specified by the bitmask. + Automatically retry on failure (0=no retry, 1=retry). + Save without user input (0=require input, 1=autosave). + Delay. + Autoreboot (0=user reboot, 1=autoreboot). + Empty. + Empty. + + + Accept a magnetometer calibration. + Bitmask of magnetometers that calibration is accepted (0 means all). + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + + + Cancel a running magnetometer calibration. + Bitmask of magnetometers to cancel a running calibration (0 means all). + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + + + Used when doing accelerometer calibration. When sent to the GCS tells it what position to put the vehicle in. When sent to the vehicle says what position the vehicle is in. + Position. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + + + Reply with the version banner. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + + + Command autopilot to get into factory test/diagnostic mode. + 0: activate test mode, 1: exit test mode. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + + + Causes the gimbal to reset and boot as if it was just powered on. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + + + Reports progress and success or failure of gimbal axis calibration procedure. + Gimbal axis we're reporting calibration progress for. + Current calibration progress for this axis. + Status of the calibration. + Empty. + Empty. + Empty. + Empty. + + + Starts commutation calibration on the gimbal. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + + + Erases gimbal application and parameters. + Magic number. + Magic number. + Magic number. + Magic number. + Magic number. + Magic number. + Magic number. + + + + Update the bootloader + Empty + Empty + Empty + Empty + Magic number - set to 290876 to actually flash + Empty + Empty + + + Reset battery capacity for batteries that accumulate consumed battery via integration. + Bitmask of batteries to reset. Least significant bit is for the first battery. + Battery percentage remaining to set. + + + Issue a trap signal to the autopilot process, presumably to enter the debugger. + Magic number - set to 32451 to actually trap. + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + + + Control onboard scripting. + Scripting command to execute + + + Scripting command as NAV command with wait for completion. + integer command number (0 to 255) + timeout for operation in seconds. Zero means no timeout (0 to 255) + argument1. + argument2. + argument3. + argument4. + Empty + + + Maintain an attitude for a specified time. + Time to maintain specified attitude and climb rate + Roll angle in degrees (positive is lean right, negative is lean left) + Pitch angle in degrees (positive is lean back, negative is lean forward) + Yaw angle + Climb rate + Empty + Empty + + + Change flight speed at a given rate. This slews the vehicle at a controllable rate between it's previous speed and the new one. (affects GUIDED only. Outside GUIDED, aircraft ignores these commands. Designed for onboard companion-computer command-and-control, not normally operator/GCS control.) + Airspeed or groundspeed. + Target Speed + Acceleration rate, 0 to take effect instantly + Empty + Empty + Empty + Empty + + + Change target altitude at a given rate. This slews the vehicle at a controllable rate between it's previous altitude and the new one. (affects GUIDED only. Outside GUIDED, aircraft ignores these commands. Designed for onboard companion-computer command-and-control, not normally operator/GCS control.) + Empty + Empty + Rate of change, toward new altitude. 0 for maximum rate change. Positive numbers only, as negative numbers will not converge on the new target alt. + Empty + Empty + Empty + Target Altitude + + + Change to target heading at a given rate, overriding previous heading/s. This slews the vehicle at a controllable rate between it's previous heading and the new one. (affects GUIDED only. Exiting GUIDED returns aircraft to normal behaviour defined elsewhere. Designed for onboard companion-computer command-and-control, not normally operator/GCS control.) + course-over-ground or raw vehicle heading. + Target heading. + Maximum centripetal accelearation, ie rate of change, toward new heading. + Empty + Empty + Empty + Empty + + + Provide an external position estimate for use when dead-reckoning. This is meant to be used for occasional position resets that may be provided by a external system such as a remote pilot using landmarks over a video link. + Timestamp that this message was sent as a time in the transmitters time domain. The sender should wrap this time back to zero based on required timing accuracy for the application and the limitations of a 32 bit float. For example, wrapping at 10 hours would give approximately 1ms accuracy. Recipient must handle time wrap in any timing jitter correction applied to this field. Wrap rollover time should not be at not more than 250 seconds, which would give approximately 10 microsecond accuracy. + The time spent in processing the sensor data that is the basis for this position. The recipient can use this to improve time alignment of the data. Set to zero if not known. + estimated one standard deviation accuracy of the measurement. Set to NaN if not known. + Empty + Latitude + Longitude + Altitude, not used. Should be sent as NaN. May be supported in a future version of this message. + + + + + Start a REPL session. + + + End a REPL session. + + + Stop execution of scripts. + + + Stop execution of scripts and restart. + + + + + Get an 8 byte session key which is used for remote secure updates which operate on flight controller data such as bootloader public keys. Return data will be 8 bytes on success. The session key remains valid until either the flight controller reboots or another SECURE_COMMAND_GET_SESSION_KEY is run. + + + Get an 8 byte session key which is used for remote secure updates which operate on RemoteID module data. Return data will be 8 bytes on success. The session key remains valid until either the remote ID module reboots or another SECURE_COMMAND_GET_REMOTEID_SESSION_KEY is run. + + + Remove range of public keys from the bootloader. Command data consists of two bytes, first byte if index of first public key to remove. Second byte is the number of keys to remove. If all keys are removed then secure boot is disabled and insecure firmware can be loaded. + + + Get current public keys from the bootloader. Command data consists of two bytes, first byte is index of first public key to fetch, 2nd byte is number of keys to fetch. Total data needs to fit in data portion of reply (max 6 keys for 32 byte keys). Reply data has the index of the first key in the first byte, followed by the keys. Returned keys may be less than the number of keys requested if there are less keys installed than requested. + + + Set current public keys in the bootloader. Data consists of a one byte public key index followed by the public keys. With 32 byte keys this allows for up to 6 keys to be set in one request. Keys outside of the range that is being set will remain unchanged. + + + Get config data for remote ID module. This command should be sent to the component ID of the flight controller which will forward it to the RemoteID module either over mavlink or DroneCAN. Data format is specific to the RemoteID implementation, see RemoteID firmware documentation for details. + + + Set config data for remote ID module. This command should be sent to the component ID of the flight controller which will forward it to the RemoteID module either over mavlink or DroneCAN. Data format is specific to the RemoteID implementation, see RemoteID firmware documentation for details. + + + Flash bootloader from local storage. Data is the filename to use for the bootloader. This is intended to be used with MAVFtp to upload a new bootloader to a microSD before flashing. + + + + + + Pre-initialization. + + + Disabled. + + + Checking limits. + + + A limit has been breached. + + + Taking action e.g. Return/RTL. + + + We're no longer in breach of a limit. + + + + + + Pre-initialization. + + + Disabled. + + + Checking limits. + + + + + Flags in RALLY_POINT message. + + Flag set when requiring favorable winds for landing. + + + Flag set when plane is to immediately descend to break altitude and land without GCS intervention. Flag not set when plane is to loiter at Rally point until commanded to land. + + + + + + Camera heartbeat, announce camera component ID at 1Hz. + + + Camera image triggered. + + + Camera connection lost. + + + Camera unknown error. + + + Camera battery low. Parameter p1 shows reported voltage. + + + Camera storage low. Parameter p1 shows reported shots remaining. + + + Camera storage low. Parameter p1 shows reported video minutes remaining. + + + + + + Shooting photos, not video. + + + Shooting video, not stills. + + + Unable to achieve requested exposure (e.g. shutter speed too low). + + + Closed loop feedback from camera, we know for sure it has successfully taken a picture. + + + Open loop camera, an image trigger has been requested but we can't know for sure it has successfully taken a picture. + + + + + + Gimbal is powered on but has not started initializing yet. + + + Gimbal is currently running calibration on the pitch axis. + + + Gimbal is currently running calibration on the roll axis. + + + Gimbal is currently running calibration on the yaw axis. + + + Gimbal has finished calibrating and initializing, but is relaxed pending reception of first rate command from copter. + + + Gimbal is actively stabilizing. + + + Gimbal is relaxed because it missed more than 10 expected rate command messages in a row. Gimbal will move back to active mode when it receives a new rate command. + + + + + Gimbal yaw axis. + + + Gimbal pitch axis. + + + Gimbal roll axis. + + + + + Axis calibration is in progress. + + + Axis calibration succeeded. + + + Axis calibration failed. + + + + + Whether or not this axis requires calibration is unknown at this time. + + + This axis requires calibration. + + + This axis does not require calibration. + + + + + + No GoPro connected. + + + The detected GoPro is not HeroBus compatible. + + + A HeroBus compatible GoPro is connected. + + + An unrecoverable error was encountered with the connected GoPro, it may require a power cycle. + + + + + + GoPro is currently recording. + + + + + The write message with ID indicated succeeded. + + + The write message with ID indicated failed. + + + + + (Get/Set). + + + (Get/Set). + + + (___/Set). + + + (Get/___). + + + (Get/___). + + + (Get/Set). + + + + (Get/Set). + + + (Get/Set). + + + (Get/Set). + + + (Get/Set). + + + (Get/Set) Hero 3+ Only. + + + (Get/Set) Hero 3+ Only. + + + (Get/Set) Hero 3+ Only. + + + (Get/Set) Hero 3+ Only. + + + (Get/Set) Hero 3+ Only. + + + (Get/Set). + + + + (Get/Set). + + + + + Video mode. + + + Photo mode. + + + Burst mode, Hero 3+ only. + + + Time lapse mode, Hero 3+ only. + + + Multi shot mode, Hero 4 only. + + + Playback mode, Hero 4 only, silver only except when LCD or HDMI is connected to black. + + + Playback mode, Hero 4 only. + + + Mode not yet known. + + + + + 848 x 480 (480p). + + + 1280 x 720 (720p). + + + 1280 x 960 (960p). + + + 1920 x 1080 (1080p). + + + 1920 x 1440 (1440p). + + + 2704 x 1440 (2.7k-17:9). + + + 2704 x 1524 (2.7k-16:9). + + + 2704 x 2028 (2.7k-4:3). + + + 3840 x 2160 (4k-16:9). + + + 4096 x 2160 (4k-17:9). + + + 1280 x 720 (720p-SuperView). + + + 1920 x 1080 (1080p-SuperView). + + + 2704 x 1520 (2.7k-SuperView). + + + 3840 x 2160 (4k-SuperView). + + + + + 12 FPS. + + + 15 FPS. + + + 24 FPS. + + + 25 FPS. + + + 30 FPS. + + + 48 FPS. + + + 50 FPS. + + + 60 FPS. + + + 80 FPS. + + + 90 FPS. + + + 100 FPS. + + + 120 FPS. + + + 240 FPS. + + + 12.5 FPS. + + + + + 0x00: Wide. + + + 0x01: Medium. + + + 0x02: Narrow. + + + + + 0=NTSC, 1=PAL. + + + + + 5MP Medium. + + + 7MP Medium. + + + 7MP Wide. + + + 10MP Wide. + + + 12MP Wide. + + + + + Auto. + + + 3000K. + + + 5500K. + + + 6500K. + + + Camera Raw. + + + + + Auto. + + + Neutral. + + + + + ISO 400. + + + ISO 800 (Only Hero 4). + + + ISO 1600. + + + ISO 3200 (Only Hero 4). + + + ISO 6400. + + + + + Low Sharpness. + + + Medium Sharpness. + + + High Sharpness. + + + + + -5.0 EV (Hero 3+ Only). + + + -4.5 EV (Hero 3+ Only). + + + -4.0 EV (Hero 3+ Only). + + + -3.5 EV (Hero 3+ Only). + + + -3.0 EV (Hero 3+ Only). + + + -2.5 EV (Hero 3+ Only). + + + -2.0 EV. + + + -1.5 EV. + + + -1.0 EV. + + + -0.5 EV. + + + 0.0 EV. + + + +0.5 EV. + + + +1.0 EV. + + + +1.5 EV. + + + +2.0 EV. + + + +2.5 EV (Hero 3+ Only). + + + +3.0 EV (Hero 3+ Only). + + + +3.5 EV (Hero 3+ Only). + + + +4.0 EV (Hero 3+ Only). + + + +4.5 EV (Hero 3+ Only). + + + +5.0 EV (Hero 3+ Only). + + + + + Charging disabled. + + + Charging enabled. + + + + + Unknown gopro model. + + + Hero 3+ Silver (HeroBus not supported by GoPro). + + + Hero 3+ Black. + + + Hero 4 Silver. + + + Hero 4 Black. + + + + + 3 Shots / 1 Second. + + + 5 Shots / 1 Second. + + + 10 Shots / 1 Second. + + + 10 Shots / 2 Second. + + + 10 Shots / 3 Second (Hero 4 Only). + + + 30 Shots / 1 Second. + + + 30 Shots / 2 Second. + + + 30 Shots / 3 Second. + + + 30 Shots / 6 Second. + + + + + Switch Low. + + + Switch Middle. + + + Switch High. + + + + + + LED patterns off (return control to regular vehicle control). + + + LEDs show pattern during firmware update. + + + Custom Pattern using custom bytes fields. + + + + + Flags in EKF_STATUS message. + + Set if EKF's attitude estimate is good. + + + Set if EKF's horizontal velocity estimate is good. + + + Set if EKF's vertical velocity estimate is good. + + + Set if EKF's horizontal position (relative) estimate is good. + + + Set if EKF's horizontal position (absolute) estimate is good. + + + Set if EKF's vertical position (absolute) estimate is good. + + + Set if EKF's vertical position (above ground) estimate is good. + + + EKF is in constant position mode and does not know it's absolute or relative position. + + + Set if EKF's predicted horizontal position (relative) estimate is good. + + + Set if EKF's predicted horizontal position (absolute) estimate is good. + + + Set if EKF has never been healthy. + + + + + + + + + + + + Special ACK block numbers control activation of dataflash log streaming. + + + + UAV to stop sending DataFlash blocks. + + + + UAV to start sending DataFlash blocks. + + + + + Possible remote log data block statuses. + + This block has NOT been received. + + + This block has been received. + + + + Bus types for device operations. + + I2C Device operation. + + + SPI Device operation. + + + + Deepstall flight stage. + + Flying to the landing point. + + + Building an estimate of the wind. + + + Waiting to breakout of the loiter to fly the approach. + + + Flying to the first arc point to turn around to the landing point. + + + Turning around back to the deepstall landing point. + + + Approaching the landing point. + + + Stalling and steering towards the land point. + + + + A mapping of plane flight modes for custom_mode field of heartbeat. + + + + + + + + + + + + + + + + + + + + + + + + + + + A mapping of copter flight modes for custom_mode field of heartbeat. + + + + + + + + + + + + + + + + + + + + + + + + + + + + A mapping of sub flight modes for custom_mode field of heartbeat. + + + + + + + + + + + + A mapping of rover flight modes for custom_mode field of heartbeat. + + + + + + + + + + + + + + + A mapping of antenna tracker flight modes for custom_mode field of heartbeat. + + + + + + + + + The type of parameter for the OSD parameter editor. + + + + + + + + + + + + The error type for the OSD parameter editor. + + + + + + + + + + Offsets and calibrations values for hardware sensors. This makes it easier to debug the calibration process. + Magnetometer X offset. + Magnetometer Y offset. + Magnetometer Z offset. + Magnetic declination. + Raw pressure from barometer. + Raw temperature from barometer. + Gyro X calibration. + Gyro Y calibration. + Gyro Z calibration. + Accel X calibration. + Accel Y calibration. + Accel Z calibration. + + + + Set the magnetometer offsets + System ID. + Component ID. + Magnetometer X offset. + Magnetometer Y offset. + Magnetometer Z offset. + + + State of autopilot RAM. + Heap top. + Free memory. + + Free memory (32 bit). + + + Raw ADC output. + ADC output 1. + ADC output 2. + ADC output 3. + ADC output 4. + ADC output 5. + ADC output 6. + + + + Configure on-board Camera Control System. + System ID. + Component ID. + Mode enumeration from 1 to N //P, TV, AV, M, etc. (0 means ignore). + Divisor number //e.g. 1000 means 1/1000 (0 means ignore). + F stop number x 10 //e.g. 28 means 2.8 (0 means ignore). + ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore). + Exposure type enumeration from 1 to N (0 means ignore). + Command Identity (incremental loop: 0 to 255). //A command sent multiple times will be executed or pooled just once. + Main engine cut-off time before camera trigger (0 means no cut-off). + Extra parameters enumeration (0 means ignore). + Correspondent value to given extra_param. + + + Control on-board Camera Control System to take shots. + System ID. + Component ID. + 0: stop, 1: start or keep it up //Session control e.g. show/hide lens. + 1 to N //Zoom's absolute position (0 means ignore). + -100 to 100 //Zooming step value to offset zoom from the current position. + 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus. + 0: ignore, 1: shot or start filming. + Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once. + Extra parameters enumeration (0 means ignore). + Correspondent value to given extra_param. + + + + Message to configure a camera mount, directional antenna, etc. + System ID. + Component ID. + Mount operating mode. + (1 = yes, 0 = no). + (1 = yes, 0 = no). + (1 = yes, 0 = no). + + + Message to control a camera mount, directional antenna, etc. + System ID. + Component ID. + Pitch (centi-degrees) or lat (degE7), depending on mount mode. + Roll (centi-degrees) or lon (degE7) depending on mount mode. + Yaw (centi-degrees) or alt (cm) depending on mount mode. + If "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING). + + + Message with some status from autopilot to GCS about camera or antenna mount. + System ID. + Component ID. + Pitch. + Roll. + Yaw. + + Mount operating mode. + + + + A fence point. Used to set a point when from GCS -> MAV. Also used to return a point from MAV -> GCS. + System ID. + Component ID. + Point index (first point is 1, 0 is for return point). + Total number of points (for sanity checking). + Latitude of point. + Longitude of point. + + + Request a current fence point from MAV. + System ID. + Component ID. + Point index (first point is 1, 0 is for return point). + + + Status of DCM attitude estimator. + X gyro drift estimate. + Y gyro drift estimate. + Z gyro drift estimate. + Average accel_weight. + Average renormalisation value. + Average error_roll_pitch value. + Average error_yaw value. + + + Status of simulation environment, if used. + Roll angle. + Pitch angle. + Yaw angle. + X acceleration. + Y acceleration. + Z acceleration. + Angular speed around X axis. + Angular speed around Y axis. + Angular speed around Z axis. + Latitude. + Longitude. + + + Status of key hardware. + Board voltage. + I2C error count. + + + Status generated by radio. + Local signal strength. + Remote signal strength. + How full the tx buffer is. + Background noise level. + Remote background noise level. + Receive errors. + Count of error corrected packets. + + + + Status of AP_Limits. Sent in extended status stream when AP_Limits is enabled. + State of AP_Limits. + Time (since boot) of last breach. + Time (since boot) of last recovery action. + Time (since boot) of last successful recovery. + Time (since boot) of last all-clear. + Number of fence breaches. + AP_Limit_Module bitfield of enabled modules. + AP_Limit_Module bitfield of required modules. + AP_Limit_Module bitfield of triggered modules. + + + Wind estimation. + Wind direction (that wind is coming from). + Wind speed in ground plane. + Vertical wind speed. + + + Data packet, size 16. + Data type. + Data length. + Raw data. + + + Data packet, size 32. + Data type. + Data length. + Raw data. + + + Data packet, size 64. + Data type. + Data length. + Raw data. + + + Data packet, size 96. + Data type. + Data length. + Raw data. + + + Rangefinder reporting. + Distance. + Raw voltage if available, zero otherwise. + + + Airspeed auto-calibration. + GPS velocity north. + GPS velocity east. + GPS velocity down. + Differential pressure. + Estimated to true airspeed ratio. + Airspeed ratio. + EKF state x. + EKF state y. + EKF state z. + EKF Pax. + EKF Pby. + EKF Pcz. + + + + A rally point. Used to set a point when from GCS -> MAV. Also used to return a point from MAV -> GCS. + System ID. + Component ID. + Point index (first point is 0). + Total number of points (for sanity checking). + Latitude of point. + Longitude of point. + Transit / loiter altitude relative to home. + + Break altitude relative to home. + Heading to aim for when landing. + Configuration flags. + + + Request a current rally point from MAV. MAV should respond with a RALLY_POINT message. MAV should not respond if the request is invalid. + System ID. + Component ID. + Point index (first point is 0). + + + Status of compassmot calibration. + Throttle. + Current. + Interference. + Motor Compensation X. + Motor Compensation Y. + Motor Compensation Z. + + + Status of secondary AHRS filter if available. + Roll angle. + Pitch angle. + Yaw angle. + Altitude (MSL). + Latitude. + Longitude. + + + + Camera Event. + Image timestamp (since UNIX epoch, according to camera clock). + System ID. + + Camera ID. + + Image index. + + Event type. + Parameter 1 (meaning depends on event_id, see CAMERA_STATUS_TYPES enum). + Parameter 2 (meaning depends on event_id, see CAMERA_STATUS_TYPES enum). + Parameter 3 (meaning depends on event_id, see CAMERA_STATUS_TYPES enum). + Parameter 4 (meaning depends on event_id, see CAMERA_STATUS_TYPES enum). + + + + Camera Capture Feedback. + Image timestamp (since UNIX epoch), as passed in by CAMERA_STATUS message (or autopilot if no CCB). + System ID. + + Camera ID. + + Image index. + + Latitude. + Longitude. + Altitude (MSL). + Altitude (Relative to HOME location). + Camera Roll angle (earth frame, +-180). + + Camera Pitch angle (earth frame, +-180). + + Camera Yaw (earth frame, 0-360, true). + + Focal Length. + + Feedback flags. + + + Completed image captures. + + + + 2nd Battery status + Voltage. + Battery current, -1: autopilot does not measure the current. + + + Status of third AHRS filter if available. This is for ANU research group (Ali and Sean). + Roll angle. + Pitch angle. + Yaw angle. + Altitude (MSL). + Latitude. + Longitude. + Test variable1. + Test variable2. + Test variable3. + Test variable4. + + + Request the autopilot version from the system/component. + System ID. + Component ID. + + + + Send a block of log data to remote location. + System ID. + Component ID. + Log data block sequence number. + Log data block. + + + Send Status of each log block that autopilot board might have sent. + System ID. + Component ID. + Log data block sequence number. + Log data block status. + + + Control vehicle LEDs. + System ID. + Component ID. + Instance (LED instance to control or 255 for all LEDs). + Pattern (see LED_PATTERN_ENUM). + Custom Byte Length. + Custom Bytes. + + + Reports progress of compass calibration. + Compass being calibrated. + Bitmask of compasses being calibrated. + Calibration Status. + Attempt number. + Completion percentage. + Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid). + Body frame direction vector for display. + Body frame direction vector for display. + Body frame direction vector for display. + + + + + EKF Status message including flags and variances. + Flags. + + Velocity variance. + + Horizontal Position variance. + Vertical Position variance. + Compass variance. + Terrain Altitude variance. + + Airspeed variance. + + + + PID tuning information. + Axis. + Desired rate. + Achieved rate. + FF component. + P component. + I component. + D component. + + Slew rate. + P/D oscillation modifier. + + + Deepstall path planning. + Landing latitude. + Landing longitude. + Final heading start point, latitude. + Final heading start point, longitude. + Arc entry point, latitude. + Arc entry point, longitude. + Altitude. + Distance the aircraft expects to travel during the deepstall. + Deepstall cross track error (only valid when in DEEPSTALL_STAGE_LAND). + Deepstall stage. + + + 3 axis gimbal measurements. + System ID. + Component ID. + Time since last update. + Delta angle X. + Delta angle Y. + Delta angle X. + Delta velocity X. + Delta velocity Y. + Delta velocity Z. + Joint ROLL. + Joint EL. + Joint AZ. + + + Control message for rate gimbal. + System ID. + Component ID. + Demanded angular rate X. + Demanded angular rate Y. + Demanded angular rate Z. + + + 100 Hz gimbal torque command telemetry. + System ID. + Component ID. + Roll Torque Command. + Elevation Torque Command. + Azimuth Torque Command. + + + + Heartbeat from a HeroBus attached GoPro. + Status. + Current capture mode. + Additional status bits. + + + + Request a GOPRO_COMMAND response from the GoPro. + System ID. + Component ID. + Command ID. + + + Response from a GOPRO_COMMAND get request. + Command ID. + Status. + Value. + + + Request to set a GOPRO_COMMAND with a desired. + System ID. + Component ID. + Command ID. + Value. + + + Response from a GOPRO_COMMAND set request. + Command ID. + Status. + + + + + RPM sensor output. + RPM Sensor1. + RPM Sensor2. + + + + Read registers for a device. + System ID. + Component ID. + Request ID - copied to reply. + The bus type. + Bus number. + Bus address. + Name of device on bus (for SPI). + First register to read. + Count of registers to read. + + Bank number. + + + Read registers reply. + Request ID - copied from request. + 0 for success, anything else is failure code. + Starting register. + Count of bytes read. + Reply data. + + Bank number. + + + Write registers for a device. + System ID. + Component ID. + Request ID - copied to reply. + The bus type. + Bus number. + Bus address. + Name of device on bus (for SPI). + First register to write. + Count of registers to write. + Write data. + + Bank number. + + + Write registers reply. + Request ID - copied from request. + 0 for success, anything else is failure code. + + + Send a secure command. Data should be signed with a private key corresponding with a public key known to the recipient. Signature should be over the concatenation of the sequence number (little-endian format), the operation (little-endian format) the data and the session key. For SECURE_COMMAND_GET_SESSION_KEY the session key should be zero length. The data array consists of the data followed by the signature. The sum of the data_length and the sig_length cannot be more than 220. The format of the data is command specific. + System ID. + Component ID. + Sequence ID for tagging reply. + Operation being requested. + Data length. + Signature length. + Signed data. + + + Reply from secure command. + Sequence ID from request. + Operation that was requested. + Result of command. + Data length. + Reply data. + + + + Adaptive Controller tuning information. + Axis. + Desired rate. + Achieved rate. + Error between model and vehicle. + Theta estimated state predictor. + Omega estimated state predictor. + Sigma estimated state predictor. + Theta derivative. + Omega derivative. + Sigma derivative. + Projection operator value. + Projection operator derivative. + u adaptive controlled output command. + + + + Camera vision based attitude and position deltas. + Timestamp (synced to UNIX time or since system boot). + Time since the last reported camera frame. + Defines a rotation vector [roll, pitch, yaw] to the current MAV_FRAME_BODY_FRD from the previous MAV_FRAME_BODY_FRD. + Change in position to the current MAV_FRAME_BODY_FRD from the previous FRAME_BODY_FRD rotated to the current MAV_FRAME_BODY_FRD. + Normalised confidence value from 0 to 100. + + + + Angle of Attack and Side Slip Angle. + Timestamp (since boot or Unix epoch). + Angle of Attack. + Side Slip Angle. + + + ESC Telemetry Data for ESCs 1 to 4, matching data sent by BLHeli ESCs. + Temperature. + Voltage. + Current. + Total current. + RPM (eRPM). + count of telemetry packets received (wraps at 65535). + + + ESC Telemetry Data for ESCs 5 to 8, matching data sent by BLHeli ESCs. + Temperature. + Voltage. + Current. + Total current. + RPM (eRPM). + count of telemetry packets received (wraps at 65535). + + + ESC Telemetry Data for ESCs 9 to 12, matching data sent by BLHeli ESCs. + Temperature. + Voltage. + Current. + Total current. + RPM (eRPM). + count of telemetry packets received (wraps at 65535). + + + Configure an OSD parameter slot. + System ID. + Component ID. + Request ID - copied to reply. + OSD parameter screen index. + OSD parameter display index. + Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Config type. + OSD parameter minimum value. + OSD parameter maximum value. + OSD parameter increment. + + + Configure OSD parameter reply. + Request ID - copied from request. + Config error type. + + + Read a configured an OSD parameter slot. + System ID. + Component ID. + Request ID - copied to reply. + OSD parameter screen index. + OSD parameter display index. + + + Read configured OSD parameter reply. + Request ID - copied from request. + Config error type. + Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Config type. + OSD parameter minimum value. + OSD parameter maximum value. + OSD parameter increment. + + + + Obstacle located as a 3D vector. + Timestamp (time since system boot). + Class id of the distance sensor type. + Coordinate frame of reference. + Unique ID given to each obstacle so that its movement can be tracked. Use UINT16_MAX if object ID is unknown or cannot be determined. + X position of the obstacle. + Y position of the obstacle. + Z position of the obstacle. + Minimum distance the sensor can measure. + Maximum distance the sensor can measure. + + + Water depth + Timestamp (time since system boot) + Onboard ID of the sensor + Sensor data healthy (0=unhealthy, 1=healthy) + Latitude + Longitude + Altitude (MSL) of vehicle + Roll angle + Pitch angle + Yaw angle + Distance (uncorrected) + Water temperature + + + The MCU status, giving MCU temperature and voltage. The min and max voltages are to allow for detecting power supply instability. + MCU instance + MCU Internal temperature + MCU voltage + MCU voltage minimum + MCU voltage maximum + + + ESC Telemetry Data for ESCs 13 to 16, matching data sent by BLHeli ESCs. + Temperature. + Voltage. + Current. + Total current. + RPM (eRPM). + count of telemetry packets received (wraps at 65535). + + + ESC Telemetry Data for ESCs 17 to 20, matching data sent by BLHeli ESCs. + Temperature. + Voltage. + Current. + Total current. + RPM (eRPM). + count of telemetry packets received (wraps at 65535). + + + ESC Telemetry Data for ESCs 21 to 24, matching data sent by BLHeli ESCs. + Temperature. + Voltage. + Current. + Total current. + RPM (eRPM). + count of telemetry packets received (wraps at 65535). + + + ESC Telemetry Data for ESCs 25 to 28, matching data sent by BLHeli ESCs. + Temperature. + Voltage. + Current. + Total current. + RPM (eRPM). + count of telemetry packets received (wraps at 65535). + + + ESC Telemetry Data for ESCs 29 to 32, matching data sent by BLHeli ESCs. + Temperature. + Voltage. + Current. + Total current. + RPM (eRPM). + count of telemetry packets received (wraps at 65535). + + + diff --git a/bp_mavlink/betapilot.xml b/bp_mavlink/betapilot.xml new file mode 100644 index 0000000000000..589f34ecada34 --- /dev/null +++ b/bp_mavlink/betapilot.xml @@ -0,0 +1,7 @@ + + + all.xml + betapilot_missing_common.xml + olliw_dev.xml + + diff --git a/bp_mavlink/betapilot_missing_common.xml b/bp_mavlink/betapilot_missing_common.xml new file mode 100644 index 0000000000000..130b9def4855b --- /dev/null +++ b/bp_mavlink/betapilot_missing_common.xml @@ -0,0 +1,60 @@ + + + 3 + + + + Reason for an event error response. + + The requested event is not available (anymore). + + + + Flags for CURRENT_EVENT_SEQUENCE. + + A sequence reset has happened (e.g. vehicle reboot). + + + + + + + + + Event message. Each new event from a particular component gets a new sequence number. The same message might be sent multiple times if (re-)requested. Most events are broadcast, some can be specific to a target component (as receivers keep track of the sequence for missed events, all events need to be broadcast. Thus we use destination_component instead of target_component). + Component ID + System ID + Event ID (as defined in the component metadata) + Timestamp (time since system boot when the event happened). + Sequence number. + Log levels: 4 bits MSB: internal (for logging purposes), 4 bits LSB: external. Levels: Emergency = 0, Alert = 1, Critical = 2, Error = 3, Warning = 4, Notice = 5, Info = 6, Debug = 7, Protocol = 8, Disabled = 9 + Arguments (depend on event ID). + + + + + Regular broadcast for the current latest event sequence number for a component. This is used to check for dropped events. + Sequence number. + Flag bitset. + + + + + Request one or more events to be (re-)sent. If first_sequence==last_sequence, only a single event is requested. Note that first_sequence can be larger than last_sequence (because the sequence number can wrap). Each sequence will trigger an EVENT or EVENT_ERROR response. + System ID + Component ID + First sequence number of the requested event. + Last sequence number of the requested event. + + + + + Response to a REQUEST_EVENT in case of an error (e.g. the event is not available anymore). + System ID + Component ID + Sequence number. + Oldest Sequence number that is still available after the sequence set in REQUEST_EVENT. + Error reason. + + + diff --git a/bp_mavlink/common.xml b/bp_mavlink/common.xml new file mode 100644 index 0000000000000..0d566d5d0e77e --- /dev/null +++ b/bp_mavlink/common.xml @@ -0,0 +1,6448 @@ + + + minimal.xml + 3 + 0 + + + These values define the type of firmware release. These values indicate the first version or release of this type. For example the first alpha release would be 64, the second would be 65. + + development release + + + alpha release + + + beta release + + + release candidate + + + official stable release + + + + Flags to report failure cases over the high latency telemtry. + + GPS failure. + + + Differential pressure sensor failure. + + + Absolute pressure sensor failure. + + + Accelerometer sensor failure. + + + Gyroscope sensor failure. + + + Magnetometer sensor failure. + + + Terrain subsystem failure. + + + Battery failure/critical low battery. + + + RC receiver failure/no rc connection. + + + Offboard link failure. + + + Engine failure. + + + Geofence violation. + + + Estimator failure, for example measurement rejection or large variances. + + + Mission failure. + + + + Actions that may be specified in MAV_CMD_OVERRIDE_GOTO to override mission execution. + + Hold at the current position. + + + Continue with the next item in mission execution. + + + Hold at the current position of the system + + + Hold at the position specified in the parameters of the DO_HOLD action + + + + These defines are predefined OR-combined mode flags. There is no need to use values from this enum, but it + simplifies the use of the mode flags. Note that manual input is enabled in all modes as a safety override. + + System is not ready to fly, booting, calibrating, etc. No flag is set. + + + System is allowed to be active, under assisted RC control. + + + System is allowed to be active, under assisted RC control. + + + System is allowed to be active, under manual (RC) control, no stabilization + + + System is allowed to be active, under manual (RC) control, no stabilization + + + System is allowed to be active, under autonomous control, manual setpoint + + + System is allowed to be active, under autonomous control, manual setpoint + + + System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by waypoints) + + + System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by waypoints) + + + UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. + + + UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. + + + + These encode the sensors whose status is sent as part of the SYS_STATUS message. + + 0x01 3D gyro + + + 0x02 3D accelerometer + + + 0x04 3D magnetometer + + + 0x08 absolute pressure + + + 0x10 differential pressure + + + 0x20 GPS + + + 0x40 optical flow + + + 0x80 computer vision position + + + 0x100 laser based position + + + 0x200 external ground truth (Vicon or Leica) + + + 0x400 3D angular rate control + + + 0x800 attitude stabilization + + + 0x1000 yaw position + + + 0x2000 z/altitude control + + + 0x4000 x/y position control + + + 0x8000 motor outputs / control + + + 0x10000 rc receiver + + + 0x20000 2nd 3D gyro + + + 0x40000 2nd 3D accelerometer + + + 0x80000 2nd 3D magnetometer + + + 0x100000 geofence + + + 0x200000 AHRS subsystem health + + + 0x400000 Terrain subsystem health + + + 0x800000 Motors are reversed + + + 0x1000000 Logging + + + 0x2000000 Battery + + + 0x4000000 Proximity + + + 0x8000000 Satellite Communication + + + 0x10000000 pre-arm check status. Always healthy when armed + + + 0x20000000 Avoidance/collision prevention + + + 0x40000000 propulsion (actuator, esc, motor or propellor) + + + + Co-ordinate frames used by MAVLink. Not all frames are supported by all commands, messages, or vehicles. + + Global frames use the following naming conventions: + - "GLOBAL": Global co-ordinate frame with WGS84 latitude/longitude and altitude positive over mean sea level (MSL) by default. + The following modifiers may be used with "GLOBAL": + - "RELATIVE_ALT": Altitude is relative to the vehicle home position rather than MSL. + - "TERRAIN_ALT": Altitude is relative to ground level rather than MSL. + - "INT": Latitude/longitude (in degrees) are scaled by multiplying by 1E7. + + Local frames use the following naming conventions: + - "LOCAL": Origin of local frame is fixed relative to earth. Unless otherwise specified this origin is the origin of the vehicle position-estimator ("EKF"). + - "BODY": Origin of local frame travels with the vehicle. NOTE, "BODY" does NOT indicate alignment of frame axis with vehicle attitude. + - "OFFSET": Deprecated synonym for "BODY" (origin travels with the vehicle). Not to be used for new frames. + + Some deprecated frames do not follow these conventions (e.g. MAV_FRAME_BODY_NED and MAV_FRAME_BODY_OFFSET_NED). + + + Global (WGS84) coordinate frame + MSL altitude. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL). + + + NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth. + + + NOT a coordinate frame, indicates a mission command. + + + Global (WGS84) coordinate frame + altitude relative to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. + + + ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth. + + + Global (WGS84) coordinate frame (scaled) + MSL altitude. First value / x: latitude in degrees*1E7, second value / y: longitude in degrees*1E7, third value / z: positive altitude over mean sea level (MSL). + + + Global (WGS84) coordinate frame (scaled) + altitude relative to the home position. First value / x: latitude in degrees*1E7, second value / y: longitude in degrees*1E7, third value / z: positive altitude with 0 being at the altitude of the home location. + + + NED local tangent frame (x: North, y: East, z: Down) with origin that travels with the vehicle. + + + + Same as MAV_FRAME_LOCAL_NED when used to represent position values. Same as MAV_FRAME_BODY_FRD when used with velocity/accelaration values. + + + + This is the same as MAV_FRAME_BODY_FRD. + + + Global (WGS84) coordinate frame with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model. + + + Global (WGS84) coordinate frame (scaled) with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees*1E7, second value / y: longitude in degrees*1E7, third value / z: positive altitude in meters with 0 being at ground level in terrain model. + + + FRD local tangent frame (x: Forward, y: Right, z: Down) with origin that travels with vehicle. The forward axis is aligned to the front of the vehicle in the horizontal plane. + + + + MAV_FRAME_BODY_FLU - Body fixed frame of reference, Z-up (x: Forward, y: Left, z: Up). + + + + MAV_FRAME_MOCAP_NED - Odometry local coordinate frame of data given by a motion capture system, Z-down (x: North, y: East, z: Down). + + + + MAV_FRAME_MOCAP_ENU - Odometry local coordinate frame of data given by a motion capture system, Z-up (x: East, y: North, z: Up). + + + + MAV_FRAME_VISION_NED - Odometry local coordinate frame of data given by a vision estimation system, Z-down (x: North, y: East, z: Down). + + + + MAV_FRAME_VISION_ENU - Odometry local coordinate frame of data given by a vision estimation system, Z-up (x: East, y: North, z: Up). + + + + MAV_FRAME_ESTIM_NED - Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-down (x: North, y: East, z: Down). + + + + MAV_FRAME_ESTIM_ENU - Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-up (x: East, y: North, z: Up). + + + FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth. The forward axis is aligned to the front of the vehicle in the horizontal plane. + + + FLU local tangent frame (x: Forward, y: Left, z: Up) with origin fixed relative to earth. The forward axis is aligned to the front of the vehicle in the horizontal plane. + + + + + + + + + + + + + + + + + + + + + + + + + + Disable fenced mode + + + Switched to guided mode to return point (fence point 0) + + + Report fence breach, but don't take action + + + Switched to guided mode to return point (fence point 0) with manual throttle control + + + Switch to RTL (return to launch) mode and head for the return point. + + + + + No last fence breach + + + Breached minimum altitude + + + Breached maximum altitude + + + Breached fence boundary + + + + + Actions being taken to mitigate/prevent fence breach + + Unknown + + + No actions being taken + + + Velocity limiting active to prevent breach + + + + + Enumeration of possible mount operation modes + + Load and keep safe position (Roll,Pitch,Yaw) from permant memory and stop stabilization + + + Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. + + + Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization + + + Load neutral position and start RC Roll,Pitch,Yaw control with stabilization + + + Load neutral position and start to point to Lat,Lon,Alt + + + Gimbal tracks system with specified system ID + + + Gimbal tracks home location + + + + + Gimbal device (low level) capability flags (bitmap). + + Gimbal device supports a retracted position. + + + Gimbal device supports a horizontal, forward looking position, stabilized. + + + Gimbal device supports rotating around roll axis. + + + Gimbal device supports to follow a roll angle relative to the vehicle. + + + Gimbal device supports locking to a roll angle (generally that's the default with roll stabilized). + + + Gimbal device supports rotating around pitch axis. + + + Gimbal device supports to follow a pitch angle relative to the vehicle. + + + Gimbal device supports locking to a pitch angle (generally that's the default with pitch stabilized). + + + Gimbal device supports rotating around yaw axis. + + + Gimbal device supports to follow a yaw angle relative to the vehicle (generally that's the default). + + + Gimbal device supports locking to an absolute heading, i.e., yaw angle relative to North (earth frame, often this is an option available). + + + Gimbal device supports yawing/panning infinetely (e.g. using slip disk). + + + Gimbal device supports yaw angles and angular velocities relative to North (earth frame). This usually requires support by an autopilot via AUTOPILOT_STATE_FOR_GIMBAL_DEVICE. Support can go on and off during runtime, which is reported by the flag GIMBAL_DEVICE_FLAGS_CAN_ACCEPT_YAW_IN_EARTH_FRAME. + + + Gimbal device supports radio control inputs as an alternative input for controlling the gimbal orientation. + + + + Gimbal manager high level capability flags (bitmap). The first 16 bits are identical to the GIMBAL_DEVICE_CAP_FLAGS. However, the gimbal manager does not need to copy the flags from the gimbal but can also enhance the capabilities and thus add flags. + + Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT. + + + Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL. + + + Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS. + + + Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW. + + + Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK. + + + Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS. + + + Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW. + + + Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK. + + + Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS. + + + Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW. + + + Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK. + + + Based on GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW. + + + Based on GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_YAW_IN_EARTH_FRAME. + + + Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_RC_INPUTS. + + + Gimbal manager supports to point to a local position. + + + Gimbal manager supports to point to a global latitude, longitude, altitude position. + + + + Flags for gimbal device (lower level) operation. + + Set to retracted safe position (no stabilization), takes presedence over all other flags. + + + Set to neutral/default position, taking precedence over all other flags except RETRACT. Neutral is commonly forward-facing and horizontal (roll=pitch=yaw=0) but may be any orientation. + + + Lock roll angle to absolute angle relative to horizon (not relative to vehicle). This is generally the default with a stabilizing gimbal. + + + Lock pitch angle to absolute angle relative to horizon (not relative to vehicle). This is generally the default with a stabilizing gimbal. + + + Lock yaw angle to absolute angle relative to North (not relative to vehicle). If this flag is set, the yaw angle and z component of angular velocity are relative to North (earth frame, x-axis pointing North), else they are relative to the vehicle heading (vehicle frame, earth frame rotated so that the x-axis is pointing forward). + + + Yaw angle and z component of angular velocity are relative to the vehicle heading (vehicle frame, earth frame rotated such that the x-axis is pointing forward). + + + Yaw angle and z component of angular velocity are relative to North (earth frame, x-axis is pointing North). + + + Gimbal device can accept yaw angle inputs relative to North (earth frame). This flag is only for reporting (attempts to set this flag are ignored). + + + The gimbal orientation is set exclusively by the RC signals feed to the gimbal's radio control inputs. MAVLink messages for setting the gimbal orientation (GIMBAL_DEVICE_SET_ATTITUDE) are ignored. + + + The gimbal orientation is determined by combining/mixing the RC signals feed to the gimbal's radio control inputs and the MAVLink messages for setting the gimbal orientation (GIMBAL_DEVICE_SET_ATTITUDE). How these two controls are combined or mixed is not defined by the protocol but is up to the implementation. + + + + Flags for high level gimbal manager operation The first 16 bits are identical to the GIMBAL_DEVICE_FLAGS. + + Based on GIMBAL_DEVICE_FLAGS_RETRACT. + + + Based on GIMBAL_DEVICE_FLAGS_NEUTRAL. + + + Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK. + + + Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK. + + + Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK. + + + Based on GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME. + + + Based on GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME. + + + Based on GIMBAL_DEVICE_FLAGS_ACCEPTS_YAW_IN_EARTH_FRAME. + + + Based on GIMBAL_DEVICE_FLAGS_RC_EXCLUSIVE. + + + Based on GIMBAL_DEVICE_FLAGS_RC_MIXED. + + + + Gimbal device (low level) error flags (bitmap, 0 means no error) + + Gimbal device is limited by hardware roll limit. + + + Gimbal device is limited by hardware pitch limit. + + + Gimbal device is limited by hardware yaw limit. + + + There is an error with the gimbal encoders. + + + There is an error with the gimbal power source. + + + There is an error with the gimbal motors. + + + There is an error with the gimbal's software. + + + There is an error with the gimbal's communication. + + + Gimbal device is currently calibrating. + + + Gimbal device is not assigned to a gimbal manager. + + + + + Gripper actions. + + Gripper release cargo. + + + Gripper grab onto cargo. + + + + + Winch actions. + + Allow motor to freewheel. + + + Wind or unwind specified length of line, optionally using specified rate. + + + Wind or unwind line at specified rate. + + + Perform the locking sequence to relieve motor while in the fully retracted position. Only action and instance command parameters are used, others are ignored. + + + Sequence of drop, slow down, touch down, reel up, lock. Only action and instance command parameters are used, others are ignored. + + + Engage motor and hold current position. Only action and instance command parameters are used, others are ignored. + + + Return the reel to the fully retracted position. Only action and instance command parameters are used, others are ignored. + + + Load the reel with line. The winch will calculate the total loaded length and stop when the tension exceeds a threshold. Only action and instance command parameters are used, others are ignored. + + + Spool out the entire length of the line. Only action and instance command parameters are used, others are ignored. + + + + + Generalized UAVCAN node health + + The node is functioning properly. + + + A critical parameter went out of range or the node has encountered a minor failure. + + + The node has encountered a major failure. + + + The node has suffered a fatal malfunction. + + + + + Generalized UAVCAN node mode + + The node is performing its primary functions. + + + The node is initializing; this mode is entered immediately after startup. + + + The node is under maintenance. + + + The node is in the process of updating its software. + + + The node is no longer available online. + + + + Flags to indicate the status of camera storage. + + Storage is missing (no microSD card loaded for example.) + + + Storage present but unformatted. + + + Storage present and ready. + + + Camera does not supply storage status information. Capacity information in STORAGE_INFORMATION fields will be ignored. + + + + Flags to indicate the type of storage. + + Storage type is not known. + + + Storage type is USB device. + + + Storage type is SD card. + + + Storage type is microSD card. + + + Storage type is CFast. + + + Storage type is CFexpress. + + + Storage type is XQD. + + + Storage type is HD mass storage type. + + + Storage type is other, not listed type. + + + + Enable axes that will be tuned via autotuning. Used in MAV_CMD_DO_AUTOTUNE_ENABLE. + + Flight stack tunes axis according to its default settings. + + + Autotune roll axis. + + + Autotune pitch axis. + + + Autotune yaw axis. + + + + + + + + Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. NaN and INT32_MAX may be used in float/integer params (respectively) to indicate optional/default values (e.g. to use the component's current yaw or latitude rather than a specific value). See https://mavlink.io/en/guide/xml_schema.html#MAV_CMD for information about the structure of the MAV_CMD entries + + Navigate to waypoint. + Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing) + Acceptance radius (if the sphere with this radius is hit, the waypoint counts as reached) + 0 to pass through the WP, if > 0 radius to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. + Desired yaw angle at waypoint (rotary wing). NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). + Latitude + Longitude + Altitude + + + Loiter around this waypoint an unlimited amount of time + Empty + Empty + Loiter radius around waypoint for forward-only moving vehicles (not multicopters). If positive loiter clockwise, else counter-clockwise + Desired yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). + Latitude + Longitude + Altitude + + + Loiter around this waypoint for X turns + Number of turns. + Empty + Radius around waypoint. If positive loiter clockwise, else counter-clockwise + Forward moving aircraft this sets exit xtrack location: 0 for center of loiter wp, 1 for exit location. Else, this is desired yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). + Latitude + Longitude + Altitude + + + Loiter around this waypoint for X seconds + Loiter time. + Empty + Radius around waypoint. If positive loiter clockwise, else counter-clockwise. + Forward moving aircraft this sets exit xtrack location: 0 for center of loiter wp, 1 for exit location. Else, this is desired yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). + Latitude + Longitude + Altitude + + + Return to launch location + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Land at location. + Minimum target altitude if landing is aborted (0 = undefined/use system default). + Precision land mode. + Empty. + Desired yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). + Latitude. + Longitude. + Landing altitude (ground level in current frame). + + + Takeoff from ground / hand. Vehicles that support multiple takeoff modes (e.g. VTOL quadplane) should take off using the currently configured mode. + Minimum pitch (if airspeed sensor present), desired pitch without sensor + Empty + Empty + Yaw angle (if magnetometer present), ignored without magnetometer. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). + Latitude + Longitude + Altitude + + + Land at local position (local frame only) + Landing target number (if available) + Maximum accepted offset from desired landing position - computed magnitude from spherical coordinates: d = sqrt(x^2 + y^2 + z^2), which gives the maximum accepted distance between the desired landing position and the position where the vehicle is about to land + Landing descend rate + Desired yaw angle + Y-axis position + X-axis position + Z-axis / ground level position + + + Takeoff from local position (local frame only) + Minimum pitch (if airspeed sensor present), desired pitch without sensor + Empty + Takeoff ascend rate + Yaw angle (if magnetometer or another yaw estimation source present), ignored without one of these + Y-axis position + X-axis position + Z-axis position + + + Vehicle following, i.e. this waypoint represents the position of a moving vehicle + Following logic to use (e.g. loitering or sinusoidal following) - depends on specific autopilot implementation + Ground speed of vehicle to be followed + Radius around waypoint. If positive loiter clockwise, else counter-clockwise + Desired yaw angle. + Latitude + Longitude + Altitude + + + Continue on the current course and climb/descend to specified altitude. When the altitude is reached continue to the next command (i.e., don't proceed to the next command until the desired altitude is reached. + Climb or Descend (0 = Neutral, command completes when within 5m of this command's altitude, 1 = Climbing, command completes when at or above this command's altitude, 2 = Descending, command completes when at or below this command's altitude. + Empty + Empty + Empty + Empty + Empty + Desired altitude + + + Begin loiter at the specified Latitude and Longitude. If Lat=Lon=0, then loiter at the current position. Don't consider the navigation command complete (don't leave loiter) until the altitude has been reached. Additionally, if the Heading Required parameter is non-zero the aircraft will not leave the loiter until heading toward the next waypoint. + Heading Required (0 = False) + Loiter radius around waypoint for forward-only moving vehicles (not multicopters). If positive loiter clockwise, negative counter-clockwise, 0 means no change to standard loiter. + Empty + Forward moving aircraft this sets exit xtrack location: 0 for center of loiter wp, 1 for exit location + Latitude + Longitude + Altitude + + + Begin following a target + System ID (of the FOLLOW_TARGET beacon). Send 0 to disable follow-me and return to the default position hold mode. + Reserved + Reserved + Altitude mode: 0: Keep current altitude, 1: keep altitude difference to target, 2: go to a fixed altitude above home. + Altitude above home. (used if mode=2) + Reserved + Time to land in which the MAV should go to the default position hold mode after a message RX timeout. + + + Reposition the MAV after a follow target command has been sent + Camera q1 (where 0 is on the ray from the camera to the tracking device) + Camera q2 + Camera q3 + Camera q4 + altitude offset from target + X offset from target + Y offset from target + + + + Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. + Region of interest mode. + Waypoint index/ target ID. (see MAV_ROI enum) + ROI index (allows a vehicle to manage multiple ROI's) + Empty + x the location of the fixed ROI (see MAV_FRAME) + y + z + + + Control autonomous path planning on the MAV. + 0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning + 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid + Empty + Yaw angle at goal + Latitude/X of goal + Longitude/Y of goal + Altitude/Z of goal + + + Navigate to waypoint using a spline path. + Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing) + Empty + Empty + Empty + Latitude/X of goal + Longitude/Y of goal + Altitude/Z of goal + + + Takeoff from ground using VTOL mode, and transition to forward flight with specified heading. The command should be ignored by vehicles that dont support both VTOL and fixed-wing flight (multicopters, boats,etc.). + Empty + Front transition heading. + Empty + Yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). + Latitude + Longitude + Altitude + + + Land using VTOL mode + See NAV_VTOL_LAND_OPTIONS enum + Empty + Approach altitude (with the same reference as the Altitude field). NaN if unspecified. + Yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). + Latitude + Longitude + Altitude (ground level) + + + + hand control over to an external controller + On / Off (> 0.5f on) + Empty + Empty + Empty + Empty + Empty + Empty + + + Delay the next navigation command a number of seconds or until a specified time + Delay (-1 to enable time-of-day fields) + hour (24h format, UTC, -1 to ignore) + minute (24h format, UTC, -1 to ignore) + second (24h format, UTC, -1 to ignore) + Empty + Empty + Empty + + + Descend and place payload. Vehicle moves to specified location, descends until it detects a hanging payload has reached the ground, and then releases the payload. If ground is not detected before the reaching the maximum descent value (param1), the command will complete without releasing the payload. + Maximum distance to descend. + Empty + Empty + Empty + Latitude + Longitude + Altitude + + + NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Delay mission state machine. + Delay + Empty + Empty + Empty + Empty + Empty + Empty + + + Ascend/descend to target altitude at specified rate. Delay mission state machine until desired altitude reached. + Descent / Ascend rate. + Empty + Empty + Empty + Empty + Empty + Target Altitude + + + Delay mission state machine until within desired distance of next NAV point. + Distance. + Empty + Empty + Empty + Empty + Empty + Empty + + + Reach a certain target angle. + target angle, 0 is north + angular speed + direction: -1: counter clockwise, 1: clockwise + 0: absolute angle, 1: relative offset + Empty + Empty + Empty + + + NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Set system mode. + Mode + Custom mode - this is system specific, please refer to the individual autopilot specifications for details. + Custom sub mode - this is system specific, please refer to the individual autopilot specifications for details. + Empty + Empty + Empty + Empty + + + Jump to the desired command in the mission list. Repeat this action only the specified number of times + Sequence number + Repeat count + Empty + Empty + Empty + Empty + Empty + + + Change speed and/or throttle set points. + Speed type (0=Airspeed, 1=Ground Speed, 2=Climb Speed, 3=Descent Speed) + Speed (-1 indicates no change, -2 indicates return to default vehicle speed) + Throttle (-1 indicates no change, -2 indicates return to default vehicle throttle value) + 0: absolute, 1: relative + Empty + Empty + Empty + + + Changes the home location either to the current location or a specified location. + Use current (1=use current location, 0=use specified location) + Empty + Empty + Empty + Latitude + Longitude + Altitude + + + Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. + Parameter number + Parameter value + Empty + Empty + Empty + Empty + Empty + + + Set a relay to a condition. + Relay instance number. + Setting. (1=on, 0=off, others possible depending on system hardware) + Empty + Empty + Empty + Empty + Empty + + + Cycle a relay on and off for a desired number of cycles with a desired period. + Relay instance number. + Cycle count. + Cycle time. + Empty + Empty + Empty + Empty + + + Set a servo to a desired PWM value. + Servo instance number. + Pulse Width Modulation. + Empty + Empty + Empty + Empty + Empty + + + Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. + Servo instance number. + Pulse Width Modulation. + Cycle count. + Cycle time. + Empty + Empty + Empty + + + Terminate flight immediately + Flight termination activated if > 0.5 + Empty + Empty + Empty + Empty + Empty + Empty + + + Change altitude set point. + Altitude. + Frame of new altitude. + Empty + Empty + Empty + Empty + Empty + + + Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. + It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. + The Latitude/Longitude/Altitude is optional, and may be set to 0 if not needed. If specified then it will be used to help find the closest landing sequence. + + Empty + Empty + Empty + Empty + Latitude + Longitude + Altitude + + + Mission command to perform a landing from a rally point. + Break altitude + Landing speed + Empty + Empty + Empty + Empty + Empty + + + Mission command to safely abort an autonomous landing. + Altitude + Empty + Empty + Empty + Empty + Empty + Empty + + + Reposition the vehicle to a specific WGS84 global position. + Ground speed, less than 0 (-1) for default + Bitmask of option flags. + Loiter radius for planes. Positive values only, direction is controlled by Yaw value. A value of zero or NaN is ignored. + Yaw heading. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). For planes indicates loiter direction (0: clockwise, 1: counter clockwise) + Latitude + Longitude + Altitude + + + If in a GPS controlled position mode, hold the current position or continue. + 0: Pause current mission or reposition command, hold current position. 1: Continue mission. A VTOL capable vehicle should enter hover mode (multicopter and VTOL planes). A plane should loiter with the default loiter radius. + Reserved + Reserved + Reserved + Reserved + Reserved + Reserved + + + Set moving direction to forward or reverse. + Direction (0=Forward, 1=Reverse) + Empty + Empty + Empty + Empty + Empty + Empty + + + Sets the region of interest (ROI) to a location. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. + Empty + Empty + Empty + Empty + Latitude of ROI location + Longitude of ROI location + Altitude of ROI location + + + Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. + Empty + Empty + Empty + Empty + Pitch offset from next waypoint, positive pitching up + Roll offset from next waypoint, positive rolling to the right + Yaw offset from next waypoint, positive yawing to the right + + + Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Mount tracks system with specified system ID. Determination of target vehicle position may be done with GLOBAL_POSITION_INT or any other means. + System ID + + + Control onboard camera system. + Camera ID (-1 for all) + Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw + Transmission mode: 0: video stream, >0: single images every n seconds + Recording: 0: disabled, 1: enabled compressed, 2: enabled raw + Empty + Empty + Empty + + + + Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. + Region of interest mode. + Waypoint index/ target ID (depends on param 1). + Region of interest index. (allows a vehicle to manage multiple ROI's) + Empty + x the location of the fixed ROI (see MAV_FRAME) + y + z + + + + Configure digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ). + Modes: P, TV, AV, M, Etc. + Shutter speed: Divisor number for one second. + Aperture: F stop number. + ISO number e.g. 80, 100, 200, Etc. + Exposure type enumerator. + Command Identity. + Main engine cut-off time before camera trigger. (0 means no cut-off) + + + Control digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ). + Session control e.g. show/hide lens + Zoom's absolute position + Zooming step value to offset zoom from the current position + Focus Locking, Unlocking or Re-locking + Shooting Command + Command Identity + Test shot identifier. If set to 1, image will only be captured, but not counted towards internal frame count. + + + + Mission command to configure a camera or antenna mount + Mount operation mode + stabilize roll? (1 = yes, 0 = no) + stabilize pitch? (1 = yes, 0 = no) + stabilize yaw? (1 = yes, 0 = no) + Empty + Empty + Empty + + + + Mission command to control a camera or antenna mount + pitch (WIP: DEPRECATED: or lat in degrees) depending on mount mode. + roll (WIP: DEPRECATED: or lon in degrees) depending on mount mode. + yaw (WIP: DEPRECATED: or alt in meters) depending on mount mode. + WIP: alt in meters depending on mount mode. + WIP: latitude in degrees * 1E7, set if appropriate mount mode. + WIP: longitude in degrees * 1E7, set if appropriate mount mode. + Mount mode. + + + Mission command to set camera trigger distance for this flight. The camera is triggered each time this distance is exceeded. This command can also be used to set the shutter integration time for the camera. + Camera trigger distance. 0 to stop triggering. + Camera shutter integration time. -1 or 0 to ignore + Trigger camera once immediately. (0 = no trigger, 1 = trigger) + Empty + Empty + Empty + Empty + + + Mission command to enable the geofence + enable? (0=disable, 1=enable, 2=disable_floor_only) + Empty + Empty + Empty + Empty + Empty + Empty + + + Mission item/command to release a parachute or enable/disable auto release. + Action + Empty + Empty + Empty + Empty + Empty + Empty + + + Mission command to perform motor test. + Motor instance number. (from 1 to max number of motors on the vehicle) + Throttle type. + Throttle. + Timeout. + Motor count. (number of motors to test to test in sequence, waiting for the timeout above between them; 0=1 motor, 1=1 motor, 2=2 motors...) + Motor test order. + Empty + + + Change to/from inverted flight. + Inverted flight. (0=normal, 1=inverted) + Empty + Empty + Empty + Empty + Empty + Empty + + + Mission command to operate a gripper. + Gripper instance number. + Gripper action to perform. + Empty + Empty + Empty + Empty + Empty + + + Enable/disable autotune. + Enable (1: enable, 0:disable). + Specify which axes are autotuned. 0 indicates autopilot default settings. + Empty. + Empty. + Empty. + Empty. + Empty. + + + Sets a desired vehicle turn angle and speed change. + Yaw angle to adjust steering by. + Speed. + Final angle. (0=absolute, 1=relative) + Empty + Empty + Empty + Empty + + + Mission command to set camera trigger interval for this flight. If triggering is enabled, the camera is triggered each time this interval expires. This command can also be used to set the shutter integration time for the camera. + Camera trigger cycle time. -1 or 0 to ignore. + Camera shutter integration time. Should be less than trigger cycle time. -1 or 0 to ignore. + Empty + Empty + Empty + Empty + Empty + + + Mission command to control a camera or antenna mount, using a quaternion as reference. + quaternion param q1, w (1 in null-rotation) + quaternion param q2, x (0 in null-rotation) + quaternion param q3, y (0 in null-rotation) + quaternion param q4, z (0 in null-rotation) + Empty + Empty + Empty + + + set id of master controller + System ID + Component ID + Empty + Empty + Empty + Empty + Empty + + + Set limits for external control + Timeout - maximum time that external controller will be allowed to control vehicle. 0 means no timeout. + Altitude (MSL) min - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit. + Altitude (MSL) max - if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit. + Horizontal move limit - if vehicle moves more than this distance from its location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal move limit. + Empty + Empty + Empty + + + Control vehicle engine. This is interpreted by the vehicles engine controller to change the target engine state. It is intended for vehicles with internal combustion engines + 0: Stop engine, 1:Start Engine + 0: Warm start, 1:Cold start. Controls use of choke where applicable + Height delay. This is for commanding engine start only after the vehicle has gained the specified height. Used in VTOL vehicles during takeoff to start engine after the aircraft is off the ground. Zero for no delay. + Empty + Empty + Empty + Empty + Empty + + + Set the mission item with sequence number seq as current item. This means that the MAV will continue to this mission item on the shortest path (not following the mission items in-between). + Mission sequence value to set + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + NOP - This command is only used to mark the upper limit of the DO commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Trigger calibration. This command will be only accepted if in pre-flight mode. Except for Temperature Calibration, only one sensor should be set in a single message and all others should be zero. + 1: gyro calibration, 3: gyro temperature calibration + 1: magnetometer calibration + 1: ground pressure calibration + 1: radio RC calibration, 2: RC trim calibration + 1: accelerometer calibration, 2: board level calibration, 3: accelerometer temperature calibration, 4: simple accelerometer calibration + 1: APM: compass/motor interference calibration (PX4: airspeed calibration, deprecated), 2: airspeed calibration + 1: ESC calibration, 3: barometer temperature calibration + + + Set sensor offsets. This command will be only accepted if in pre-flight mode. + Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow, 5: second magnetometer, 6: third magnetometer + X axis offset (or generic dimension 1), in the sensor's raw units + Y axis offset (or generic dimension 2), in the sensor's raw units + Z axis offset (or generic dimension 3), in the sensor's raw units + Generic dimension 4, in the sensor's raw units + Generic dimension 5, in the sensor's raw units + Generic dimension 6, in the sensor's raw units + + + Trigger UAVCAN configuration (actuator ID assignment and direction mapping). Note that this maps to the legacy UAVCAN v0 function UAVCAN_ENUMERATE, which is intended to be executed just once during initial vehicle configuration (it is not a normal pre-flight command and has been poorly named). + 1: Trigger actuator ID assignment and direction mapping. 0: Cancel command. + Reserved + Reserved + Reserved + Reserved + Reserved + Reserved + + + Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. + Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM, 2: Reset to defaults + Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM, 2: Reset to defaults + Onboard logging: 0: Ignore, 1: Start default rate logging, -1: Stop logging, > 1: logging rate (e.g. set to 1000 for 1000 Hz logging) + Reserved + Empty + Empty + Empty + + + Request the reboot or shutdown of system components. + 0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot, 3: Reboot autopilot and keep it in the bootloader until upgraded. + 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer, 3: Reboot onboard computer and keep it in the bootloader until upgraded. + WIP: 0: Do nothing for camera, 1: Reboot onboard camera, 2: Shutdown onboard camera, 3: Reboot onboard camera and keep it in the bootloader until upgraded + WIP: 0: Do nothing for mount (e.g. gimbal), 1: Reboot mount, 2: Shutdown mount, 3: Reboot mount and keep it in the bootloader until upgraded + Reserved (set to 0) + Reserved (set to 0) + WIP: ID (e.g. camera ID -1 for all IDs) + + + Override current mission with command to pause mission, pause mission and move to position, continue/resume mission. When param 1 indicates that the mission is paused (MAV_GOTO_DO_HOLD), param 2 defines whether it holds in place or moves to another position. + MAV_GOTO_DO_HOLD: pause mission and either hold or move to specified position (depending on param2), MAV_GOTO_DO_CONTINUE: resume mission. + MAV_GOTO_HOLD_AT_CURRENT_POSITION: hold at current position, MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position. + Coordinate frame of hold point. + Desired yaw angle. + Latitude/X position. + Longitude/Y position. + Altitude/Z position. + + + Mission command to set a Camera Auto Mount Pivoting Oblique Survey (Replaces CAM_TRIGG_DIST for this purpose). The camera is triggered each time this distance is exceeded, then the mount moves to the next position. Params 4~6 set-up the angle limits and number of positions for oblique survey, where mount-enabled vehicles automatically roll the camera between shots to emulate an oblique camera setup (providing an increased HFOV). This command can also be used to set the shutter integration time for the camera. + Camera trigger distance. 0 to stop triggering. + Camera shutter integration time. 0 to ignore + The minimum interval in which the camera is capable of taking subsequent pictures repeatedly. 0 to ignore. + Total number of roll positions at which the camera will capture photos (images captures spread evenly across the limits defined by param5). + Angle limits that the camera can be rolled to left and right of center. + Fixed pitch angle that the camera will hold in oblique mode if the mount is actuated in the pitch axis. + Empty + + + start running a mission + first_item: the first mission item to run + last_item: the last mission item to run (after this item is run, the mission ends) + + + Arms / Disarms a component + 0: disarm, 1: arm + 0: arm-disarm unless prevented by safety checks (i.e. when landed), 21196: force arming/disarming (e.g. allow arming to override preflight checks and disarming in flight) + + + Instructs system to run pre-arm checks. This command should return MAV_RESULT_TEMPORARILY_REJECTED in the case the system is armed, otherwse MAV_RESULT_ACCEPTED. Note that the return value from executing this command does not indicate whether the vehicle is armable or not, just whether the system has successfully run/is currently running the checks. The result of the checks is reflected in the SYS_STATUS message. + + + Request the home position from the vehicle. + Reserved + Reserved + Reserved + Reserved + Reserved + Reserved + Reserved + + + Starts receiver pairing. + 0:Spektrum. + RC type. + + + Request the interval between messages for a particular MAVLink message ID. The receiver should ACK the command and then emit its response in a MESSAGE_INTERVAL message. + The MAVLink message ID + + + Set the interval between messages for a particular MAVLink message ID. This interface replaces REQUEST_DATA_STREAM. + The MAVLink message ID + The interval between two messages. Set to -1 to disable and 0 to request default rate. + Target address of message stream (if message has target address fields). 0: Flight-stack default (recommended), 1: address of requestor, 2: broadcast. + + + Request the target system(s) emit a single instance of a specified message (i.e. a "one-shot" version of MAV_CMD_SET_MESSAGE_INTERVAL). + The MAVLink message ID of the requested message. + Use for index ID, if required. Otherwise, the use of this parameter (if any) must be defined in the requested message. By default assumed not used (0). + The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0). + The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0). + The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0). + The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0). + Target address for requested message (if message has target address fields). 0: Flight-stack default, 1: address of requestor, 2: broadcast. + + + + Request MAVLink protocol version compatibility. All receivers should ACK the command and then emit their capabilities in an PROTOCOL_VERSION message + 1: Request supported protocol versions by all nodes on the network + Reserved (all remaining params) + + + + Request autopilot capabilities. The receiver should ACK the command and then emit its capabilities in an AUTOPILOT_VERSION message + 1: Request autopilot version + Reserved (all remaining params) + + + + Request camera information (CAMERA_INFORMATION). + 0: No action 1: Request camera capabilities + Reserved (all remaining params) + + + + Request camera settings (CAMERA_SETTINGS). + 0: No Action 1: Request camera settings + Reserved (all remaining params) + + + + Request storage information (STORAGE_INFORMATION). Use the command's target_component to target a specific component's storage. + Storage ID (0 for all, 1 for first, 2 for second, etc.) + 0: No Action 1: Request storage information + Reserved (all remaining params) + + + Format a storage medium. Once format is complete, a STORAGE_INFORMATION message is sent. Use the command's target_component to target a specific component's storage. + Storage ID (1 for first, 2 for second, etc.) + 0: No action 1: Format storage + Reserved (all remaining params) + + + + Request camera capture status (CAMERA_CAPTURE_STATUS) + 0: No Action 1: Request camera capture status + Reserved (all remaining params) + + + + Request flight information (FLIGHT_INFORMATION) + 1: Request flight information + Reserved (all remaining params) + + + Reset all camera settings to Factory Default + 0: No Action 1: Reset all settings + Reserved (all remaining params) + + + Set camera running mode. Use NaN for reserved values. GCS will send a MAV_CMD_REQUEST_VIDEO_STREAM_STATUS command after a mode change if the camera supports video streaming. + Reserved (Set to 0) + Camera mode + + + + + + Set camera zoom. Camera must respond with a CAMERA_SETTINGS message (on success). + Zoom type + Zoom value. The range of valid values depend on the zoom type. + + + + + + Set camera focus. Camera must respond with a CAMERA_SETTINGS message (on success). + Focus type + Focus value + + + + + + Tagged jump target. Can be jumped to with MAV_CMD_DO_JUMP_TAG. + Tag. + + + Jump to the matching tag in the mission list. Repeat this action for the specified number of times. A mission should contain a single matching tag for each jump. If this is not the case then a jump to a missing tag should complete the mission, and a jump where there are multiple matching tags should always select the one with the lowest mission sequence number. + Target tag to jump to. + Repeat count. + + + + Set gimbal manager pitch/yaw setpoints (low rate command). It is possible to set combinations of the values below. E.g. an angle as well as a desired angular rate can be used to get to this angle at a certain angular rate, or an angular rate only will result in continuous turning. NaN is to be used to signal unset. Note: only the gimbal manager will react to this command - it will be ignored by a gimbal device. Use GIMBAL_MANAGER_SET_PITCHYAW if you need to stream pitch/yaw setpoints at higher rate. + Pitch angle (positive to pitch up, relative to vehicle for FOLLOW mode, relative to world horizon for LOCK mode). + Yaw angle (positive to yaw to the right, relative to vehicle for FOLLOW mode, absolute to North for LOCK mode). + Pitch rate (positive to pitch up). + Yaw rate (positive to yaw to the right). + Gimbal manager flags to use. + Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + + + Gimbal configuration to set which sysid/compid is in primary and secondary control. + Sysid for primary control (0: no one in control, -1: leave unchanged, -2: set itself in control (for missions where the own sysid is still unknown), -3: remove control if currently in control). + Compid for primary control (0: no one in control, -1: leave unchanged, -2: set itself in control (for missions where the own sysid is still unknown), -3: remove control if currently in control). + Sysid for secondary control (0: no one in control, -1: leave unchanged, -2: set itself in control (for missions where the own sysid is still unknown), -3: remove control if currently in control). + Compid for secondary control (0: no one in control, -1: leave unchanged, -2: set itself in control (for missions where the own sysid is still unknown), -3: remove control if currently in control). + Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + + + Start image capture sequence. Sends CAMERA_IMAGE_CAPTURED after each capture. Use NaN for reserved values. + Reserved (Set to 0) + Desired elapsed time between two consecutive pictures (in seconds). Minimum values depend on hardware (typically greater than 2 seconds). + Total number of images to capture. 0 to capture forever/until MAV_CMD_IMAGE_STOP_CAPTURE. + Capture sequence number starting from 1. This is only valid for single-capture (param3 == 1), otherwise set to 0. Increment the capture ID for each capture command to prevent double captures when a command is re-transmitted. + + + + + + Stop image capture sequence Use NaN for reserved values. + Reserved (Set to 0) + + + + + + + Enable or disable on-board camera triggering system. + Trigger enable/disable (0 for disable, 1 for start), -1 to ignore + 1 to reset the trigger sequence, -1 or 0 to ignore + 1 to pause triggering, but without switching the camera off or retracting it. -1 to ignore + + + If the camera supports point visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_POINT is set), this command allows to initiate the tracking. + Point to track x value (normalized 0..1, 0 is left, 1 is right). + Point to track y value (normalized 0..1, 0 is top, 1 is bottom). + Point radius (normalized 0..1, 0 is image left, 1 is image right). + + + If the camera supports rectangle visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE is set), this command allows to initiate the tracking. + Top left corner of rectangle x value (normalized 0..1, 0 is left, 1 is right). + Top left corner of rectangle y value (normalized 0..1, 0 is top, 1 is bottom). + Bottom right corner of rectangle x value (normalized 0..1, 0 is left, 1 is right). + Bottom right corner of rectangle y value (normalized 0..1, 0 is top, 1 is bottom). + + + Stops ongoing tracking. + + + Starts video capture (recording). + Video Stream ID (0 for all streams) + Frequency CAMERA_CAPTURE_STATUS messages should be sent while recording (0 for no messages, otherwise frequency) + + + + + + + + Stop the current video capture (recording). + Video Stream ID (0 for all streams) + + + + + + + + + Start video streaming + Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.) + + + Stop the given video stream + Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.) + + + + Request video stream information (VIDEO_STREAM_INFORMATION) + Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.) + + + + Request video stream status (VIDEO_STREAM_STATUS) + Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.) + + + Request to start streaming logging data over MAVLink (see also LOGGING_DATA message) + Format: 0: ULog + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + + + Request to stop streaming log data over MAVLink + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + + + + Landing gear ID (default: 0, -1 for all) + Landing gear position (Down: 0, Up: 1, NaN for no change) + + + + + + + + Request to start/stop transmitting over the high latency telemetry + Control transmission over high latency telemetry (0: stop, 1: start) + Empty + Empty + Empty + Empty + Empty + Empty + + + Create a panorama at the current position + Viewing angle horizontal of the panorama (+- 0.5 the total angle) + Viewing angle vertical of panorama. + Speed of the horizontal rotation. + Speed of the vertical rotation. + + + Request VTOL transition + The target VTOL state. Only MAV_VTOL_STATE_MC and MAV_VTOL_STATE_FW can be used. + + + Request authorization to arm the vehicle to a external entity, the arm authorizer is responsible to request all data that is needs from the vehicle before authorize or deny the request. If approved the progress of command_ack message should be set with period of time that this authorization is valid in seconds or in case it was denied it should be set with one of the reasons in ARM_AUTH_DENIED_REASON. + + Vehicle system id, this way ground station can request arm authorization on behalf of any vehicle + + + This command sets the submode to standard guided when vehicle is in guided mode. The vehicle holds position and altitude and the user can input the desired velocities along all three axes. + + + + This command sets submode circle when vehicle is in guided mode. Vehicle flies along a circle facing the center of the circle. The user can input the velocity along the circle and change the radius. If no input is given the vehicle will hold position. + + Radius of desired circle in CIRCLE_MODE + User defined + User defined + User defined + Target latitude of center of circle in CIRCLE_MODE + Target longitude of center of circle in CIRCLE_MODE + + + Fence return point (there can only be one such point in a geofence definition). If rally points are supported they should be used instead. + Reserved + Reserved + Reserved + Reserved + Latitude + Longitude + Altitude + + + Fence vertex for an inclusion polygon (the polygon must not be self-intersecting). The vehicle must stay within this area. Minimum of 3 vertices required. + + Polygon vertex count + Vehicle must be inside ALL inclusion zones in a single group, vehicle must be inside at least one group, must be the same for all points in each polygon + Reserved + Reserved + Latitude + Longitude + Reserved + + + Fence vertex for an exclusion polygon (the polygon must not be self-intersecting). The vehicle must stay outside this area. Minimum of 3 vertices required. + + Polygon vertex count + Reserved + Reserved + Reserved + Latitude + Longitude + Reserved + + + Circular fence area. The vehicle must stay inside this area. + + Radius. + Vehicle must be inside ALL inclusion zones in a single group, vehicle must be inside at least one group + Reserved + Reserved + Latitude + Longitude + Reserved + + + Circular fence area. The vehicle must stay outside this area. + + Radius. + Reserved + Reserved + Reserved + Latitude + Longitude + Reserved + + + Rally point. You can have multiple rally points defined. + + Reserved + Reserved + Reserved + Reserved + Latitude + Longitude + Altitude + + + + + Commands the vehicle to respond with a sequence of messages UAVCAN_NODE_INFO, one message per every UAVCAN node that is online. Note that some of the response messages can be lost, which the receiver can detect easily by checking whether every received UAVCAN_NODE_STATUS has a matching message UAVCAN_NODE_INFO received earlier; if not, this command should be sent again in order to request re-transmission of the node information messages. + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + + + + Trigger the start of an ADSB-out IDENT. This should only be used when requested to do so by an Air Traffic Controller in controlled airspace. This starts the IDENT which is then typically held for 18 seconds by the hardware per the Mode A, C, and S transponder spec. + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + Reserved (set to 0) + + + + + Deploy payload on a Lat / Lon / Alt position. This includes the navigation to reach the required release position and velocity. + Operation mode. 0: prepare single payload deploy (overwriting previous requests), but do not execute it. 1: execute payload deploy immediately (rejecting further deploy commands during execution, but allowing abort). 2: add payload deploy to existing deployment list. + Desired approach vector in compass heading. A negative value indicates the system can define the approach vector at will. + Desired ground speed at release time. This can be overridden by the airframe in case it needs to meet minimum airspeed. A negative value indicates the system can define the ground speed at will. + Minimum altitude clearance to the release position. A negative value indicates the system can define the clearance at will. + Latitude. Note, if used in MISSION_ITEM (deprecated) the units are degrees (unscaled) + Longitude. Note, if used in MISSION_ITEM (deprecated) the units are degrees (unscaled) + Altitude (MSL) + + + Control the payload deployment. + Operation mode. 0: Abort deployment, continue normal mission. 1: switch to payload deployment mode. 100: delete first payload deployment request. 101: delete all payload deployment requests. + Reserved + Reserved + Reserved + Reserved + Reserved + Reserved + + + + Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location. + Yaw of vehicle in earth frame. + CompassMask, 0 for all. + Latitude. + Longitude. + Empty. + Empty. + Empty. + + + Command to operate winch. + Winch instance number. + Action to perform. + Length of cable to release (negative to wind). + Release rate (negative to wind). + Empty. + Empty. + Empty. + + + + + User defined waypoint item. Ground Station will show the Vehicle as flying through this item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL) + + + User defined waypoint item. Ground Station will show the Vehicle as flying through this item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL) + + + User defined waypoint item. Ground Station will show the Vehicle as flying through this item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL) + + + User defined waypoint item. Ground Station will show the Vehicle as flying through this item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL) + + + User defined waypoint item. Ground Station will show the Vehicle as flying through this item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL) + + + User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL) + + + User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL) + + + User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL) + + + User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL) + + + User defined spatial item. Ground Station will not show the Vehicle as flying through this item. Example: ROI item. + User defined + User defined + User defined + User defined + Latitude unscaled + Longitude unscaled + Altitude (MSL) + + + User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. + User defined + User defined + User defined + User defined + User defined + User defined + User defined + + + User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. + User defined + User defined + User defined + User defined + User defined + User defined + User defined + + + User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. + User defined + User defined + User defined + User defined + User defined + User defined + User defined + + + User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. + User defined + User defined + User defined + User defined + User defined + User defined + User defined + + + User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. + User defined + User defined + User defined + User defined + User defined + User defined + User defined + + + + Request forwarding of CAN packets from the given CAN bus to this interface. CAN Frames are sent using CAN_FRAME and CANFD_FRAME messages + Bus number (0 to disable forwarding, 1 for first bus, 2 for 2nd bus, 3 for 3rd bus). + Empty. + Empty. + Empty. + Empty. + Empty. + Empty. + + + + A data stream is not a fixed set of messages, but rather a + recommendation to the autopilot software. Individual autopilots may or may not obey + the recommended messages. + + Enable all data streams + + + Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. + + + Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS + + + Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW + + + Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. + + + Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. + + + Dependent on the autopilot + + + Dependent on the autopilot + + + Dependent on the autopilot + + + + + The ROI (region of interest) for the vehicle. This can be + be used by the vehicle for camera/vehicle attitude alignment (see + MAV_CMD_NAV_ROI). + + No region of interest. + + + Point toward next waypoint, with optional pitch/roll/yaw offset. + + + Point toward given waypoint. + + + Point toward fixed location. + + + Point toward of given id. + + + + Specifies the datatype of a MAVLink parameter. + + 8-bit unsigned integer + + + 8-bit signed integer + + + 16-bit unsigned integer + + + 16-bit signed integer + + + 32-bit unsigned integer + + + 32-bit signed integer + + + 64-bit unsigned integer + + + 64-bit signed integer + + + 32-bit floating-point + + + 64-bit floating-point + + + + Specifies the datatype of a MAVLink extended parameter. + + 8-bit unsigned integer + + + 8-bit signed integer + + + 16-bit unsigned integer + + + 16-bit signed integer + + + 32-bit unsigned integer + + + 32-bit signed integer + + + 64-bit unsigned integer + + + 64-bit signed integer + + + 32-bit floating-point + + + 64-bit floating-point + + + Custom Type + + + + Result from a MAVLink command (MAV_CMD) + + Command is valid (is supported and has valid parameters), and was executed. + + + Command is valid, but cannot be executed at this time. This is used to indicate a problem that should be fixed just by waiting (e.g. a state machine is busy, can't arm because have not got GPS lock, etc.). Retrying later should work. + + + Command is invalid (is supported but has invalid parameters). Retrying same command and parameters will not work. + + + Command is not supported (unknown). + + + Command is valid, but execution has failed. This is used to indicate any non-temporary or unexpected problem, i.e. any problem that must be fixed before the command can succeed/be retried. For example, attempting to write a file when out of memory, attempting to arm when sensors are not calibrated, etc. + + + Command is valid and is being executed. This will be followed by further progress updates, i.e. the component may send further COMMAND_ACK messages with result MAV_RESULT_IN_PROGRESS (at a rate decided by the implementation), and must terminate by sending a COMMAND_ACK message with final result of the operation. The COMMAND_ACK.progress field can be used to indicate the progress of the operation. There is no need for the sender to retry the command, but if done during execution, the component will return MAV_RESULT_IN_PROGRESS with an updated progress. + + + + Result of mission operation (in a MISSION_ACK message). + + mission accepted OK + + + Generic error / not accepting mission commands at all right now. + + + Coordinate frame is not supported. + + + Command is not supported. + + + Mission items exceed storage space. + + + One of the parameters has an invalid value. + + + param1 has an invalid value. + + + param2 has an invalid value. + + + param3 has an invalid value. + + + param4 has an invalid value. + + + x / param5 has an invalid value. + + + y / param6 has an invalid value. + + + z / param7 has an invalid value. + + + Mission item received out of sequence + + + Not accepting any mission commands from this communication partner. + + + Current mission operation cancelled (e.g. mission upload, mission download). + + + + Indicates the severity level, generally used for status messages to indicate their relative urgency. Based on RFC-5424 using expanded definitions at: http://www.kiwisyslog.com/kb/info:-syslog-message-levels/. + + System is unusable. This is a "panic" condition. + + + Action should be taken immediately. Indicates error in non-critical systems. + + + Action must be taken immediately. Indicates failure in a primary system. + + + Indicates an error in secondary/redundant systems. + + + Indicates about a possible future error if this is not resolved within a given timeframe. Example would be a low battery warning. + + + An unusual event has occurred, though not an error condition. This should be investigated for the root cause. + + + Normal operational messages. Useful for logging. No action is required for these messages. + + + Useful non-operational messages that can assist in debugging. These should not occur during normal operation. + + + + Power supply status flags (bitmask) + + main brick power supply valid + + + main servo power supply valid for FMU + + + USB power is connected + + + peripheral supply is in over-current state + + + hi-power peripheral supply is in over-current state + + + Power status has changed since boot + + + + SERIAL_CONTROL device types + + First telemetry port + + + Second telemetry port + + + First GPS port + + + Second GPS port + + + system shell + + + SERIAL0 + + + SERIAL1 + + + SERIAL2 + + + SERIAL3 + + + SERIAL4 + + + SERIAL5 + + + SERIAL6 + + + SERIAL7 + + + SERIAL8 + + + SERIAL9 + + + + SERIAL_CONTROL flags (bitmask) + + Set if this is a reply + + + Set if the sender wants the receiver to send a response as another SERIAL_CONTROL message + + + Set if access to the serial port should be removed from whatever driver is currently using it, giving exclusive access to the SERIAL_CONTROL protocol. The port can be handed back by sending a request without this flag set + + + Block on writes to the serial port + + + Send multiple replies until port is drained + + + + Enumeration of distance sensor types + + Laser rangefinder, e.g. LightWare SF02/F or PulsedLight units + + + Ultrasound rangefinder, e.g. MaxBotix units + + + Infrared rangefinder, e.g. Sharp units + + + Radar type, e.g. uLanding units + + + Broken or unknown type, e.g. analog units + + + + Enumeration of sensor orientation, according to its rotations + + Roll: 0, Pitch: 0, Yaw: 0 + + + Roll: 0, Pitch: 0, Yaw: 45 + + + Roll: 0, Pitch: 0, Yaw: 90 + + + Roll: 0, Pitch: 0, Yaw: 135 + + + Roll: 0, Pitch: 0, Yaw: 180 + + + Roll: 0, Pitch: 0, Yaw: 225 + + + Roll: 0, Pitch: 0, Yaw: 270 + + + Roll: 0, Pitch: 0, Yaw: 315 + + + Roll: 180, Pitch: 0, Yaw: 0 + + + Roll: 180, Pitch: 0, Yaw: 45 + + + Roll: 180, Pitch: 0, Yaw: 90 + + + Roll: 180, Pitch: 0, Yaw: 135 + + + Roll: 0, Pitch: 180, Yaw: 0 + + + Roll: 180, Pitch: 0, Yaw: 225 + + + Roll: 180, Pitch: 0, Yaw: 270 + + + Roll: 180, Pitch: 0, Yaw: 315 + + + Roll: 90, Pitch: 0, Yaw: 0 + + + Roll: 90, Pitch: 0, Yaw: 45 + + + Roll: 90, Pitch: 0, Yaw: 90 + + + Roll: 90, Pitch: 0, Yaw: 135 + + + Roll: 270, Pitch: 0, Yaw: 0 + + + Roll: 270, Pitch: 0, Yaw: 45 + + + Roll: 270, Pitch: 0, Yaw: 90 + + + Roll: 270, Pitch: 0, Yaw: 135 + + + Roll: 0, Pitch: 90, Yaw: 0 + + + Roll: 0, Pitch: 270, Yaw: 0 + + + Roll: 0, Pitch: 180, Yaw: 90 + + + Roll: 0, Pitch: 180, Yaw: 270 + + + Roll: 90, Pitch: 90, Yaw: 0 + + + Roll: 180, Pitch: 90, Yaw: 0 + + + Roll: 270, Pitch: 90, Yaw: 0 + + + Roll: 90, Pitch: 180, Yaw: 0 + + + Roll: 270, Pitch: 180, Yaw: 0 + + + Roll: 90, Pitch: 270, Yaw: 0 + + + Roll: 180, Pitch: 270, Yaw: 0 + + + Roll: 270, Pitch: 270, Yaw: 0 + + + Roll: 90, Pitch: 180, Yaw: 90 + + + Roll: 90, Pitch: 0, Yaw: 270 + + + Roll: 90, Pitch: 68, Yaw: 293 + + + Pitch: 315 + + + Roll: 90, Pitch: 315 + + + Custom orientation + + + + Bitmask of (optional) autopilot capabilities (64 bit). If a bit is set, the autopilot supports this capability. + + Autopilot supports MISSION float message type. + + + Autopilot supports the new param float message type. + + + This flag must always be set if missions are supported, because missions must always use MISSION_ITEM_INT (rather than MISSION_ITEM, which is deprecated). + Autopilot supports MISSION_ITEM_INT scaled integer message type. + + + Autopilot supports COMMAND_INT scaled integer message type. + + + Autopilot supports the new param union message type. + + + Autopilot supports the File Transfer Protocol v1: https://mavlink.io/en/services/ftp.html. + + + Autopilot supports commanding attitude offboard. + + + Autopilot supports commanding position and velocity targets in local NED frame. + + + Autopilot supports commanding position and velocity targets in global scaled integers. + + + Autopilot supports terrain protocol / data handling. + + + Autopilot supports direct actuator control. + + + Autopilot supports the MAV_CMD_DO_FLIGHTTERMINATION command (flight termination). + + + Autopilot supports onboard compass calibration. + + + Autopilot supports MAVLink version 2. + + + Autopilot supports mission fence protocol. + + + Autopilot supports mission rally point protocol. + + + Autopilot supports the flight information protocol. + + + + Type of mission items being requested/sent in mission protocol. + + Items are mission commands for main mission. + + + Specifies GeoFence area(s). Items are MAV_CMD_NAV_FENCE_ GeoFence items. + + + Specifies the rally points for the vehicle. Rally points are alternative RTL points. Items are MAV_CMD_NAV_RALLY_POINT rally point items. + + + Only used in MISSION_CLEAR_ALL to clear all mission types. + + + + Enumeration of estimator types + + Unknown type of the estimator. + + + This is a naive estimator without any real covariance feedback. + + + Computer vision based estimate. Might be up to scale. + + + Visual-inertial estimate. + + + Plain GPS estimate. + + + Estimator integrating GPS and inertial sensing. + + + Estimate from external motion capturing system. + + + Estimator based on lidar sensor input. + + + Estimator on autopilot. + + + + Enumeration of battery types + + Not specified. + + + Lithium polymer battery + + + Lithium-iron-phosphate battery + + + Lithium-ION battery + + + Nickel metal hydride battery + + + + Enumeration of battery functions + + Battery function is unknown + + + Battery supports all flight systems + + + Battery for the propulsion system + + + Avionics battery + + + Payload battery + + + + Enumeration for battery charge states. + + Low battery state is not provided + + + Battery is not in low state. Normal operation. + + + Battery state is low, warn and monitor close. + + + Battery state is critical, return or abort immediately. + + + Battery state is too low for ordinary abort sequence. Perform fastest possible emergency stop to prevent damage. + + + Battery failed, damage unavoidable. Possible causes (faults) are listed in MAV_BATTERY_FAULT. + + + Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in MAV_BATTERY_FAULT. + + + Battery is charging. + + + + Battery mode. Note, the normal operation mode (i.e. when flying) should be reported as MAV_BATTERY_MODE_UNKNOWN to allow message trimming in normal flight. + + Battery mode not supported/unknown battery mode/normal operation. + + + Battery is auto discharging (towards storage level). + + + Battery in hot-swap mode (current limited to prevent spikes that might damage sensitive electrical circuits). + + + + Smart battery supply status/fault flags (bitmask) for health indication. The battery must also report either MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY if any of these are set. + + Battery has deep discharged. + + + Voltage spikes. + + + One or more cells have failed. Battery should also report MAV_BATTERY_CHARGE_STATE_FAILE (and should not be used). + + + Over-current fault. + + + Over-temperature fault. + + + Under-temperature fault. + + + Vehicle voltage is not compatible with this battery (batteries on same power rail should have similar voltage). + + + Battery firmware is not compatible with current autopilot firmware. + + + Battery is not compatible due to cell configuration (e.g. 5s1p when vehicle requires 6s). + + + + Flags to report status/failure cases for a power generator (used in GENERATOR_STATUS). Note that FAULTS are conditions that cause the generator to fail. Warnings are conditions that require attention before the next use (they indicate the system is not operating properly). + + Generator is off. + + + Generator is ready to start generating power. + + + Generator is generating power. + + + Generator is charging the batteries (generating enough power to charge and provide the load). + + + Generator is operating at a reduced maximum power. + + + Generator is providing the maximum output. + + + Generator is near the maximum operating temperature, cooling is insufficient. + + + Generator hit the maximum operating temperature and shutdown. + + + Power electronics are near the maximum operating temperature, cooling is insufficient. + + + Power electronics hit the maximum operating temperature and shutdown. + + + Power electronics experienced a fault and shutdown. + + + The power source supplying the generator failed e.g. mechanical generator stopped, tether is no longer providing power, solar cell is in shade, hydrogen reaction no longer happening. + + + Generator controller having communication problems. + + + Power electronic or generator cooling system error. + + + Generator controller power rail experienced a fault. + + + Generator controller exceeded the overcurrent threshold and shutdown to prevent damage. + + + Generator controller detected a high current going into the batteries and shutdown to prevent battery damage. + + + Generator controller exceeded it's overvoltage threshold and shutdown to prevent it exceeding the voltage rating. + + + Batteries are under voltage (generator will not start). + + + Generator start is inhibited by e.g. a safety switch. + + + Generator requires maintenance. + + + Generator is not ready to generate yet. + + + Generator is idle. + + + + Enumeration of VTOL states + + MAV is not configured as VTOL + + + VTOL is in transition from multicopter to fixed-wing + + + VTOL is in transition from fixed-wing to multicopter + + + VTOL is in multicopter state + + + VTOL is in fixed-wing state + + + + Enumeration of landed detector states + + MAV landed state is unknown + + + MAV is landed (on ground) + + + MAV is in air + + + MAV currently taking off + + + MAV currently landing + + + + Enumeration of the ADSB altimeter types + + Altitude reported from a Baro source using QNH reference + + + Altitude reported from a GNSS source + + + + ADSB classification for the type of vehicle emitting the transponder signal + + + + + + + + + + + + + + + + + + + + + + + These flags indicate status such as data validity of each data source. Set = data valid + + + + + + + + + + + + + Bitmap of options for the MAV_CMD_DO_REPOSITION + + The aircraft should immediately transition into guided. This should not be set for follow me applications + + + + + Flags in ESTIMATOR_STATUS message + + True if the attitude estimate is good + + + True if the horizontal velocity estimate is good + + + True if the vertical velocity estimate is good + + + True if the horizontal position (relative) estimate is good + + + True if the horizontal position (absolute) estimate is good + + + True if the vertical position (absolute) estimate is good + + + True if the vertical position (above ground) estimate is good + + + True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) + + + True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate + + + True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate + + + True if the EKF has detected a GPS glitch + + + True if the EKF has detected bad accelerometer data + + + + + Sequence that motors are tested when using MAV_CMD_DO_MOTOR_TEST. + + Default autopilot motor test method. + + + Motor numbers are specified as their index in a predefined vehicle-specific sequence. + + + Motor numbers are specified as the output as labeled on the board. + + + + + Defines how throttle value is represented in MAV_CMD_DO_MOTOR_TEST. + + Throttle as a percentage (0 ~ 100) + + + Throttle as an absolute PWM value (normally in range of 1000~2000). + + + Throttle pass-through from pilot's transmitter. + + + Per-motor compass calibration test. + + + + + + ignore altitude field + + + ignore hdop field + + + ignore vdop field + + + ignore horizontal velocity field (vn and ve) + + + ignore vertical velocity field (vd) + + + ignore speed accuracy field + + + ignore horizontal accuracy field + + + ignore vertical accuracy field + + + + Possible actions an aircraft can take to avoid a collision. + + Ignore any potential collisions + + + Report potential collision + + + Ascend or Descend to avoid threat + + + Move horizontally to avoid threat + + + Aircraft to move perpendicular to the collision's velocity vector + + + Aircraft to fly directly back to its launch point + + + Aircraft to stop in place + + + + Aircraft-rated danger from this threat. + + Not a threat + + + Craft is mildly concerned about this threat + + + Craft is panicking, and may take actions to avoid threat + + + + Source of information about this collision. + + ID field references ADSB_VEHICLE packets + + + ID field references MAVLink SRC ID + + + + Type of GPS fix + + No GPS connected + + + No position information, GPS is connected + + + 2D position + + + 3D position + + + DGPS/SBAS aided 3D position + + + RTK float, 3D position + + + RTK Fixed, 3D position + + + Static fixed, typically used for base stations + + + PPP, 3D position. + + + + RTK GPS baseline coordinate system, used for RTK corrections + + Earth-centered, Earth-fixed + + + RTK basestation centered, north, east, down + + + + Type of landing target + + Landing target signaled by light beacon (ex: IR-LOCK) + + + Landing target signaled by radio beacon (ex: ILS, NDB) + + + Landing target represented by a fiducial marker (ex: ARTag) + + + Landing target represented by a pre-defined visual shape/feature (ex: X-marker, H-marker, square) + + + + Direction of VTOL transition + + Respect the heading configuration of the vehicle. + + + Use the heading pointing towards the next waypoint. + + + Use the heading on takeoff (while sitting on the ground). + + + Use the specified heading in parameter 4. + + + Use the current heading when reaching takeoff altitude (potentially facing the wind when weather-vaning is active). + + + + Camera capability flags (Bitmap) + + Camera is able to record video + + + Camera is able to capture images + + + Camera has separate Video and Image/Photo modes (MAV_CMD_SET_CAMERA_MODE) + + + Camera can capture images while in video mode + + + Camera can capture videos while in Photo/Image mode + + + Camera has image survey mode (MAV_CMD_SET_CAMERA_MODE) + + + Camera has basic zoom control (MAV_CMD_SET_CAMERA_ZOOM) + + + Camera has basic focus control (MAV_CMD_SET_CAMERA_FOCUS) + + + Camera has video streaming capabilities (request VIDEO_STREAM_INFORMATION with MAV_CMD_REQUEST_MESSAGE for video streaming info) + + + Camera supports tracking of a point on the camera view. + + + Camera supports tracking of a selection rectangle on the camera view. + + + Camera supports tracking geo status (CAMERA_TRACKING_GEO_STATUS). + + + + Stream status flags (Bitmap) + + Stream is active (running) + + + Stream is thermal imaging + + + + Video stream types + + Stream is RTSP + + + Stream is RTP UDP (URI gives the port number) + + + Stream is MPEG on TCP + + + Stream is h.264 on MPEG TS (URI gives the port number) + + + + Camera tracking status flags + + Camera is not tracking + + + Camera is tracking + + + Camera tracking in error state + + + + Camera tracking modes + + Not tracking + + + Target is a point + + + Target is a rectangle + + + + Camera tracking target data (shows where tracked target is within image) + + No target data + + + Target data embedded in image data (proprietary) + + + Target data rendered in image + + + Target data within status message (Point or Rectangle) + + + + Zoom types for MAV_CMD_SET_CAMERA_ZOOM + + Zoom one step increment (-1 for wide, 1 for tele) + + + Continuous zoom up/down until stopped (-1 for wide, 1 for tele, 0 to stop zooming) + + + Zoom value as proportion of full camera range (a percentage value between 0.0 and 100.0) + + + Zoom value/variable focal length in millimetres. Note that there is no message to get the valid zoom range of the camera, so this can type can only be used for cameras where the zoom range is known (implying that this cannot reliably be used in a GCS for an arbitrary camera) + + + + Focus types for MAV_CMD_SET_CAMERA_FOCUS + + Focus one step increment (-1 for focusing in, 1 for focusing out towards infinity). + + + Continuous focus up/down until stopped (-1 for focusing in, 1 for focusing out towards infinity, 0 to stop focusing) + + + Focus value as proportion of full camera focus range (a value between 0.0 and 100.0) + + + Focus value in metres. Note that there is no message to get the valid focus range of the camera, so this can type can only be used for cameras where the range is known (implying that this cannot reliably be used in a GCS for an arbitrary camera). + + + Focus automatically. + + + Single auto focus. Mainly used for still pictures. Usually abbreviated as AF-S. + + + Continuous auto focus. Mainly used for dynamic scenes. Abbreviated as AF-C. + + + + Result from PARAM_EXT_SET message (or a PARAM_SET within a transaction). + + Parameter value ACCEPTED and SET + + + Parameter value UNKNOWN/UNSUPPORTED + + + Parameter failed to set + + + Parameter value received but not yet set/accepted. A subsequent PARAM_ACK_TRANSACTION or PARAM_EXT_ACK with the final result will follow once operation is completed. This is returned immediately for parameters that take longer to set, indicating taht the the parameter was recieved and does not need to be resent. + + + + Camera Modes. + + Camera is in image/photo capture mode. + + + Camera is in video capture mode. + + + Camera is in image survey capture mode. It allows for camera controller to do specific settings for surveys. + + + + + Not a specific reason + + + Authorizer will send the error as string to GCS + + + At least one waypoint have a invalid value + + + Timeout in the authorizer process(in case it depends on network) + + + Airspace of the mission in use by another vehicle, second result parameter can have the waypoint id that caused it to be denied. + + + Weather is not good to fly + + + + RC type + + Spektrum DSM2 + + + Spektrum DSMX + + + + Bitmap to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 9 is set the floats afx afy afz should be interpreted as force instead of acceleration. + + Ignore position x + + + Ignore position y + + + Ignore position z + + + Ignore velocity x + + + Ignore velocity y + + + Ignore velocity z + + + Ignore acceleration x + + + Ignore acceleration y + + + Ignore acceleration z + + + Use force instead of acceleration + + + Ignore yaw + + + Ignore yaw rate + + + + Bitmap to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates that none of the setpoint dimensions should be ignored. + + Ignore body roll rate + + + Ignore body pitch rate + + + Ignore body yaw rate + + + Ignore throttle + + + Ignore attitude + + + + Airborne status of UAS. + + The flight state can't be determined. + + + UAS on ground. + + + UAS airborne. + + + UAS is in an emergency flight state. + + + UAS has no active controls. + + + + Flags for the global position report. + + The field time contains valid data. + + + The field uas_id contains valid data. + + + The fields lat, lon and h_acc contain valid data. + + + The fields alt and v_acc contain valid data. + + + The field relative_alt contains valid data. + + + The fields vx and vy contain valid data. + + + The field vz contains valid data. + + + The fields next_lat, next_lon and next_alt contain valid data. + + + + Precision land modes (used in MAV_CMD_NAV_LAND). + + Normal (non-precision) landing. + + + Use precision landing if beacon detected when land command accepted, otherwise land normally. + + + Use precision landing, searching for beacon if not found when land command accepted (land normally if beacon cannot be found). + + + + + Parachute actions. Trigger release and enable/disable auto-release. + + Disable auto-release of parachute (i.e. release triggered by crash detectors). + + + Enable auto-release of parachute. + + + Release parachute and kill motors. + + + + + + Encoding of payload unknown. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + Registered for STorM32 gimbal controller. + + + + + No type defined. + + + Manufacturer Serial Number (ANSI/CTA-2063 format). + + + CAA (Civil Aviation Authority) registered ID. Format: [ICAO Country Code].[CAA Assigned ID]. + + + UTM (Unmanned Traffic Management) assigned UUID (RFC4122). + + + A 20 byte ID for a specific flight/session. The exact ID type is indicated by the first byte of uas_id and these type values are managed by ICAO. + + + + + No UA (Unmanned Aircraft) type defined. + + + Aeroplane/Airplane. Fixed wing. + + + Helicopter or multirotor. + + + Gyroplane. + + + VTOL (Vertical Take-Off and Landing). Fixed wing aircraft that can take off vertically. + + + Ornithopter. + + + Glider. + + + Kite. + + + Free Balloon. + + + Captive Balloon. + + + Airship. E.g. a blimp. + + + Free Fall/Parachute (unpowered). + + + Rocket. + + + Tethered powered aircraft. + + + Ground Obstacle. + + + Other type of aircraft not listed earlier. + + + + + The status of the (UA) Unmanned Aircraft is undefined. + + + The UA is on the ground. + + + The UA is in the air. + + + The UA is having an emergency. + + + The remote ID system is failing or unreliable in some way. + + + + + The height field is relative to the take-off location. + + + The height field is relative to ground. + + + + + The horizontal accuracy is unknown. + + + The horizontal accuracy is smaller than 10 Nautical Miles. 18.52 km. + + + The horizontal accuracy is smaller than 4 Nautical Miles. 7.408 km. + + + The horizontal accuracy is smaller than 2 Nautical Miles. 3.704 km. + + + The horizontal accuracy is smaller than 1 Nautical Miles. 1.852 km. + + + The horizontal accuracy is smaller than 0.5 Nautical Miles. 926 m. + + + The horizontal accuracy is smaller than 0.3 Nautical Miles. 555.6 m. + + + The horizontal accuracy is smaller than 0.1 Nautical Miles. 185.2 m. + + + The horizontal accuracy is smaller than 0.05 Nautical Miles. 92.6 m. + + + The horizontal accuracy is smaller than 30 meter. + + + The horizontal accuracy is smaller than 10 meter. + + + The horizontal accuracy is smaller than 3 meter. + + + The horizontal accuracy is smaller than 1 meter. + + + + + The vertical accuracy is unknown. + + + The vertical accuracy is smaller than 150 meter. + + + The vertical accuracy is smaller than 45 meter. + + + The vertical accuracy is smaller than 25 meter. + + + The vertical accuracy is smaller than 10 meter. + + + The vertical accuracy is smaller than 3 meter. + + + The vertical accuracy is smaller than 1 meter. + + + + + The speed accuracy is unknown. + + + The speed accuracy is smaller than 10 meters per second. + + + The speed accuracy is smaller than 3 meters per second. + + + The speed accuracy is smaller than 1 meters per second. + + + The speed accuracy is smaller than 0.3 meters per second. + + + + + The timestamp accuracy is unknown. + + + The timestamp accuracy is smaller than or equal to 0.1 second. + + + The timestamp accuracy is smaller than or equal to 0.2 second. + + + The timestamp accuracy is smaller than or equal to 0.3 second. + + + The timestamp accuracy is smaller than or equal to 0.4 second. + + + The timestamp accuracy is smaller than or equal to 0.5 second. + + + The timestamp accuracy is smaller than or equal to 0.6 second. + + + The timestamp accuracy is smaller than or equal to 0.7 second. + + + The timestamp accuracy is smaller than or equal to 0.8 second. + + + The timestamp accuracy is smaller than or equal to 0.9 second. + + + The timestamp accuracy is smaller than or equal to 1.0 second. + + + The timestamp accuracy is smaller than or equal to 1.1 second. + + + The timestamp accuracy is smaller than or equal to 1.2 second. + + + The timestamp accuracy is smaller than or equal to 1.3 second. + + + The timestamp accuracy is smaller than or equal to 1.4 second. + + + The timestamp accuracy is smaller than or equal to 1.5 second. + + + + + No authentication type is specified. + + + Signature for the UAS (Unmanned Aircraft System) ID. + + + Signature for the Operator ID. + + + Signature for the entire message set. + + + Authentication is provided by Network Remote ID. + + + The exact authentication type is indicated by the first byte of authentication_data and these type values are managed by ICAO. + + + + + Free-form text description of the purpose of the flight. + + + Optional additional clarification when status == MAV_ODID_STATUS_EMERGENCY. + + + Optional additional clarification when status != MAV_ODID_STATUS_EMERGENCY. + + + + + The location of the operator is the same as the take-off location. + + + The location of the operator is based on live GNSS data. + + + The location of the operator is a fixed location. + + + + + The classification type for the UA is undeclared. + + + The classification type for the UA follows EU (European Union) specifications. + + + + + The category for the UA, according to the EU specification, is undeclared. + + + The category for the UA, according to the EU specification, is the Open category. + + + The category for the UA, according to the EU specification, is the Specific category. + + + The category for the UA, according to the EU specification, is the Certified category. + + + + + The class for the UA, according to the EU specification, is undeclared. + + + The class for the UA, according to the EU specification, is Class 0. + + + The class for the UA, according to the EU specification, is Class 1. + + + The class for the UA, according to the EU specification, is Class 2. + + + The class for the UA, according to the EU specification, is Class 3. + + + The class for the UA, according to the EU specification, is Class 4. + + + The class for the UA, according to the EU specification, is Class 5. + + + The class for the UA, according to the EU specification, is Class 6. + + + + + CAA (Civil Aviation Authority) registered operator ID. + + + + + Passing arming checks. + + + Generic arming failure, see error string for details. + + + + + Type of AIS vessel, enum duplicated from AIS standard, https://gpsd.gitlab.io/gpsd/AIVDM.html + + Not available (default). + + + + + + + + + + + + + + + + + + + + + + Wing In Ground effect. + + + + + + + + + + + + + + Towing: length exceeds 200m or breadth exceeds 25m. + + + Dredging or other underwater ops. + + + + + + + + + High Speed Craft. + + + + + + + + + + + + + Search And Rescue vessel. + + + + + Anti-pollution equipment. + + + + + + + Noncombatant ship according to RR Resolution No. 18. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Navigational status of AIS vessel, enum duplicated from AIS standard, https://gpsd.gitlab.io/gpsd/AIVDM.html + + Under way using engine. + + + + + + + + + + + + + + + + Search And Rescue Transponder. + + + Not available (default). + + + + + These flags are used in the AIS_VESSEL.fields bitmask to indicate validity of data in the other message fields. When set, the data is valid. + + 1 = Position accuracy less than 10m, 0 = position accuracy greater than 10m. + + + + + 1 = Velocity over 52.5765m/s (102.2 knots) + + + + Only the sign of the returned turn rate value is valid, either greater than 5deg/30s or less than -5deg/30s + + + + Distance to bow is larger than 511m + + + Distance to stern is larger than 511m + + + Distance to port side is larger than 63m + + + Distance to starboard side is larger than 63m + + + + + + Winch status flags used in WINCH_STATUS + + Winch is healthy + + + Winch thread is fully retracted + + + Winch motor is moving + + + Winch clutch is engaged allowing motor to move freely + + + + + + + + + + + + + + + + + + + + Default autopilot landing behaviour. + + + Use a fixed wing spiral desent approach before landing. + + + Use a fixed wing approach before detransitioning and landing vertically. + + + + + States of the mission state machine. + Note that these states are independent of whether the mission is in a mode that can execute mission items or not (is suspended). + They may not all be relevant on all vehicles. + + + The mission status reporting is not supported. + + + No mission on the vehicle. + + + Mission has not started. This is the case after a mission has uploaded but not yet started executing. + + + Mission is active, and will execute mission items when in auto mode. + + + Mission is paused when in auto mode. + + + Mission has executed all mission items. + + + + + + The general system state. If the system is following the MAVLink standard, the system state is mainly defined by three orthogonal states/modes: The system mode, which is either LOCKED (motors shut down and locked), MANUAL (system under RC control), GUIDED (system with autonomous position control, position setpoint controlled manually) or AUTO (system guided by path/waypoint planner). The NAV_MODE defined the current flight state: LIFTOFF (often an open-loop maneuver), LANDING, WAYPOINTS or VECTOR. This represents the internal navigation state machine. The system status shows whether the system is currently active or not and if an emergency occurred. During the CRITICAL and EMERGENCY states the MAV is still considered to be active, but should start emergency procedures autonomously. After a failure occurred it should first move from active to critical to allow manual intervention and then move to emergency after a certain timeout. + Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. + Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. + Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy. + Maximum usage in percent of the mainloop time. Values: [0-1000] - should always be below 1000 + Battery voltage, UINT16_MAX: Voltage not sent by autopilot + Battery current, -1: Current not sent by autopilot + Battery energy remaining, -1: Battery remaining energy not sent by autopilot + Communication drop rate, (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + Autopilot-specific errors + Autopilot-specific errors + Autopilot-specific errors + Autopilot-specific errors + + + The system time is the time of the master clock, typically the computer clock of the main onboard computer. + Timestamp (UNIX epoch time). + Timestamp (time since system boot). + + + to be removed / merged with SYSTEM_TIME + A ping message either requesting or responding to a ping. This allows to measure the system latencies, including serial port, radio modem and UDP connections. The ping microservice is documented at https://mavlink.io/en/services/ping.html + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + PING sequence + 0: request ping from all receiving systems. If greater than 0: message is a ping response and number is the system id of the requesting system + 0: request ping from all receiving components. If greater than 0: message is a ping response and number is the component id of the requesting component. + + + Request to control this MAV + System the GCS requests control for + 0: request control of this MAV, 1: Release control of this MAV + 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + + + Accept / deny control of this MAV + ID of the GCS this message + 0: request control of this MAV, 1: Release control of this MAV + 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + + + Emit an encrypted signature / key identifying this system. PLEASE NOTE: This protocol has been kept simple, so transmitting the key requires an encrypted channel for true safety. + key + + + Use COMMAND_LONG with MAV_CMD_DO_SET_MODE instead + Set the system mode, as defined by enum MAV_MODE. There is no target component id as the mode is by definition for the overall aircraft, not only for one component. + The system setting the mode + The new base mode. + The new autopilot-specific mode. This field can be ignored by an autopilot. + + + + + Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also https://mavlink.io/en/services/parameter.html for a full documentation of QGroundControl and IMU code. + System ID + Component ID + Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) + + + Request all parameters of this component. After this request, all parameters are emitted. The parameter microservice is documented at https://mavlink.io/en/services/parameter.html + System ID + Component ID + + + Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout. The parameter microservice is documented at https://mavlink.io/en/services/parameter.html + Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Onboard parameter value + Onboard parameter type. + Total number of onboard parameters + Index of this onboard parameter + + + Set a parameter value (write new value to permanent storage). + The receiving component should acknowledge the new parameter value by broadcasting a PARAM_VALUE message (broadcasting ensures that multiple GCS all have an up-to-date list of all parameters). If the sending GCS did not receive a PARAM_VALUE within its timeout time, it should re-send the PARAM_SET message. The parameter microservice is documented at https://mavlink.io/en/services/parameter.html. + + System ID + Component ID + Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Onboard parameter value + Onboard parameter type. + + + The global position, as returned by the Global Positioning System (GPS). This is + NOT the global position estimate of the system, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + GPS fix type. + Latitude (WGS84, EGM96 ellipsoid) + Longitude (WGS84, EGM96 ellipsoid) + Altitude (MSL). Positive for up. Note that virtually all GPS modules provide the MSL altitude in addition to the WGS84 altitude. + GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX + GPS ground speed. If unknown, set to: UINT16_MAX + Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + Number of satellites visible. If unknown, set to 255 + + Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + Position uncertainty. + Altitude uncertainty. + Speed uncertainty. + Heading / track uncertainty + Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + + + The positioning status, as reported by GPS. This message is intended to display status information about each satellite visible to the receiver. See message GLOBAL_POSITION for the global position estimate. This message can contain information for up to 20 satellites. + Number of satellites visible + Global satellite ID + 0: Satellite not used, 1: used for localization + Elevation (0: right on top of receiver, 90: on the horizon) of satellite + Direction of satellite, 0: 0 deg, 255: 360 deg. + Signal to noise ratio of satellite + + + The RAW IMU readings for the usual 9DOF sensor setup. This message should contain the scaled values to the described units + Timestamp (time since system boot). + X acceleration + Y acceleration + Z acceleration + Angular speed around X axis + Angular speed around Y axis + Angular speed around Z axis + X Magnetic field + Y Magnetic field + Z Magnetic field + + Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + + + The RAW IMU readings for a 9DOF sensor, which is identified by the id (default IMU1). This message should always contain the true raw values without any scaling to allow data capture and system debugging. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + X acceleration (raw) + Y acceleration (raw) + Z acceleration (raw) + Angular speed around X axis (raw) + Angular speed around Y axis (raw) + Angular speed around Z axis (raw) + X Magnetic field (raw) + Y Magnetic field (raw) + Z Magnetic field (raw) + + Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) + Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + + + The RAW pressure readings for the typical setup of one absolute pressure and one differential pressure sensor. The sensor values should be the raw, UNSCALED ADC values. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Absolute pressure (raw) + Differential pressure 1 (raw, 0 if nonexistent) + Differential pressure 2 (raw, 0 if nonexistent) + Raw Temperature measurement (raw) + + + The pressure readings for the typical setup of one absolute and differential pressure sensor. The units are as specified in each field. + Timestamp (time since system boot). + Absolute pressure + Differential pressure 1 + Absolute pressure temperature + + Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. + + + The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right). + Timestamp (time since system boot). + Roll angle (-pi..+pi) + Pitch angle (-pi..+pi) + Yaw angle (-pi..+pi) + Roll angular speed + Pitch angular speed + Yaw angular speed + + + The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right), expressed as quaternion. Quaternion order is w, x, y, z and a zero rotation would be expressed as (1 0 0 0). + Timestamp (time since system boot). + Quaternion component 1, w (1 in null-rotation) + Quaternion component 2, x (0 in null-rotation) + Quaternion component 3, y (0 in null-rotation) + Quaternion component 4, z (0 in null-rotation) + Roll angular speed + Pitch angular speed + Yaw angular speed + + Rotation offset by which the attitude quaternion and angular speed vector should be rotated for user display (quaternion with [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], send [0, 0, 0, 0] if field not supported). This field is intended for systems in which the reference attitude may change during flight. For example, tailsitters VTOLs rotate their reference attitude by 90 degrees between hover mode and fixed wing mode, thus repr_offset_q is equal to [1, 0, 0, 0] in hover mode and equal to [0.7071, 0, 0.7071, 0] in fixed wing mode. + + + The filtered local position (e.g. fused computer vision and accelerometers). Coordinate frame is right-handed, Z-axis down (aeronautical frame, NED / north-east-down convention) + Timestamp (time since system boot). + X Position + Y Position + Z Position + X Speed + Y Speed + Z Speed + + + The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up). It + is designed as scaled integer message since the resolution of float is not sufficient. + Timestamp (time since system boot). + Latitude, expressed + Longitude, expressed + Altitude (MSL). Note that virtually all GPS modules provide both WGS84 and MSL. + Altitude above ground + Ground X Speed (Latitude, positive north) + Ground Y Speed (Longitude, positive east) + Ground Z Speed (Altitude, positive down) + Vehicle heading (yaw angle), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + + + The scaled values of the RC channels received: (-100%) -10000, (0%) 0, (100%) 10000. Channels that are inactive should be set to UINT16_MAX. + Timestamp (time since system boot). + Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX. + RC channel 1 value scaled. + RC channel 2 value scaled. + RC channel 3 value scaled. + RC channel 4 value scaled. + RC channel 5 value scaled. + RC channel 6 value scaled. + RC channel 7 value scaled. + RC channel 8 value scaled. + Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + + + The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. A value of UINT16_MAX implies the channel is unused. Individual receivers/transmitters might violate this specification. + Timestamp (time since system boot). + Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX. + RC channel 1 value. + RC channel 2 value. + RC channel 3 value. + RC channel 4 value. + RC channel 5 value. + RC channel 6 value. + RC channel 7 value. + RC channel 8 value. + Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + + + Superseded by ACTUATOR_OUTPUT_STATUS. The RAW values of the servo outputs (for RC input from the remote, use the RC_CHANNELS messages). The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX. + Servo output 1 value + Servo output 2 value + Servo output 3 value + Servo output 4 value + Servo output 5 value + Servo output 6 value + Servo output 7 value + Servo output 8 value + + Servo output 9 value + Servo output 10 value + Servo output 11 value + Servo output 12 value + Servo output 13 value + Servo output 14 value + Servo output 15 value + Servo output 16 value + + + Request a partial list of mission items from the system/component. https://mavlink.io/en/services/mission.html. If start and end index are the same, just send one waypoint. + System ID + Component ID + Start index + End index, -1 by default (-1: send list to end). Else a valid index of the list + + Mission type. + + + This message is sent to the MAV to write a partial list. If start index == end index, only one item will be transmitted / updated. If the start index is NOT 0 and above the current list size, this request should be REJECTED! + System ID + Component ID + Start index. Must be smaller / equal to the largest index of the current onboard list. + End index, equal or greater than start index. + + Mission type. + + + + Message encoding a mission item. This message is emitted to announce + the presence of a mission item and to set a mission item on the system. The mission item can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed (NED), global frame is Z-up, right handed (ENU). NaN may be used to indicate an optional/default value (e.g. to use the system's current latitude or yaw rather than a specific value). See also https://mavlink.io/en/services/mission.html. + System ID + Component ID + Sequence + The coordinate system of the waypoint. + The scheduled action for the waypoint. + false:0, true:1 + Autocontinue to next waypoint + PARAM1, see MAV_CMD enum + PARAM2, see MAV_CMD enum + PARAM3, see MAV_CMD enum + PARAM4, see MAV_CMD enum + PARAM5 / local: X coordinate, global: latitude + PARAM6 / local: Y coordinate, global: longitude + PARAM7 / local: Z coordinate, global: altitude (relative or absolute, depending on frame). + + Mission type. + + + A system that gets this request should respond with MISSION_ITEM_INT (as though MISSION_REQUEST_INT was received). + Request the information of the mission item with the sequence number seq. The response of the system to this message should be a MISSION_ITEM message. https://mavlink.io/en/services/mission.html + System ID + Component ID + Sequence + + Mission type. + + + Set the mission item with sequence number seq as current item. This means that the MAV will continue to this mission item on the shortest path (not following the mission items in-between). + System ID + Component ID + Sequence + + + Message that announces the sequence number of the current active mission item. The MAV will fly towards this mission item. + Sequence + + Total number of mission items on vehicle (on last item, sequence == total). If the autopilot stores its home location as part of the mission this will be excluded from the total. 0: Not supported, UINT16_MAX if no mission is present on the vehicle. + Mission state machine state. MISSION_STATE_UNKNOWN if state reporting not supported. + Vehicle is in a mode that can execute mission items or suspended. 0: Unknown, 1: In mission mode, 2: Suspended (not in mission mode). + + + Request the overall list of mission items from the system/component. + System ID + Component ID + + Mission type. + + + This message is emitted as response to MISSION_REQUEST_LIST by the MAV and to initiate a write transaction. The GCS can then request the individual mission item based on the knowledge of the total number of waypoints. + System ID + Component ID + Number of mission items in the sequence + + Mission type. + + + Delete all mission items at once. + System ID + Component ID + + Mission type. + + + A certain mission item has been reached. The system will either hold this position (or circle on the orbit) or (if the autocontinue on the WP was set) continue to the next waypoint. + Sequence + + + Acknowledgment message during waypoint handling. The type field states if this message is a positive ack (type=0) or if an error happened (type=non-zero). + System ID + Component ID + Mission result. + + Mission type. + + + Sets the GPS co-ordinates of the vehicle local origin (0,0,0) position. Vehicle should emit GPS_GLOBAL_ORIGIN irrespective of whether the origin is changed. This enables transform between the local coordinate frame and the global (GPS) coordinate frame, which may be necessary when (for example) indoor and outdoor settings are connected and the MAV should move from in- to outdoor. + System ID + Latitude (WGS84) + Longitude (WGS84) + Altitude (MSL). Positive for up. + + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + + + Publishes the GPS co-ordinates of the vehicle local origin (0,0,0) position. Emitted whenever a new GPS-Local position mapping is requested or set - e.g. following SET_GPS_GLOBAL_ORIGIN message. + Latitude (WGS84) + Longitude (WGS84) + Altitude (MSL). Positive for up. + + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + + + Bind a RC channel to a parameter. The parameter should change according to the RC channel value. + System ID + Component ID + Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored), send -2 to disable any existing map for this rc_channel_index. + Index of parameter RC channel. Not equal to the RC channel id. Typically corresponds to a potentiometer-knob on the RC. + Initial parameter value + Scale, maps the RC range [-1, 1] to a parameter value + Minimum param value. The protocol does not define if this overwrites an onboard minimum value. (Depends on implementation) + Maximum param value. The protocol does not define if this overwrites an onboard maximum value. (Depends on implementation) + + + Request the information of the mission item with the sequence number seq. The response of the system to this message should be a MISSION_ITEM_INT message. https://mavlink.io/en/services/mission.html + System ID + Component ID + Sequence + + Mission type. + + + Set a safety zone (volume), which is defined by two corners of a cube. This message can be used to tell the MAV which setpoints/waypoints to accept and which to reject. Safety areas are often enforced by national or competition regulations. + System ID + Component ID + Coordinate frame. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + x position 1 / Latitude 1 + y position 1 / Longitude 1 + z position 1 / Altitude 1 + x position 2 / Latitude 2 + y position 2 / Longitude 2 + z position 2 / Altitude 2 + + + Read out the safety zone the MAV currently assumes. + Coordinate frame. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + x position 1 / Latitude 1 + y position 1 / Longitude 1 + z position 1 / Altitude 1 + x position 2 / Latitude 2 + y position 2 / Longitude 2 + z position 2 / Altitude 2 + + + The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right), expressed as quaternion. Quaternion order is w, x, y, z and a zero rotation would be expressed as (1 0 0 0). + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + Roll angular speed + Pitch angular speed + Yaw angular speed + Row-major representation of a 3x3 attitude covariance matrix (states: roll, pitch, yaw; first three entries are the first ROW, next three entries are the second row, etc.). If unknown, assign NaN value to first element in the array. + + + The state of the fixed wing navigation and position controller. + Current desired roll + Current desired pitch + Current desired heading + Bearing to current waypoint/target + Distance to active waypoint + Current altitude error + Current airspeed error + Current crosstrack error on x-y plane + + + The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up). It is designed as scaled integer message since the resolution of float is not sufficient. NOTE: This message is intended for onboard networks / companion computers and higher-bandwidth links and optimized for accuracy and completeness. Please use the GLOBAL_POSITION_INT message for a minimal subset. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Class id of the estimator this estimate originated from. + Latitude + Longitude + Altitude in meters above MSL + Altitude above ground + Ground X Speed (Latitude) + Ground Y Speed (Longitude) + Ground Z Speed (Altitude) + Row-major representation of a 6x6 position and velocity 6x6 cross-covariance matrix (states: lat, lon, alt, vx, vy, vz; first six entries are the first ROW, next six entries are the second row, etc.). If unknown, assign NaN value to first element in the array. + + + The filtered local position (e.g. fused computer vision and accelerometers). Coordinate frame is right-handed, Z-axis down (aeronautical frame, NED / north-east-down convention) + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Class id of the estimator this estimate originated from. + X Position + Y Position + Z Position + X Speed + Y Speed + Z Speed + X Acceleration + Y Acceleration + Z Acceleration + Row-major representation of position, velocity and acceleration 9x9 cross-covariance matrix upper right triangle (states: x, y, z, vx, vy, vz, ax, ay, az; first nine entries are the first ROW, next eight entries are the second row, etc.). If unknown, assign NaN value to first element in the array. + + + The PPM values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. A value of UINT16_MAX implies the channel is unused. Individual receivers/transmitters might violate this specification. + Timestamp (time since system boot). + Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + RC channel 1 value. + RC channel 2 value. + RC channel 3 value. + RC channel 4 value. + RC channel 5 value. + RC channel 6 value. + RC channel 7 value. + RC channel 8 value. + RC channel 9 value. + RC channel 10 value. + RC channel 11 value. + RC channel 12 value. + RC channel 13 value. + RC channel 14 value. + RC channel 15 value. + RC channel 16 value. + RC channel 17 value. + RC channel 18 value. + Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + + + Request a data stream. + The target requested to send the message stream. + The target requested to send the message stream. + The ID of the requested data stream + The requested message rate + 1 to start sending, 0 to stop sending. + + + Data stream status information. + The ID of the requested data stream + The message rate + 1 stream is enabled, 0 stream is stopped. + + + This message provides an API for manually controlling the vehicle using standard joystick axes nomenclature, along with a joystick-like input device. Unused axes can be disabled an buttons are also transmit as boolean values of their + The system to be controlled. + X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. + Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. + Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. Positive values are positive thrust, negative values are negative thrust. + R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. + A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. + + + The RAW values of the RC channels sent to the MAV to override info received from the RC radio. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. Note carefully the semantic differences between the first 8 channels and the subsequent channels + System ID + Component ID + RC channel 1 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + RC channel 2 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + RC channel 3 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + RC channel 4 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + RC channel 5 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + RC channel 6 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + RC channel 7 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + RC channel 8 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + + RC channel 9 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + RC channel 10 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + RC channel 11 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + RC channel 12 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + RC channel 13 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + RC channel 14 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + RC channel 15 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + RC channel 16 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + RC channel 17 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + RC channel 18 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + + + Message encoding a mission item. This message is emitted to announce + the presence of a mission item and to set a mission item on the system. The mission item can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed (NED), global frame is Z-up, right handed (ENU). NaN or INT32_MAX may be used in float/integer params (respectively) to indicate optional/default values (e.g. to use the component's current latitude, yaw rather than a specific value). See also https://mavlink.io/en/services/mission.html. + System ID + Component ID + Waypoint ID (sequence number). Starts at zero. Increases monotonically for each waypoint, no gaps in the sequence (0,1,2,3,4). + The coordinate system of the waypoint. + The scheduled action for the waypoint. + false:0, true:1 + Autocontinue to next waypoint + PARAM1, see MAV_CMD enum + PARAM2, see MAV_CMD enum + PARAM3, see MAV_CMD enum + PARAM4, see MAV_CMD enum + PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 + PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + + Mission type. + + + Metrics typically displayed on a HUD for fixed wing aircraft. + Vehicle speed in form appropriate for vehicle type. For standard aircraft this is typically calibrated airspeed (CAS) or indicated airspeed (IAS) - either of which can be used by a pilot to estimate stall speed. + Current ground speed. + Current heading in compass units (0-360, 0=north). + Current throttle setting (0 to 100). + Current altitude (MSL). + Current climb rate. + + + Message encoding a command with parameters as scaled integers. Scaling depends on the actual command value. The command microservice is documented at https://mavlink.io/en/services/command.html + System ID + Component ID + The coordinate system of the COMMAND. + The scheduled action for the mission item. + Not used. + Not used (set 0). + PARAM1, see MAV_CMD enum + PARAM2, see MAV_CMD enum + PARAM3, see MAV_CMD enum + PARAM4, see MAV_CMD enum + PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + PARAM6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7 + PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame). + + + Send a command with up to seven parameters to the MAV. The command microservice is documented at https://mavlink.io/en/services/command.html + System which should execute the command + Component which should execute the command, 0 for all components + Command ID (of command to send). + 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + Parameter 1 (for the specific command). + Parameter 2 (for the specific command). + Parameter 3 (for the specific command). + Parameter 4 (for the specific command). + Parameter 5 (for the specific command). + Parameter 6 (for the specific command). + Parameter 7 (for the specific command). + + + Report status of a command. Includes feedback whether the command was executed. The command microservice is documented at https://mavlink.io/en/services/command.html + Command ID (of acknowledged command). + Result of command. + + Also used as result_param1, it can be set with a enum containing the errors reasons of why the command was denied or the progress percentage or 255 if unknown the progress when result is MAV_RESULT_IN_PROGRESS. + Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. + System which requested the command to be executed + Component which requested the command to be executed + + + Setpoint in roll, pitch, yaw and thrust from the operator + Timestamp (time since system boot). + Desired roll rate + Desired pitch rate + Desired yaw rate + Collective thrust, normalized to 0 .. 1 + Flight mode switch position, 0.. 255 + Override mode switch position, 0.. 255 + + + Sets a desired vehicle attitude. Used by an external controller to command the vehicle (manual controller or other system). + Timestamp (time since system boot). + System ID + Component ID + Bitmap to indicate which dimensions should be ignored by the vehicle. + Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + Body roll rate + Body pitch rate + Body yaw rate + Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + + + Reports the current commanded attitude of the vehicle as specified by the autopilot. This should match the commands sent in a SET_ATTITUDE_TARGET message if the vehicle is being controlled this way. + Timestamp (time since system boot). + Bitmap to indicate which dimensions should be ignored by the vehicle. + Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + Body roll rate + Body pitch rate + Body yaw rate + Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + + + Sets a desired vehicle position in a local north-east-down coordinate frame. Used by an external controller to command the vehicle (manual controller or other system). + Timestamp (time since system boot). + System ID + Component ID + Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + Bitmap to indicate which dimensions should be ignored by the vehicle. + X Position in NED frame + Y Position in NED frame + Z Position in NED frame (note, altitude is negative in NED) + X velocity in NED frame + Y velocity in NED frame + Z velocity in NED frame + X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + yaw setpoint + yaw rate setpoint + + + Reports the current commanded vehicle position, velocity, and acceleration as specified by the autopilot. This should match the commands sent in SET_POSITION_TARGET_LOCAL_NED if the vehicle is being controlled this way. + Timestamp (time since system boot). + Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + Bitmap to indicate which dimensions should be ignored by the vehicle. + X Position in NED frame + Y Position in NED frame + Z Position in NED frame (note, altitude is negative in NED) + X velocity in NED frame + Y velocity in NED frame + Z velocity in NED frame + X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + yaw setpoint + yaw rate setpoint + + + Sets a desired vehicle position, velocity, and/or acceleration in a global coordinate system (WGS84). Used by an external controller to command the vehicle (manual controller or other system). + Timestamp (time since system boot). The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + System ID + Component ID + Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + Bitmap to indicate which dimensions should be ignored by the vehicle. + X Position in WGS84 frame + Y Position in WGS84 frame + Altitude (MSL, Relative to home, or AGL - depending on frame) + X velocity in NED frame + Y velocity in NED frame + Z velocity in NED frame + X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + yaw setpoint + yaw rate setpoint + + + Reports the current commanded vehicle position, velocity, and acceleration as specified by the autopilot. This should match the commands sent in SET_POSITION_TARGET_GLOBAL_INT if the vehicle is being controlled this way. + Timestamp (time since system boot). The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + Bitmap to indicate which dimensions should be ignored by the vehicle. + X Position in WGS84 frame + Y Position in WGS84 frame + Altitude (MSL, AGL or relative to home altitude, depending on frame) + X velocity in NED frame + Y velocity in NED frame + Z velocity in NED frame + X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + yaw setpoint + yaw rate setpoint + + + The offset in X, Y, Z and yaw between the LOCAL_POSITION_NED messages of MAV X and the global coordinate frame in NED coordinates. Coordinate frame is right-handed, Z-axis down (aeronautical frame, NED / north-east-down convention) + Timestamp (time since system boot). + X Position + Y Position + Z Position + Roll + Pitch + Yaw + + + Suffers from missing airspeed fields and singularities due to Euler angles + Sent from simulation to autopilot. This packet is useful for high throughput applications such as hardware in the loop simulations. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Roll angle + Pitch angle + Yaw angle + Body frame roll / phi angular speed + Body frame pitch / theta angular speed + Body frame yaw / psi angular speed + Latitude + Longitude + Altitude + Ground X Speed (Latitude) + Ground Y Speed (Longitude) + Ground Z Speed (Altitude) + X acceleration + Y acceleration + Z acceleration + + + Sent from autopilot to simulation. Hardware in the loop control outputs + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Control output -1 .. 1 + Control output -1 .. 1 + Control output -1 .. 1 + Throttle 0 .. 1 + Aux 1, -1 .. 1 + Aux 2, -1 .. 1 + Aux 3, -1 .. 1 + Aux 4, -1 .. 1 + System mode. + Navigation mode (MAV_NAV_MODE) + + + Sent from simulation to autopilot. The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + RC channel 1 value + RC channel 2 value + RC channel 3 value + RC channel 4 value + RC channel 5 value + RC channel 6 value + RC channel 7 value + RC channel 8 value + RC channel 9 value + RC channel 10 value + RC channel 11 value + RC channel 12 value + Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + + + Sent from autopilot to simulation. Hardware in the loop control outputs (replacement for HIL_CONTROLS) + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Control outputs -1 .. 1. Channel assignment depends on the simulated hardware. + System mode. Includes arming state. + Flags as bitfield, 1: indicate simulation using lockstep. + + + Optical flow from a flow sensor (e.g. optical mouse sensor) + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Sensor ID + Flow in x-sensor direction + Flow in y-sensor direction + Flow in x-sensor direction, angular-speed compensated + Flow in y-sensor direction, angular-speed compensated + Optical flow quality / confidence. 0: bad, 255: maximum quality + Ground distance. Positive value: distance known. Negative value: Unknown distance + + Flow rate about X axis + Flow rate about Y axis + + + Global position/attitude estimate from a vision source. + Timestamp (UNIX time or since system boot) + Global X position + Global Y position + Global Z position + Roll angle + Pitch angle + Yaw angle + + Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x_global, y_global, z_global, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + + + Local position/attitude estimate from a vision source. + Timestamp (UNIX time or time since system boot) + Local X position + Local Y position + Local Z position + Roll angle + Pitch angle + Yaw angle + + Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + + + Speed estimate from a vision source. + Timestamp (UNIX time or time since system boot) + Global X speed + Global Y speed + Global Z speed + + Row-major representation of 3x3 linear velocity covariance matrix (states: vx, vy, vz; 1st three entries - 1st row, etc.). If unknown, assign NaN value to first element in the array. + Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + + + Global position estimate from a Vicon motion system source. + Timestamp (UNIX time or time since system boot) + Global X position + Global Y position + Global Z position + Roll angle + Pitch angle + Yaw angle + + Row-major representation of 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + + + The IMU readings in SI units in NED body frame + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + X acceleration + Y acceleration + Z acceleration + Angular speed around X axis + Angular speed around Y axis + Angular speed around Z axis + X Magnetic field + Y Magnetic field + Z Magnetic field + Absolute pressure + Differential pressure + Altitude calculated from pressure + Temperature + Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + + Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) + + + Optical flow from an angular rate flow sensor (e.g. PX4FLOW or mouse sensor) + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Sensor ID + Integration time. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + Flow around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + Flow around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + RH rotation around X axis + RH rotation around Y axis + RH rotation around Z axis + Temperature + Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + Time since the distance was sampled. + Distance to the center of the flow field. Positive value (including zero): distance known. Negative value: Unknown distance. + + + The IMU readings in SI units in NED body frame + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + X acceleration + Y acceleration + Z acceleration + Angular speed around X axis in body frame + Angular speed around Y axis in body frame + Angular speed around Z axis in body frame + X Magnetic field + Y Magnetic field + Z Magnetic field + Absolute pressure + Differential pressure (airspeed) + Altitude calculated from pressure + Temperature + Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature, bit 31: full reset of attitude/position/velocities/etc was performed in sim. + + Sensor ID (zero indexed). Used for multiple sensor inputs + + + Status of simulation environment, if used + True attitude quaternion component 1, w (1 in null-rotation) + True attitude quaternion component 2, x (0 in null-rotation) + True attitude quaternion component 3, y (0 in null-rotation) + True attitude quaternion component 4, z (0 in null-rotation) + Attitude roll expressed as Euler angles, not recommended except for human-readable outputs + Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs + Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs + X acceleration + Y acceleration + Z acceleration + Angular speed around X axis + Angular speed around Y axis + Angular speed around Z axis + Latitude + Longitude + Altitude + Horizontal position standard deviation + Vertical position standard deviation + True velocity in north direction in earth-fixed NED frame + True velocity in east direction in earth-fixed NED frame + True velocity in down direction in earth-fixed NED frame + + + Status generated by radio and injected into MAVLink stream. + Local (message sender) recieved signal strength indication in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + Remote (message receiver) signal strength indication in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + Remaining free transmitter buffer space. + Local background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], 255: invalid/unknown. + Remote background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], 255: invalid/unknown. + Count of radio packet receive errors (since boot). + Count of error corrected radio packets (since boot). + + + File transfer message + Network ID (0 for broadcast) + System ID (0 for broadcast) + Component ID (0 for broadcast) + Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + + + Time synchronization message. + Time sync timestamp 1 + Time sync timestamp 2 + + + Camera-IMU triggering and synchronisation message. + Timestamp for image frame (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Image frame sequence + + + The global position, as returned by the Global Positioning System (GPS). This is + NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + Latitude (WGS84) + Longitude (WGS84) + Altitude (MSL). Positive for up. + GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX + GPS ground speed. If unknown, set to: 65535 + GPS velocity in north direction in earth-fixed NED frame + GPS velocity in east direction in earth-fixed NED frame + GPS velocity in down direction in earth-fixed NED frame + Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: 65535 + Number of satellites visible. If unknown, set to 255 + + GPS ID (zero indexed). Used for multiple GPS inputs + Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north + + + Simulated optical flow from a flow sensor (e.g. PX4FLOW or optical mouse sensor) + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Sensor ID + Integration time. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + RH rotation around X axis + RH rotation around Y axis + RH rotation around Z axis + Temperature + Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + Time since the distance was sampled. + Distance to the center of the flow field. Positive value (including zero): distance known. Negative value: Unknown distance. + + + Sent from simulation to autopilot, avoids in contrast to HIL_STATE singularities. This packet is useful for high throughput applications such as hardware in the loop simulations. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) + Body frame roll / phi angular speed + Body frame pitch / theta angular speed + Body frame yaw / psi angular speed + Latitude + Longitude + Altitude + Ground X Speed (Latitude) + Ground Y Speed (Longitude) + Ground Z Speed (Altitude) + Indicated airspeed + True airspeed + X acceleration + Y acceleration + Z acceleration + + + The RAW IMU readings for secondary 9DOF sensor setup. This message should contain the scaled values to the described units + Timestamp (time since system boot). + X acceleration + Y acceleration + Z acceleration + Angular speed around X axis + Angular speed around Y axis + Angular speed around Z axis + X Magnetic field + Y Magnetic field + Z Magnetic field + + Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + + + Request a list of available logs. On some systems calling this may stop on-board logging until LOG_REQUEST_END is called. If there are no log files available this request shall be answered with one LOG_ENTRY message with id = 0 and num_logs = 0. + System ID + Component ID + First log id (0 for first available) + Last log id (0xffff for last available) + + + Reply to LOG_REQUEST_LIST + Log id + Total number of logs + High log number + UTC timestamp of log since 1970, or 0 if not available + Size of the log (may be approximate) + + + Request a chunk of a log + System ID + Component ID + Log id (from LOG_ENTRY reply) + Offset into the log + Number of bytes + + + Reply to LOG_REQUEST_DATA + Log id (from LOG_ENTRY reply) + Offset into the log + Number of bytes (zero for end of log) + log data + + + Erase all logs + System ID + Component ID + + + Stop log transfer and resume normal logging + System ID + Component ID + + + Data for injecting into the onboard GPS (used for DGPS) + System ID + Component ID + Data length + Raw data (110 is enough for 12 satellites of RTCMv2) + + + Second GPS data. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + GPS fix type. + Latitude (WGS84) + Longitude (WGS84) + Altitude (MSL). Positive for up. + GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX + GPS ground speed. If unknown, set to: UINT16_MAX + Course over ground (NOT heading, but direction of movement): 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + Number of satellites visible. If unknown, set to 255 + Number of DGPS satellites + Age of DGPS info + + Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + Position uncertainty. + Altitude uncertainty. + Speed uncertainty. + Heading / track uncertainty + + + Power supply status + 5V rail voltage. + Servo rail voltage. + Bitmap of power supply status flags. + + + Control a serial port. This can be used for raw access to an onboard serial peripheral such as a GPS or telemetry radio. It is designed to make it possible to update the devices firmware via MAVLink messages or change the devices settings. A message with zero bytes can be used to change just the baudrate. + Serial control device type. + Bitmap of serial control flags. + Timeout for reply data + Baudrate of transfer. Zero means no change. + how many bytes in this transfer + serial data + + + RTK GPS data. Gives information on the relative baseline calculation the GPS is reporting + Time since boot of last baseline message received. + Identification of connected RTK receiver. + GPS Week Number of last baseline + GPS Time of Week of last baseline + GPS-specific health report for RTK data. + Rate of baseline messages being received by GPS + Current number of sats used for RTK calculation. + Coordinate system of baseline + Current baseline in ECEF x or NED north component. + Current baseline in ECEF y or NED east component. + Current baseline in ECEF z or NED down component. + Current estimate of baseline accuracy. + Current number of integer ambiguity hypotheses. + + + RTK GPS data. Gives information on the relative baseline calculation the GPS is reporting + Time since boot of last baseline message received. + Identification of connected RTK receiver. + GPS Week Number of last baseline + GPS Time of Week of last baseline + GPS-specific health report for RTK data. + Rate of baseline messages being received by GPS + Current number of sats used for RTK calculation. + Coordinate system of baseline + Current baseline in ECEF x or NED north component. + Current baseline in ECEF y or NED east component. + Current baseline in ECEF z or NED down component. + Current estimate of baseline accuracy. + Current number of integer ambiguity hypotheses. + + + The RAW IMU readings for 3rd 9DOF sensor setup. This message should contain the scaled values to the described units + Timestamp (time since system boot). + X acceleration + Y acceleration + Z acceleration + Angular speed around X axis + Angular speed around Y axis + Angular speed around Z axis + X Magnetic field + Y Magnetic field + Z Magnetic field + + Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + + + Handshake message to initiate, control and stop image streaming when using the Image Transmission Protocol: https://mavlink.io/en/services/image_transmission.html. + Type of requested/acknowledged data. + total data size (set on ACK only). + Width of a matrix or image. + Height of a matrix or image. + Number of packets being sent (set on ACK only). + Payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only). + JPEG quality. Values: [1-100]. + + + Data packet for images sent using the Image Transmission Protocol: https://mavlink.io/en/services/image_transmission.html. + sequence number (starting with 0 on every transmission) + image data bytes + + + Distance sensor information for an onboard rangefinder. + Timestamp (time since system boot). + Minimum distance the sensor can measure + Maximum distance the sensor can measure + Current distance reading + Type of distance sensor. + Onboard ID of the sensor + Direction the sensor faces. downward-facing: ROTATION_PITCH_270, upward-facing: ROTATION_PITCH_90, backward-facing: ROTATION_PITCH_180, forward-facing: ROTATION_NONE, left-facing: ROTATION_YAW_90, right-facing: ROTATION_YAW_270 + Measurement variance. Max standard deviation is 6cm. 255 if unknown. + + Horizontal Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + Vertical Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. Set it to 0 if invalid." + Signal quality of the sensor. Specific to each sensor type, representing the relation of the signal strength with the target reflectivity, distance, size or aspect, but normalised as a percentage. 0 = unknown/unset signal quality, 1 = invalid signal, 100 = perfect signal. + + + Request for terrain data and terrain status. See terrain protocol docs: https://mavlink.io/en/services/terrain.html + Latitude of SW corner of first grid + Longitude of SW corner of first grid + Grid spacing + Bitmask of requested 4x4 grids (row major 8x7 array of grids, 56 bits) + + + Terrain data sent from GCS. The lat/lon and grid_spacing must be the same as a lat/lon from a TERRAIN_REQUEST. See terrain protocol docs: https://mavlink.io/en/services/terrain.html + Latitude of SW corner of first grid + Longitude of SW corner of first grid + Grid spacing + bit within the terrain request mask + Terrain data MSL + + + Request that the vehicle report terrain height at the given location (expected response is a TERRAIN_REPORT). Used by GCS to check if vehicle has all terrain data needed for a mission. + Latitude + Longitude + + + Streamed from drone to report progress of terrain map download (initiated by TERRAIN_REQUEST), or sent as a response to a TERRAIN_CHECK request. See terrain protocol docs: https://mavlink.io/en/services/terrain.html + Latitude + Longitude + grid spacing (zero if terrain at this location unavailable) + Terrain height MSL + Current vehicle height above lat/lon terrain height + Number of 4x4 terrain blocks waiting to be received or read from disk + Number of 4x4 terrain blocks in memory + + + Barometer readings for 2nd barometer + Timestamp (time since system boot). + Absolute pressure + Differential pressure + Absolute pressure temperature + + Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. + + + Motion capture attitude and position + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + X position (NED) + Y position (NED) + Z position (NED) + + Row-major representation of a pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + + + Set the vehicle attitude and body angular rates. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Actuator group. The "_mlx" indicates this is a multi-instance message and a MAVLink parser should use this field to difference between instances. + System ID + Component ID + Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for single rotation direction motors is 0..1, negative range for reverse direction. Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through mixer to repurpose them as generic outputs. + + + Set the vehicle attitude and body angular rates. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Actuator group. The "_mlx" indicates this is a multi-instance message and a MAVLink parser should use this field to difference between instances. + Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for single rotation direction motors is 0..1, negative range for reverse direction. Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through mixer to repurpose them as generic outputs. + + + The current system altitude. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + This altitude measure is initialized on system boot and monotonic (it is never reset, but represents the local altitude change). The only guarantee on this field is that it will never be reset and is consistent within a flight. The recommended value for this field is the uncorrected barometric altitude at boot time. This altitude will also drift and vary between flights. + This altitude measure is strictly above mean sea level and might be non-monotonic (it might reset on events like GPS lock or when a new QNH value is set). It should be the altitude to which global altitude waypoints are compared to. Note that it is *not* the GPS altitude, however, most GPS modules already output MSL by default and not the WGS84 altitude. + This is the local altitude in the local coordinate frame. It is not the altitude above home, but in reference to the coordinate origin (0, 0, 0). It is up-positive. + This is the altitude above the home position. It resets on each change of the current home position. + This is the altitude above terrain. It might be fed by a terrain database or an altimeter. Values smaller than -1000 should be interpreted as unknown. + This is not the altitude, but the clear space below the system according to the fused clearance estimate. It generally should max out at the maximum range of e.g. the laser altimeter. It is generally a moving target. A negative value indicates no measurement available. + + + The autopilot is requesting a resource (file, binary, other type of data) + Request ID. This ID should be re-used when sending back URI contents + The type of requested URI. 0 = a file via URL. 1 = a UAVCAN binary + The requested unique resource identifier (URI). It is not necessarily a straight domain name (depends on the URI type enum) + The way the autopilot wants to receive the URI. 0 = MAVLink FTP. 1 = binary stream. + The storage path the autopilot wants the URI to be stored in. Will only be valid if the transfer_type has a storage associated (e.g. MAVLink FTP). + + + Barometer readings for 3rd barometer + Timestamp (time since system boot). + Absolute pressure + Differential pressure + Absolute pressure temperature + + Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. + + + Current motion information from a designated system + Timestamp (time since system boot). + bit positions for tracker reporting capabilities (POS = 0, VEL = 1, ACCEL = 2, ATT + RATES = 3) + Latitude (WGS84) + Longitude (WGS84) + Altitude (MSL) + target velocity (0,0,0) for unknown + linear target acceleration (0,0,0) for unknown + (1 0 0 0 for unknown) + (0 0 0 for unknown) + eph epv + button states or switches of a tracker device + + + The smoothed, monotonic system state used to feed the control loops of the system. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + X acceleration in body frame + Y acceleration in body frame + Z acceleration in body frame + X velocity in body frame + Y velocity in body frame + Z velocity in body frame + X position in local frame + Y position in local frame + Z position in local frame + Airspeed, set to -1 if unknown + Variance of body velocity estimate + Variance in local position + The attitude, represented as Quaternion + Angular rate in roll axis + Angular rate in pitch axis + Angular rate in yaw axis + + + Battery information + Battery ID + Function of the battery + Type (chemistry) of the battery + Temperature of the battery. INT16_MAX for unknown temperature. + Battery voltage of cells 1 to 10 (see voltages_ext for cells 11-14). Cells in this field above the valid cell count for this battery should have the UINT16_MAX value. If individual cell voltages are unknown or not measured for this battery, then the overall battery voltage should be filled in cell 0, with all others set to UINT16_MAX. If the voltage of the battery is greater than (UINT16_MAX - 1), then cell 0 should be set to (UINT16_MAX - 1), and cell 1 to the remaining voltage. This can be extended to multiple cells if the total voltage is greater than 2 * (UINT16_MAX - 1). + Battery current, -1: autopilot does not measure the current + Consumed charge, -1: autopilot does not provide consumption estimate + Consumed energy, -1: autopilot does not provide energy consumption estimate + Remaining battery energy. Values: [0-100], -1: autopilot does not estimate the remaining battery. + + Remaining battery time, 0: autopilot does not provide remaining battery time estimate + State for extent of discharge, provided by autopilot for warning or external reactions + Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead. + Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode. + Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported). + + + Version and capability of autopilot software. This should be emitted in response to a request with MAV_CMD_REQUEST_MESSAGE. + Bitmap of capabilities + Firmware version number + Middleware version number + Operating system version number + HW / board version (last 8 bits should be silicon ID, if any). The first 16 bits of this field specify https://github.com/ardupilot/ardupilot/blob/master/Tools/AP_Bootloader/board_types.txt + Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + ID of the board vendor + ID of the product + UID if provided by hardware (see uid2) + + UID if provided by hardware (supersedes the uid field. If this is non-zero, use this field, otherwise use uid) + + + The location of a landing target. See: https://mavlink.io/en/services/landing_target.html + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + The ID of the target if multiple targets are present + Coordinate frame used for following fields. + X-axis angular offset of the target from the center of the image + Y-axis angular offset of the target from the center of the image + Distance to the target from the vehicle + Size of target along x-axis + Size of target along y-axis + + X Position of the landing target in MAV_FRAME + Y Position of the landing target in MAV_FRAME + Z Position of the landing target in MAV_FRAME + Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + Type of landing target + Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid). + + + + Status of geo-fencing. Sent in extended status stream when fencing enabled. + Breach status (0 if currently inside fence, 1 if outside). + Number of fence breaches. + Last breach type. + Time (since boot) of last breach. + + Active action to prevent fence breach + + + + + Reports results of completed compass calibration. Sent until MAG_CAL_ACK received. + Compass being calibrated. + Bitmask of compasses being calibrated. + Calibration Status. + 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters. + RMS milligauss residuals. + X offset. + Y offset. + Z offset. + X diagonal (matrix 11). + Y diagonal (matrix 22). + Z diagonal (matrix 33). + X off-diagonal (matrix 12 and 21). + Y off-diagonal (matrix 13 and 31). + Z off-diagonal (matrix 32 and 23). + + Confidence in orientation (higher is better). + orientation before calibration. + orientation after calibration. + field radius correction factor + + + + EFI status output + EFI health status + ECU index + RPM + Fuel consumed + Fuel flow rate + Engine load + Throttle position + Spark dwell time + Barometric pressure + Intake manifold pressure( + Intake manifold temperature + Cylinder head temperature + Ignition timing (Crank angle degrees) + Injection time + Exhaust gas temperature + Output throttle + Pressure/temperature compensation + + Supply voltage to EFI sparking system. Zero in this value means "unknown", so if the supply voltage really is zero volts use 0.0001 instead. + Fuel pressure. Zero in this value means "unknown", so if the fuel pressure really is zero kPa use 0.0001 instead. + + + + Estimator status message including flags, innovation test ratios and estimated accuracies. The flags message is an integer bitmask containing information on which EKF outputs are valid. See the ESTIMATOR_STATUS_FLAGS enum definition for further information. The innovation test ratios show the magnitude of the sensor innovation divided by the innovation check threshold. Under normal operation the innovation test ratios should be below 0.5 with occasional values up to 1.0. Values greater than 1.0 should be rare under normal operation and indicate that a measurement has been rejected by the filter. The user should be notified if an innovation test ratio greater than 1.0 is recorded. Notifications for values in the range between 0.5 and 1.0 should be optional and controllable by the user. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Bitmap indicating which EKF outputs are valid. + Velocity innovation test ratio + Horizontal position innovation test ratio + Vertical position innovation test ratio + Magnetometer innovation test ratio + Height above terrain innovation test ratio + True airspeed innovation test ratio + Horizontal position 1-STD accuracy relative to the EKF local origin + Vertical position 1-STD accuracy relative to the EKF local origin + + + Wind covariance estimate from vehicle. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Wind in X (NED) direction + Wind in Y (NED) direction + Wind in Z (NED) direction + Variability of the wind in XY. RMS of a 1 Hz lowpassed wind estimate. + Variability of the wind in Z. RMS of a 1 Hz lowpassed wind estimate. + Altitude (MSL) that this measurement was taken at + Horizontal speed 1-STD accuracy + Vertical speed 1-STD accuracy + + + GPS sensor input message. This is a raw sensor value sent by the GPS. This is NOT the global position estimate of the system. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + ID of the GPS for multiple GPS inputs + Bitmap indicating which GPS input flags fields to ignore. All other fields must be provided. + GPS time (from start of GPS week) + GPS week number + 0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK + Latitude (WGS84) + Longitude (WGS84) + Altitude (MSL). Positive for up. + GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX + GPS velocity in north direction in earth-fixed NED frame + GPS velocity in east direction in earth-fixed NED frame + GPS velocity in down direction in earth-fixed NED frame + GPS speed accuracy + GPS horizontal accuracy + GPS vertical accuracy + Number of satellites visible. + + Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north + + + RTCM message for injecting into the onboard GPS (used for DGPS) + LSB: 1 means message is fragmented, next 2 bits are the fragment ID, the remaining 5 bits are used for the sequence ID. Messages are only to be flushed to the GPS when the entire message has been reconstructed on the autopilot. The fragment ID specifies which order the fragments should be assembled into a buffer, while the sequence ID is used to detect a mismatch between different buffers. The buffer is considered fully reconstructed when either all 4 fragments are present, or all the fragments before the first fragment with a non full payload is received. This management is used to ensure that normal GPS operation doesn't corrupt RTCM data, and to recover from a unreliable transport delivery order. + data length + RTCM message (may be fragmented) + + + + Message appropriate for high latency connections like Iridium + Bitmap of enabled system modes. + A bitfield for use for autopilot-specific flags. + The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + roll + pitch + heading + throttle (percentage) + heading setpoint + Latitude + Longitude + Altitude above mean sea level + Altitude setpoint relative to the home position + airspeed + airspeed setpoint + groundspeed + climb rate + Number of satellites visible. If unknown, set to 255 + GPS Fix type. + Remaining battery (percentage) + Autopilot temperature (degrees C) + Air temperature (degrees C) from airspeed sensor + failsafe (each bit represents a failsafe where 0=ok, 1=failsafe active (bit0:RC, bit1:batt, bit2:GPS, bit3:GCS, bit4:fence) + current waypoint number + distance to target + + + Message appropriate for high latency connections like Iridium (version 2) + Timestamp (milliseconds since boot or Unix epoch) + Type of the MAV (quadrotor, helicopter, etc.) + Autopilot type / class. Use MAV_AUTOPILOT_INVALID for components that are not flight controllers. + A bitfield for use for autopilot-specific flags (2 byte version). + Latitude + Longitude + Altitude above mean sea level + Altitude setpoint + Heading + Heading setpoint + Distance to target waypoint or position + Throttle + Airspeed + Airspeed setpoint + Groundspeed + Windspeed + Wind heading + Maximum error horizontal position since last message + Maximum error vertical position since last message + Air temperature from airspeed sensor + Maximum climb rate magnitude since last message + Battery level (-1 if field not provided). + Current waypoint number + Bitmap of failure flags. + Field for custom payload. + Field for custom payload. + Field for custom payload. + + + Vibration levels and accelerometer clipping + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Vibration levels on X-axis + Vibration levels on Y-axis + Vibration levels on Z-axis + first accelerometer clipping count + second accelerometer clipping count + third accelerometer clipping count + + + This message can be requested by sending the MAV_CMD_GET_HOME_POSITION command. The position the system will return to and land on. The position is set automatically by the system during the takeoff in case it was not explicitly set by the operator before or after. The global and local positions encode the position in the respective coordinate frames, while the q parameter encodes the orientation of the surface. Under normal conditions it describes the heading and terrain slope, which can be used by the aircraft to adjust the approach. The approach 3D vector describes the point to which the system should fly in normal flight mode and then perform a landing sequence along the vector. + Latitude (WGS84) + Longitude (WGS84) + Altitude (MSL). Positive for up. + Local X position of this position in the local coordinate frame + Local Y position of this position in the local coordinate frame + Local Z position of this position in the local coordinate frame + World to surface normal and heading transformation of the takeoff position. Used to indicate the heading and slope of the ground + Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + + + This message is being superseded by MAV_CMD_DO_SET_HOME. Using the command protocols allows a GCS to detect setting of the home position has failed. + The position the system will return to and land on. The position is set automatically by the system during the takeoff in case it was not explicitly set by the operator before or after. The global and local positions encode the position in the respective coordinate frames, while the q parameter encodes the orientation of the surface. Under normal conditions it describes the heading and terrain slope, which can be used by the aircraft to adjust the approach. The approach 3D vector describes the point to which the system should fly in normal flight mode and then perform a landing sequence along the vector. + System ID. + Latitude (WGS84) + Longitude (WGS84) + Altitude (MSL). Positive for up. + Local X position of this position in the local coordinate frame + Local Y position of this position in the local coordinate frame + Local Z position of this position in the local coordinate frame + World to surface normal and heading transformation of the takeoff position. Used to indicate the heading and slope of the ground + Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + + + The interval between messages for a particular MAVLink message ID. This message is the response to the MAV_CMD_GET_MESSAGE_INTERVAL command. This interface replaces DATA_STREAM. + The ID of the requested MAVLink message. v1.0 is limited to 254 messages. + The interval between two messages. A value of -1 indicates this stream is disabled, 0 indicates it is not available, > 0 indicates the interval at which it is sent. + + + Provides state for additional features + The VTOL state if applicable. Is set to MAV_VTOL_STATE_UNDEFINED if UAV is not in VTOL configuration. + The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + + + The location and information of an ADSB vehicle + ICAO address + Latitude + Longitude + ADSB altitude type. + Altitude(ASL) + Course over ground + The horizontal velocity + The vertical velocity. Positive is up + The callsign, 8+null + ADSB emitter type. + Time since last communication in seconds + Bitmap to indicate various statuses including valid data fields + Squawk code + + + Information about a potential collision + Collision data source + Unique identifier, domain based on src field + Action that is being taken to avoid this collision + How concerned the aircraft is about this collision + Estimated time until collision occurs + Closest vertical distance between vehicle and object + Closest horizontal distance between vehicle and object + + + Message implementing parts of the V2 payload specs in V1 frames for transitional support. + Network ID (0 for broadcast) + System ID (0 for broadcast) + Component ID (0 for broadcast) + A code that identifies the software component that understands this message (analogous to USB device classes or mime type strings). If this code is less than 32768, it is considered a 'registered' protocol extension and the corresponding entry should be added to https://github.com/mavlink/mavlink/definition_files/extension_message_ids.xml. Software creators can register blocks of message IDs as needed (useful for GCS specific metadata, etc...). Message_types greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + Variable length payload. The length must be encoded in the payload as part of the message_type protocol, e.g. by including the length as payload data, or by terminating the payload data with a non-zero marker. This is required in order to reconstruct zero-terminated payloads that are (or otherwise would be) trimmed by MAVLink 2 empty-byte truncation. The entire content of the payload block is opaque unless you understand the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the MAVLink specification. + + + Send raw controller memory. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Starting address of the debug variables + Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + Memory contents at specified address + + + To debug something using a named 3D vector. + Name + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + x + y + z + + + Send a key-value pair as float. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Timestamp (time since system boot). + Name of the debug variable + Floating point value + + + Send a key-value pair as integer. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Timestamp (time since system boot). + Name of the debug variable + Signed integer value + + + Status text message. These messages are printed in yellow in the COMM console of QGroundControl. WARNING: They consume quite some bandwidth, so use only for important status and error messages. If implemented wisely, these messages are buffered on the MCU and sent only at a limited rate (e.g. 10 Hz). + Severity of status. Relies on the definitions within RFC-5424. + Status text message, without null termination character + + Unique (opaque) identifier for this statustext message. May be used to reassemble a logical long-statustext message from a sequence of chunks. A value of zero indicates this is the only chunk in the sequence and the message can be emitted immediately. + This chunk's sequence number; indexing is from zero. Any null character in the text field is taken to mean this was the last chunk. + + + Send a debug value. The index is used to discriminate between values. These values show up in the plot of QGroundControl as DEBUG N. + Timestamp (time since system boot). + index of debug variable + DEBUG value + + + + Setup a MAVLink2 signing key. If called with secret_key of all zero and zero initial_timestamp will disable signing + system id of the target + component ID of the target + signing key + initial timestamp + + + Report button state change. + Timestamp (time since system boot). + Time of last change of button state. + Bitmap for state of buttons. + + + Control vehicle tone generation (buzzer). + System ID + Component ID + tune in board specific format + + tune extension (appended to tune) + + + Information about a camera. Can be requested with a MAV_CMD_REQUEST_MESSAGE command. + Timestamp (time since system boot). + Name of the camera vendor + Name of the camera model + Version of the camera firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). Use 0 if not known. + Focal length. Use NaN if not known. + Image sensor size horizontal. Use NaN if not known. + Image sensor size vertical. Use NaN if not known. + Horizontal image resolution. Use 0 if not known. + Vertical image resolution. Use 0 if not known. + Reserved for a lens ID. Use 0 if not known. + Bitmap of camera capability flags. + Camera definition version (iteration). Use 0 if not known. + Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). The definition file may be xz compressed, which will be indicated by the file extension .xml.xz (a GCS that implements the protocol must support decompressing the file). The string needs to be zero terminated. Use a zero-length string if not known. + + Gimbal id of a gimbal associated with this camera. This is the component id of the gimbal device, or 1-6 for non mavlink gimbals. Use 0 if no gimbal is associated with the camera. + + + Settings of a camera. Can be requested with a MAV_CMD_REQUEST_MESSAGE command. + Timestamp (time since system boot). + Camera mode + + Current zoom level as a percentage of the full range (0.0 to 100.0, NaN if not known) + Current focus level as a percentage of the full range (0.0 to 100.0, NaN if not known) + + + Information about a storage medium. This message is sent in response to a request with MAV_CMD_REQUEST_MESSAGE and whenever the status of the storage changes (STORAGE_STATUS). Use MAV_CMD_REQUEST_MESSAGE.param2 to indicate the index/id of requested storage: 0 for all, 1 for first, 2 for second, etc. + Timestamp (time since system boot). + Storage ID (1 for first, 2 for second, etc.) + Number of storage devices + Status of storage + Total capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + Used capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + Available storage capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + Read speed. + Write speed. + + Type of storage + Textual storage name to be used in UI (microSD 1, Internal Memory, etc.) This is a NULL terminated string. If it is exactly 32 characters long, add a terminating NULL. If this string is empty, the generic type is shown to the user. + + + Information about the status of a capture. Can be requested with a MAV_CMD_REQUEST_MESSAGE command. + Timestamp (time since system boot). + Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress) + Current status of video capturing (0: idle, 1: capture in progress) + Image capture interval + Time since recording started + Available storage capacity. + + Total number of images captured ('forever', or until reset using MAV_CMD_STORAGE_FORMAT). + + + Information about a captured image. This is emitted every time a message is captured. It may be re-requested using MAV_CMD_REQUEST_MESSAGE, using param2 to indicate the sequence number for the missing image. + Timestamp (time since system boot). + Timestamp (time since UNIX epoch) in UTC. 0 for unknown. + Deprecated/unused. Component IDs are used to differentiate multiple cameras. + Latitude where image was taken + Longitude where capture was taken + Altitude (MSL) where image was taken + Altitude above ground + Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1) + Boolean indicating success (1) or failure (0) while capturing this image. + URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. + + + Information about flight since last arming. + Timestamp (time since system boot). + Timestamp at arming (time since UNIX epoch) in UTC, 0 for unknown + Timestamp at takeoff (time since UNIX epoch) in UTC, 0 for unknown + Universally unique identifier (UUID) of flight, should correspond to name of log files + + + Orientation of a mount + Timestamp (time since system boot). + Roll in global frame (set to NaN for invalid). + Pitch in global frame (set to NaN for invalid). + Yaw relative to vehicle (set to NaN for invalid). + + Yaw in absolute frame relative to Earth's North, north is 0 (set to NaN for invalid). + + + A message containing logged data (see also MAV_CMD_LOGGING_START) + system ID of the target + component ID of the target + sequence number (can wrap) + data length + offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + logged data + + + A message containing logged data which requires a LOGGING_ACK to be sent back + system ID of the target + component ID of the target + sequence number (can wrap) + data length + offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + logged data + + + An ack for a LOGGING_DATA_ACKED message + system ID of the target + component ID of the target + sequence number (must match the one in LOGGING_DATA_ACKED) + + + Information about video stream. It may be requested using MAV_CMD_REQUEST_MESSAGE, where param2 indicates the video stream id: 0 for all streams, 1 for first, 2 for second, etc. + Video Stream ID (1 for first, 2 for second, etc.) + Number of streams available. + Type of stream. + Bitmap of stream status flags. + Frame rate. + Horizontal resolution. + Vertical resolution. + Bit rate. + Video image rotation clockwise. + Horizontal Field of view. + Stream name. + Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to). + + + Information about the status of a video stream. It may be requested using MAV_CMD_REQUEST_MESSAGE. + Video Stream ID (1 for first, 2 for second, etc.) + Bitmap of stream status flags + Frame rate + Horizontal resolution + Vertical resolution + Bit rate + Video image rotation clockwise + Horizontal Field of view + + + Information about the field of view of a camera. Can be requested with a MAV_CMD_REQUEST_MESSAGE command. + Timestamp (time since system boot). + Latitude of camera (INT32_MAX if unknown). + Longitude of camera (INT32_MAX if unknown). + Altitude (MSL) of camera (INT32_MAX if unknown). + Latitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + Longitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + Altitude (MSL) of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + Horizontal field of view (NaN if unknown). + Vertical field of view (NaN if unknown). + + + Camera tracking status, sent while in active tracking. Use MAV_CMD_SET_MESSAGE_INTERVAL to define message interval. + Current tracking status + Current tracking mode + Defines location of target data + Current tracked point x value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is left, 1 is right), NAN if unknown + Current tracked point y value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + Current tracked radius if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is image left, 1 is image right), NAN if unknown + Current tracked rectangle top x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + Current tracked rectangle top y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + Current tracked rectangle bottom x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + Current tracked rectangle bottom y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + + + Camera tracking status, sent while in active tracking. Use MAV_CMD_SET_MESSAGE_INTERVAL to define message interval. + Current tracking status + Latitude of tracked object + Longitude of tracked object + Altitude of tracked object(AMSL, WGS84) + Horizontal accuracy. NAN if unknown + Vertical accuracy. NAN if unknown + North velocity of tracked object. NAN if unknown + East velocity of tracked object. NAN if unknown + Down velocity of tracked object. NAN if unknown + Velocity accuracy. NAN if unknown + Distance between camera and tracked object. NAN if unknown + Heading in radians, in NED. NAN if unknown + Accuracy of heading, in NED. NAN if unknown + + + Information about a high level gimbal manager. This message should be requested by a ground station using MAV_CMD_REQUEST_MESSAGE. + Timestamp (time since system boot). + Bitmap of gimbal capability flags. + Gimbal device ID that this gimbal manager is responsible for. Component ID of gimbal device (or 1-6 for non-MAVLink gimbal). + Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + Minimum pitch angle (positive: up, negative: down) + Maximum pitch angle (positive: up, negative: down) + Minimum yaw angle (positive: to the right, negative: to the left) + Maximum yaw angle (positive: to the right, negative: to the left) + + + Current status about a high level gimbal manager. This message should be broadcast at a low regular rate (e.g. 5Hz). + Timestamp (time since system boot). + High level gimbal manager flags currently applied. + Gimbal device ID that this gimbal manager is responsible for. Component ID of gimbal device (or 1-6 for non-MAVLink gimbal). + System ID of MAVLink component with primary control, 0 for none. + Component ID of MAVLink component with primary control, 0 for none. + System ID of MAVLink component with secondary control, 0 for none. + Component ID of MAVLink component with secondary control, 0 for none. + + + High level message to control a gimbal's attitude. This message is to be sent to the gimbal manager (e.g. from a ground station). Angles and rates can be set to NaN according to use case. + System ID + Component ID + High level gimbal manager flags to use. + Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_MANAGER_FLAGS_YAW_LOCK is set) + X component of angular velocity, positive is rolling to the right, NaN to be ignored. + Y component of angular velocity, positive is pitching up, NaN to be ignored. + Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + + + Information about a low level gimbal. This message should be requested by the gimbal manager or a ground station using MAV_CMD_REQUEST_MESSAGE. The maximum angles and rates are the limits by hardware. However, the limits by software used are likely different/smaller and dependent on mode/settings/etc.. + Timestamp (time since system boot). + Name of the gimbal vendor. + Name of the gimbal model. + Custom name of the gimbal given to it by the user. + Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + UID of gimbal hardware (0 if unknown). + Bitmap of gimbal capability flags. + Bitmap for use for gimbal-specific capability flags. + Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left). NAN if unknown. + Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left). NAN if unknown. + Minimum hardware pitch angle (positive: up, negative: down). NAN if unknown. + Maximum hardware pitch angle (positive: up, negative: down). NAN if unknown. + Minimum hardware yaw angle (positive: to the right, negative: to the left). NAN if unknown. + Maximum hardware yaw angle (positive: to the right, negative: to the left). NAN if unknown. + + This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set to a number between 1-6. If the component ID is separate, this field is not required and must be set to 0. + + + Low level message to control a gimbal device's attitude. + This message is to be sent from the gimbal manager to the gimbal device component. + The quaternion and angular velocities can be set to NaN according to use case. + For the angles encoded in the quaternion and the angular velocities holds: + If the flag GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME is set, then they are relative to the vehicle heading (vehicle frame). + If the flag GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME is set, then they are relative to absolute North (earth frame). + If neither of these flags are set, then (for backwards compatibility) it holds: + If the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, then they are relative to absolute North (earth frame), + else they are relative to the vehicle heading (vehicle frame). + Setting both GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME and GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME is not allowed. + These rules are to ensure backwards compatibility. + New implementations should always set either GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME or GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME. + System ID + Component ID + Low level gimbal flags. + Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description. Set fields to NaN to be ignored. + X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN to be ignored. + Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN to be ignored. + Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN to be ignored. + + + Message reporting the status of a gimbal device. + This message should be broadcast by a gimbal device component at a low regular rate (e.g. 5 Hz). + For the angles encoded in the quaternion and the angular velocities holds: + If the flag GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME is set, then they are relative to the vehicle heading (vehicle frame). + If the flag GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME is set, then they are relative to absolute North (earth frame). + If neither of these flags are set, then (for backwards compatibility) it holds: + If the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, then they are relative to absolute North (earth frame), + else they are relative to the vehicle heading (vehicle frame). + Other conditions of the flags are not allowed. + The quaternion and angular velocities in the other frame can be calculated from delta_yaw and delta_yaw_velocity as + q_earth = q_delta_yaw * q_vehicle and w_earth = w_delta_yaw_velocity + w_vehicle (if not NaN). + If neither the GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME nor the GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME flag is set, + then (for backwards compatibility) the data in the delta_yaw and delta_yaw_velocity fields are to be ignored. + New implementations should always set either GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME or GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME, + and always should set delta_yaw and delta_yaw_velocity either to the proper value or NaN. + System ID + Component ID + Timestamp (time since system boot). + Current gimbal flags set. + Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description. + X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN if unknown. + Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN if unknown. + Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN if unknown. + Failure flags (0 for no failure) + + Yaw angle relating the quaternions in earth and body frames (see message description). NaN if unknown. + Yaw angular velocity relating the angular velocities in earth and body frames (see message description). NaN if unknown. + This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set a number between 1-6. If the component ID is separate, this field is not required and must be set to 0. + + + Low level message containing autopilot state relevant for a gimbal device. This message is to be sent from the autopilot to the gimbal device component. The data of this message are for the gimbal device's estimator corrections, in particular horizon compensation, as well as indicates autopilot control intentions, e.g. feed forward angular control in the z-axis. + System ID + Component ID + Timestamp (time since system boot). + Quaternion components of autopilot attitude: w, x, y, z (1 0 0 0 is the null-rotation, Hamilton convention). + Estimated delay of the attitude data. 0 if unknown. + X Speed in NED (North, East, Down). NAN if unknown. + Y Speed in NED (North, East, Down). NAN if unknown. + Z Speed in NED (North, East, Down). NAN if unknown. + Estimated delay of the speed data. 0 if unknown. + Feed forward Z component of angular velocity (positive: yawing to the right). NaN to be ignored. This is to indicate if the autopilot is actively yawing. + Bitmap indicating which estimator outputs are valid. + The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + + Z component of angular velocity in NED (North, East, Down). NaN if unknown. + + + Set gimbal manager pitch and yaw angles (high rate message). This message is to be sent to the gimbal manager (e.g. from a ground station) and will be ignored by gimbal devices. Angles and rates can be set to NaN according to use case. Use MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW for low-rate adjustments that require confirmation. + System ID + Component ID + High level gimbal manager flags to use. + Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + Pitch angle (positive: up, negative: down, NaN to be ignored). + Yaw angle (positive: to the right, negative: to the left, NaN to be ignored). + Pitch angular rate (positive: up, negative: down, NaN to be ignored). + Yaw angular rate (positive: to the right, negative: to the left, NaN to be ignored). + + + High level message to control a gimbal manually. The angles or angular rates are unitless; the actual rates will depend on internal gimbal manager settings/configuration (e.g. set by parameters). This message is to be sent to the gimbal manager (e.g. from a ground station). Angles and rates can be set to NaN according to use case. + System ID + Component ID + High level gimbal manager flags. + Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + Pitch angle unitless (-1..1, positive: up, negative: down, NaN to be ignored). + Yaw angle unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + Pitch angular rate unitless (-1..1, positive: up, negative: down, NaN to be ignored). + Yaw angular rate unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + + + Configure WiFi AP SSID, password, and mode. This message is re-emitted as an acknowledgement by the AP. The message may also be explicitly requested using MAV_CMD_REQUEST_MESSAGE + Name of Wi-Fi network (SSID). Blank to leave it unchanged when setting. Current SSID when sent back as a response. + Password. Blank for an open AP. MD5 hash when message is sent back as a response. + + + + The location and information of an AIS vessel + Mobile Marine Service Identifier, 9 decimal digits + Latitude + Longitude + Course over ground + True heading + Speed over ground + Turn rate + Navigational status + Type of vessels + Distance from lat/lon location to bow + Distance from lat/lon location to stern + Distance from lat/lon location to port side + Distance from lat/lon location to starboard side + The vessel callsign + The vessel name + Time since last communication in seconds + Bitmask to indicate various statuses including valid data fields + + + + General status information of an UAVCAN node. Please refer to the definition of the UAVCAN message "uavcan.protocol.NodeStatus" for the background information. The UAVCAN specification is available at http://uavcan.org. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Time since the start-up of the node. + Generalized node health status. + Generalized operating mode. + Not used currently. + Vendor-specific status information. + + + General information describing a particular UAVCAN node. Please refer to the definition of the UAVCAN service "uavcan.protocol.GetNodeInfo" for the background information. This message should be emitted by the system whenever a new node appears online, or an existing node reboots. Additionally, it can be emitted upon request from the other end of the MAVLink channel (see MAV_CMD_UAVCAN_GET_NODE_INFO). It is also not prohibited to emit this message unconditionally at a low frequency. The UAVCAN specification is available at http://uavcan.org. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Time since the start-up of the node. + Node name string. For example, "sapog.px4.io". + Hardware major version number. + Hardware minor version number. + Hardware unique 128-bit ID. + Software major version number. + Software minor version number. + Version control system (VCS) revision identifier (e.g. git short commit hash). Zero if unknown. + + + Request to read the value of a parameter with either the param_id string id or param_index. PARAM_EXT_VALUE should be emitted in response. + System ID + Component ID + Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Parameter index. Set to -1 to use the Parameter ID field as identifier (else param_id will be ignored) + + + Request all parameters of this component. All parameters should be emitted in response as PARAM_EXT_VALUE. + System ID + Component ID + + + Emit the value of a parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows them to re-request missing parameters after a loss or timeout. + Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Parameter value + Parameter type. + Total number of parameters + Index of this parameter + + + Set a parameter value. In order to deal with message loss (and retransmission of PARAM_EXT_SET), when setting a parameter value and the new value is the same as the current value, you will immediately get a PARAM_ACK_ACCEPTED response. If the current state is PARAM_ACK_IN_PROGRESS, you will accordingly receive a PARAM_ACK_IN_PROGRESS in response. + System ID + Component ID + Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Parameter value + Parameter type. + + + Response from a PARAM_EXT_SET message. + Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Parameter value (new value if PARAM_ACK_ACCEPTED, current value otherwise) + Parameter type. + Result code. + + + Obstacle distances in front of the sensor, starting from the left in increment degrees to the right + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Class id of the distance sensor type. + Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm. + Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero. + Minimum distance the sensor can measure. + Maximum distance the sensor can measure. + + Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise. + Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise. + Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned. + + + Odometry message to communicate odometry information with an external interface. Fits ROS REP 147 standard for aerial vehicles (http://www.ros.org/reps/rep-0147.html). + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Coordinate frame of reference for the pose data. + Coordinate frame of reference for the velocity in free space (twist) data. + X Position + Y Position + Z Position + Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + X linear speed + Y linear speed + Z linear speed + Roll angular speed + Pitch angular speed + Yaw angular speed + Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + + Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + Type of estimator that is providing the odometry. + Optional odometry quality metric as a percentage. -1 = odometry has failed, 0 = unknown/unset quality, 1 = worst quality, 100 = best quality + + + Status of the Iridium SBD link. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Timestamp of the last successful sbd session. The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Number of failed SBD sessions. + Number of successful SBD sessions. + Signal quality equal to the number of bars displayed on the ISU signal strength indicator. Range is 0 to 5, where 0 indicates no signal and 5 indicates maximum signal strength. + 1: Ring call pending, 0: No call pending. + 1: Transmission session pending, 0: No transmission session pending. + 1: Receiving session pending, 0: No receiving session pending. + + + + + RPM sensor data message. + Index of this RPM sensor (0-indexed) + Indicated rate + + + The global position resulting from GPS and sensor fusion. + Time of applicability of position (microseconds since UNIX epoch). + Unique UAS ID. + Latitude (WGS84) + Longitude (WGS84) + Altitude (WGS84) + Altitude above ground + Ground X speed (latitude, positive north) + Ground Y speed (longitude, positive east) + Ground Z speed (altitude, positive down) + Horizontal position uncertainty (standard deviation) + Altitude uncertainty (standard deviation) + Speed uncertainty (standard deviation) + Next waypoint, latitude (WGS84) + Next waypoint, longitude (WGS84) + Next waypoint, altitude (WGS84) + Time until next update. Set to 0 if unknown or in data driven mode. + Flight state + Bitwise OR combination of the data available flags. + + + Large debug/prototyping array. The message uses the maximum available payload for data. The array_id and name fields are used to discriminate between messages in code and in user interfaces (respectively). Do not use in production code. + Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + Name, for human-friendly display in a Ground Control Station + Unique ID used to discriminate between arrays + + data + + + Smart Battery information (static/infrequent update). Use for updates from: smart battery to flight stack, flight stack to GCS. Use BATTERY_STATUS for smart battery frequent updates. + Battery ID + Function of the battery + Type (chemistry) of the battery + Capacity when full according to manufacturer, -1: field not provided. + Capacity when full (accounting for battery degradation), -1: field not provided. + Charge/discharge cycle count. UINT16_MAX: field not provided. + Serial number in ASCII characters, 0 terminated. All 0: field not provided. + Static device name in ASCII characters, 0 terminated. All 0: field not provided. Encode as manufacturer name then product name separated using an underscore. + Battery weight. 0: field not provided. + Minimum per-cell voltage when discharging. If not supplied set to UINT16_MAX value. + Minimum per-cell voltage when charging. If not supplied set to UINT16_MAX value. + Minimum per-cell voltage when resting. If not supplied set to UINT16_MAX value. + + Maximum per-cell voltage when charged. 0: field not provided. + Number of battery cells in series. 0: field not provided. + Maximum pack discharge current. 0: field not provided. + Maximum pack discharge burst current. 0: field not provided. + Manufacture date (DD/MM/YYYY) in ASCII characters, 0 terminated. All 0: field not provided. + + + Telemetry of power generation system. Alternator or mechanical generator. + Status flags. + Speed of electrical generator or alternator. UINT16_MAX: field not provided. + Current into/out of battery. Positive for out. Negative for in. NaN: field not provided. + Current going to the UAV. If battery current not available this is the DC current from the generator. Positive for out. Negative for in. NaN: field not provided + The power being generated. NaN: field not provided + Voltage of the bus seen at the generator, or battery bus if battery bus is controlled by generator and at a different voltage to main bus. + The temperature of the rectifier or power converter. INT16_MAX: field not provided. + The target battery current. Positive for out. Negative for in. NaN: field not provided + The temperature of the mechanical motor, fuel cell core or generator. INT16_MAX: field not provided. + Seconds this generator has run since it was rebooted. UINT32_MAX: field not provided. + Seconds until this generator requires maintenance. A negative value indicates maintenance is past-due. INT32_MAX: field not provided. + + + The raw values of the actuator outputs (e.g. on Pixhawk, from MAIN, AUX ports). This message supersedes SERVO_OUTPUT_RAW. + Timestamp (since system boot). + Active outputs + Servo / motor output array values. Zero values indicate unused channels. + + + Reports the on/off state of relays, as controlled by MAV_CMD_DO_SET_RELAY. + Timestamp (time since system boot). + Relay states. Relay instance numbers are represented as individual bits in this mask by offset. + Relay present. Relay instance numbers are represented as individual bits in this mask by offset. Bits will be true if a relay instance is configured. + + + Message for transporting "arbitrary" variable-length data from one component to another (broadcast is not forbidden, but discouraged). The encoding of the data is usually extension specific, i.e. determined by the source, and is usually not documented as part of the MAVLink specification. + System ID (can be 0 for broadcast, but this is discouraged) + Component ID (can be 0 for broadcast, but this is discouraged) + A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + Length of the data transported in payload + Variable length payload. The payload length is defined by payload_length. The entire content of this block is opaque unless you understand the encoding specified by payload_type. + + + A forwarded CAN frame as requested by MAV_CMD_CAN_FORWARD. + System ID. + Component ID. + bus number + Frame length + Frame ID + Frame data + + + A forwarded CANFD frame as requested by MAV_CMD_CAN_FORWARD. These are separated from CAN_FRAME as they need different handling (eg. TAO handling) + System ID. + Component ID. + bus number + Frame length + Frame ID + Frame data + + + Modify the filter of what CAN messages to forward over the mavlink. This can be used to make CAN forwarding work well on low bandwith links. The filtering is applied on bits 8 to 24 of the CAN id (2nd and 3rd bytes) which corresponds to the DroneCAN message ID for DroneCAN. Filters with more than 16 IDs can be constructed by sending multiple CAN_FILTER_MODIFY messages. + System ID. + Component ID. + bus number + what operation to perform on the filter list. See CAN_FILTER_OP enum. + number of IDs in filter list + filter IDs, length num_ids + + + + Cumulative distance traveled for each reported wheel. + Timestamp (synced to UNIX time or since system boot). + Number of wheels reported. + Distance reported by individual wheel encoders. Forward rotations increase values, reverse rotations decrease them. Not all wheels will necessarily have wheel encoders; the mapping of encoders to wheel positions must be agreed/understood by the endpoints. + + + Winch status. + Timestamp (synced to UNIX time or since system boot). + Length of line released. NaN if unknown + Speed line is being released or retracted. Positive values if being released, negative values if being retracted, NaN if unknown + Tension on the line. NaN if unknown + Voltage of the battery supplying the winch. NaN if unknown + Current draw from the winch. NaN if unknown + Temperature of the motor. INT16_MAX if unknown + Status flags + + + + + Data for filling the OpenDroneID Basic ID message. This and the below messages are primarily meant for feeding data to/from an OpenDroneID implementation. E.g. https://github.com/opendroneid/opendroneid-core-c. These messages are compatible with the ASTM F3411 Remote ID standard and the ASD-STAN prEN 4709-002 Direct Remote ID standard. Additional information and usage of these messages is documented at https://mavlink.io/en/services/opendroneid.html. + System ID (0 for broadcast). + Component ID (0 for broadcast). + Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + Indicates the format for the uas_id field of this message. + Indicates the type of UA (Unmanned Aircraft). + UAS (Unmanned Aircraft System) ID following the format specified by id_type. Shall be filled with nulls in the unused portion of the field. + + + + + Data for filling the OpenDroneID Location message. The float data types are 32-bit IEEE 754. The Location message provides the location, altitude, direction and speed of the aircraft. + System ID (0 for broadcast). + Component ID (0 for broadcast). + Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + Indicates whether the unmanned aircraft is on the ground or in the air. + Direction over ground (not heading, but direction of movement) measured clockwise from true North: 0 - 35999 centi-degrees. If unknown: 36100 centi-degrees. + Ground speed. Positive only. If unknown: 25500 cm/s. If speed is larger than 25425 cm/s, use 25425 cm/s. + The vertical speed. Up is positive. If unknown: 6300 cm/s. If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s. + Current latitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + Current longitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. + The geodetic altitude as defined by WGS84. If unknown: -1000 m. + Indicates the reference point for the height field. + The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m. + The accuracy of the horizontal position. + The accuracy of the vertical position. + The accuracy of the barometric altitude. + The accuracy of the horizontal and vertical speed. + Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. If unknown: 0xFFFF. + The accuracy of the timestamps. + + + + + Data for filling the OpenDroneID Authentication message. The Authentication Message defines a field that can provide a means of authenticity for the identity of the UAS (Unmanned Aircraft System). The Authentication message can have two different formats. For data page 0, the fields PageCount, Length and TimeStamp are present and AuthData is only 17 bytes. For data page 1 through 15, PageCount, Length and TimeStamp are not present and the size of AuthData is 23 bytes. + System ID (0 for broadcast). + Component ID (0 for broadcast). + Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + Indicates the type of authentication. + Allowed range is 0 - 15. + This field is only present for page 0. Allowed range is 0 - 15. See the description of struct ODID_Auth_data at https://github.com/opendroneid/opendroneid-core-c/blob/master/libopendroneid/opendroneid.h. + This field is only present for page 0. Total bytes of authentication_data from all data pages. See the description of struct ODID_Auth_data at https://github.com/opendroneid/opendroneid-core-c/blob/master/libopendroneid/opendroneid.h. + This field is only present for page 0. 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + Opaque authentication data. For page 0, the size is only 17 bytes. For other pages, the size is 23 bytes. Shall be filled with nulls in the unused portion of the field. + + + + + Data for filling the OpenDroneID Self ID message. The Self ID Message is an opportunity for the operator to (optionally) declare their identity and purpose of the flight. This message can provide additional information that could reduce the threat profile of a UA (Unmanned Aircraft) flying in a particular area or manner. This message can also be used to provide optional additional clarification in an emergency/remote ID system failure situation. + System ID (0 for broadcast). + Component ID (0 for broadcast). + Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + Indicates the type of the description field. + Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + + + + + Data for filling the OpenDroneID System message. The System Message contains general system information including the operator location/altitude and possible aircraft group and/or category/class information. + System ID (0 for broadcast). + Component ID (0 for broadcast). + Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + Specifies the operator location type. + Specifies the classification type of the UA. + Latitude of the operator. If unknown: 0 (both Lat/Lon). + Longitude of the operator. If unknown: 0 (both Lat/Lon). + Number of aircraft in the area, group or formation (default 1). + Radius of the cylindrical area of the group or formation (default 0). + Area Operations Ceiling relative to WGS84. If unknown: -1000 m. + Area Operations Floor relative to WGS84. If unknown: -1000 m. + When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the category of the UA. + When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the class of the UA. + Geodetic altitude of the operator relative to WGS84. If unknown: -1000 m. + 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + + + + + Data for filling the OpenDroneID Operator ID message, which contains the CAA (Civil Aviation Authority) issued operator ID. + System ID (0 for broadcast). + Component ID (0 for broadcast). + Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + Indicates the type of the operator_id field. + Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + + + Status from the transmitter telling the flight controller if the remote ID system is ready for arming. + Status level indicating if arming is allowed. + Text error message, should be empty if status is good to arm. Fill with nulls in unused portion. + + + + + + An OpenDroneID message pack is a container for multiple encoded OpenDroneID messages (i.e. not in the format given for the above message descriptions but after encoding into the compressed OpenDroneID byte format). Used e.g. when transmitting on Bluetooth 5.0 Long Range/Extended Advertising or on WiFi Neighbor Aware Networking or on WiFi Beacon. + System ID (0 for broadcast). + Component ID (0 for broadcast). + Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specificed to have this length. + Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 9. + Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field. + + + Update the data in the OPEN_DRONE_ID_SYSTEM message with new location information. This can be sent to update the location information for the operator when no other information in the SYSTEM message has changed. This message allows for efficient operation on radio links which have limited uplink bandwidth while meeting requirements for update frequency of the operator location. + System ID (0 for broadcast). + Component ID (0 for broadcast). + Latitude of the operator. If unknown: 0 (both Lat/Lon). + Longitude of the operator. If unknown: 0 (both Lat/Lon). + Geodetic altitude of the operator relative to WGS84. If unknown: -1000 m. + 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + + + Temperature and humidity from hygrometer. + Hygrometer ID + Temperature + Humidity + + + diff --git a/bp_mavlink/cubepilot.xml b/bp_mavlink/cubepilot.xml new file mode 100644 index 0000000000000..65b58c8c1865f --- /dev/null +++ b/bp_mavlink/cubepilot.xml @@ -0,0 +1,48 @@ + + + + + + + common.xml + + + Raw RC Data + + + + Information about video stream + Video Stream ID (1 for first, 2 for second, etc.) + Number of streams available. + Frame rate. + Horizontal resolution. + Vertical resolution. + Bit rate. + Video image rotation clockwise. + Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to). + + + Herelink Telemetry + + + + + + + + + + Start firmware update with encapsulated data. + System ID. + Component ID. + FW Size. + FW CRC. + + + offset response to encapsulated data. + System ID. + Component ID. + FW Offset. + + + diff --git a/bp_mavlink/development.xml b/bp_mavlink/development.xml new file mode 100644 index 0000000000000..dbe60ab8b3600 --- /dev/null +++ b/bp_mavlink/development.xml @@ -0,0 +1,42 @@ + + + + standard.xml + 0 + 0 + + + Airspeed sensor flags + + Airspeed sensor is unhealthy + + + True if the data from this sensor is being actively used by the flight controller for guidance, navigation or control. + + + + + + Checksum for the current mission, rally point or geofence plan, or for the "combined" plan (a GCS can use these checksums to determine if it has matching plans). + This message must be broadcast with the appropriate checksum following any change to a mission, geofence or rally point definition + (immediately after the MISSION_ACK that completes the upload sequence). + It may also be requested using MAV_CMD_REQUEST_MESSAGE, where param 2 indicates the plan type for which the checksum is required. + The checksum must be calculated on the autopilot, but may also be calculated by the GCS. + The checksum uses the same CRC32 algorithm as MAVLink FTP (https://mavlink.io/en/services/ftp.html#crc32-implementation). + The checksum for a mission, geofence or rally point definition is run over each item in the plan in seq order (excluding the home location if present in the plan), and covers the following fields (in order): + frame, command, autocontinue, param1, param2, param3, param4, param5, param6, param7. + The checksum for the whole plan (MAV_MISSION_TYPE_ALL) is calculated using the same approach, running over each sub-plan in the following order: mission, geofence then rally point. + + Mission type. + CRC32 checksum of current plan for specified type. + + + Airspeed information from a sensor. + Sensor ID. + Calibrated airspeed (CAS). + Temperature. INT16_MAX for value unknown/not supplied. + Raw differential pressure. NaN for value unknown/not supplied. + Airspeed sensor flags. + + + diff --git a/bp_mavlink/icarous.xml b/bp_mavlink/icarous.xml new file mode 100644 index 0000000000000..7dbdb95ebbe60 --- /dev/null +++ b/bp_mavlink/icarous.xml @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + ICAROUS heartbeat + See the FMS_STATE enum. + + + Kinematic multi bands (track) output from Daidalus + Number of track bands + See the TRACK_BAND_TYPES enum. + min angle (degrees) + max angle (degrees) + See the TRACK_BAND_TYPES enum. + min angle (degrees) + max angle (degrees) + See the TRACK_BAND_TYPES enum. + min angle (degrees) + max angle (degrees) + See the TRACK_BAND_TYPES enum. + min angle (degrees) + max angle (degrees) + See the TRACK_BAND_TYPES enum. + min angle (degrees) + max angle (degrees) + + + diff --git a/bp_mavlink/loweheiser.xml b/bp_mavlink/loweheiser.xml new file mode 100644 index 0000000000000..d899db91ebe0e --- /dev/null +++ b/bp_mavlink/loweheiser.xml @@ -0,0 +1,55 @@ + + + + + + + + minimal.xml + + + + + Set Loweheiser desired states + EFI Index + Desired Engine/EFI State (0: Power Off, 1:Running) + Desired Governor State (0:manual throttle, 1:Governed throttle) + Manual throttle level, 0% - 100% + Electronic Start up (0:Off, 1:On) + Empty + Empty + + + + + + Composite EFI and Governor data from Loweheiser equipment. This message is created by the EFI unit based on its own data and data received from a governor attached to that EFI unit. + + Generator Battery voltage. + Generator Battery current. + Current being produced by generator. + Load current being consumed by the UAV (sum of curr_gen and curr_batt) + Generator fuel remaining in litres. + Throttle Output. + Seconds this generator has run since it was rebooted. + Seconds until this generator requires maintenance. A negative value indicates maintenance is past due. + The Temperature of the rectifier. + The temperature of the mechanical motor, fuel cell core or generator. + + EFI Supply Voltage. + Motor RPM. + Injector pulse-width in miliseconds. + Fuel flow rate in litres/hour. + Fuel consumed. + Atmospheric pressure. + Manifold Air Temperature. + Cylinder Head Temperature. + Throttle Position. + Exhaust gas temperature. + + EFI index. + Generator status. + EFI status. + + + diff --git a/bp_mavlink/matrixpilot.xml b/bp_mavlink/matrixpilot.xml new file mode 100644 index 0000000000000..e3c4996c6fdb7 --- /dev/null +++ b/bp_mavlink/matrixpilot.xml @@ -0,0 +1,349 @@ + + + common.xml + + + + Action required when performing CMD_PREFLIGHT_STORAGE + + Read all parameters from storage + + + Write all parameters to storage + + + Clear all parameters in storage + + + Read specific parameters from storage + + + Write specific parameters to storage + + + Clear specific parameters in storage + + + do nothing + + + + + Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. + Storage action: Action defined by MAV_PREFLIGHT_STORAGE_ACTION_ADVANCED + Storage area as defined by parameter database + Storage flags as defined by parameter database + Empty + Empty + Empty + Empty + + + + + + Depreciated but used as a compiler flag. Do not remove + System ID + Component ID + + + Reqest reading of flexifunction data + System ID + Component ID + Type of flexifunction data requested + index into data where needed + + + Flexifunction type and parameters for component at function index from buffer + System ID + Component ID + Function index + Total count of functions + Address in the flexifunction data, Set to 0xFFFF to use address in target memory + Size of the + Settings data + + + Flexifunction type and parameters for component at function index from buffer + System ID + Component ID + Function index + result of acknowledge, 0=fail, 1=good + + + Acknowldge sucess or failure of a flexifunction command + System ID + Component ID + 0=inputs, 1=outputs + index of first directory entry to write + count of directory entries to write + Settings data + + + Acknowldge sucess or failure of a flexifunction command + System ID + Component ID + 0=inputs, 1=outputs + index of first directory entry to write + count of directory entries to write + result of acknowledge, 0=fail, 1=good + + + Acknowldge sucess or failure of a flexifunction command + System ID + Component ID + Flexifunction command type + + + Acknowldge sucess or failure of a flexifunction command + Command acknowledged + result of acknowledge + + + Backwards compatible MAVLink version of SERIAL_UDB_EXTRA - F2: Format Part A + Serial UDB Extra Time + Serial UDB Extra Status + Serial UDB Extra Latitude + Serial UDB Extra Longitude + Serial UDB Extra Altitude + Serial UDB Extra Waypoint Index + Serial UDB Extra Rmat 0 + Serial UDB Extra Rmat 1 + Serial UDB Extra Rmat 2 + Serial UDB Extra Rmat 3 + Serial UDB Extra Rmat 4 + Serial UDB Extra Rmat 5 + Serial UDB Extra Rmat 6 + Serial UDB Extra Rmat 7 + Serial UDB Extra Rmat 8 + Serial UDB Extra GPS Course Over Ground + Serial UDB Extra Speed Over Ground + Serial UDB Extra CPU Load + Serial UDB Extra 3D IMU Air Speed + Serial UDB Extra Estimated Wind 0 + Serial UDB Extra Estimated Wind 1 + Serial UDB Extra Estimated Wind 2 + Serial UDB Extra Magnetic Field Earth 0 + Serial UDB Extra Magnetic Field Earth 1 + Serial UDB Extra Magnetic Field Earth 2 + Serial UDB Extra Number of Sattelites in View + Serial UDB Extra GPS Horizontal Dilution of Precision + + + Backwards compatible version of SERIAL_UDB_EXTRA - F2: Part B + Serial UDB Extra Time + Serial UDB Extra PWM Input Channel 1 + Serial UDB Extra PWM Input Channel 2 + Serial UDB Extra PWM Input Channel 3 + Serial UDB Extra PWM Input Channel 4 + Serial UDB Extra PWM Input Channel 5 + Serial UDB Extra PWM Input Channel 6 + Serial UDB Extra PWM Input Channel 7 + Serial UDB Extra PWM Input Channel 8 + Serial UDB Extra PWM Input Channel 9 + Serial UDB Extra PWM Input Channel 10 + Serial UDB Extra PWM Input Channel 11 + Serial UDB Extra PWM Input Channel 12 + Serial UDB Extra PWM Output Channel 1 + Serial UDB Extra PWM Output Channel 2 + Serial UDB Extra PWM Output Channel 3 + Serial UDB Extra PWM Output Channel 4 + Serial UDB Extra PWM Output Channel 5 + Serial UDB Extra PWM Output Channel 6 + Serial UDB Extra PWM Output Channel 7 + Serial UDB Extra PWM Output Channel 8 + Serial UDB Extra PWM Output Channel 9 + Serial UDB Extra PWM Output Channel 10 + Serial UDB Extra PWM Output Channel 11 + Serial UDB Extra PWM Output Channel 12 + Serial UDB Extra IMU Location X + Serial UDB Extra IMU Location Y + Serial UDB Extra IMU Location Z + Serial UDB Location Error Earth X + Serial UDB Location Error Earth Y + Serial UDB Location Error Earth Z + Serial UDB Extra Status Flags + Serial UDB Extra Oscillator Failure Count + Serial UDB Extra IMU Velocity X + Serial UDB Extra IMU Velocity Y + Serial UDB Extra IMU Velocity Z + Serial UDB Extra Current Waypoint Goal X + Serial UDB Extra Current Waypoint Goal Y + Serial UDB Extra Current Waypoint Goal Z + Aeroforce in UDB X Axis + Aeroforce in UDB Y Axis + Aeroforce in UDB Z axis + SUE barometer temperature + SUE barometer pressure + SUE barometer altitude + SUE battery voltage + SUE battery current + SUE battery milli amp hours used + Sue autopilot desired height + Serial UDB Extra Stack Memory Free + + + Backwards compatible version of SERIAL_UDB_EXTRA F4: format + Serial UDB Extra Roll Stabilization with Ailerons Enabled + Serial UDB Extra Roll Stabilization with Rudder Enabled + Serial UDB Extra Pitch Stabilization Enabled + Serial UDB Extra Yaw Stabilization using Rudder Enabled + Serial UDB Extra Yaw Stabilization using Ailerons Enabled + Serial UDB Extra Navigation with Ailerons Enabled + Serial UDB Extra Navigation with Rudder Enabled + Serial UDB Extra Type of Alitude Hold when in Stabilized Mode + Serial UDB Extra Type of Alitude Hold when in Waypoint Mode + Serial UDB Extra Firmware racing mode enabled + + + Backwards compatible version of SERIAL_UDB_EXTRA F5: format + Serial UDB YAWKP_AILERON Gain for Proporional control of navigation + Serial UDB YAWKD_AILERON Gain for Rate control of navigation + Serial UDB Extra ROLLKP Gain for Proportional control of roll stabilization + Serial UDB Extra ROLLKD Gain for Rate control of roll stabilization + + + Backwards compatible version of SERIAL_UDB_EXTRA F6: format + Serial UDB Extra PITCHGAIN Proportional Control + Serial UDB Extra Pitch Rate Control + Serial UDB Extra Rudder to Elevator Mix + Serial UDB Extra Roll to Elevator Mix + Gain For Boosting Manual Elevator control When Plane Stabilized + + + Backwards compatible version of SERIAL_UDB_EXTRA F7: format + Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation + Serial UDB YAWKD_RUDDER Gain for Rate control of navigation + Serial UDB Extra ROLLKP_RUDDER Gain for Proportional control of roll stabilization + Serial UDB Extra ROLLKD_RUDDER Gain for Rate control of roll stabilization + SERIAL UDB EXTRA Rudder Boost Gain to Manual Control when stabilized + Serial UDB Extra Return To Landing - Angle to Pitch Plane Down + + + Backwards compatible version of SERIAL_UDB_EXTRA F8: format + Serial UDB Extra HEIGHT_TARGET_MAX + Serial UDB Extra HEIGHT_TARGET_MIN + Serial UDB Extra ALT_HOLD_THROTTLE_MIN + Serial UDB Extra ALT_HOLD_THROTTLE_MAX + Serial UDB Extra ALT_HOLD_PITCH_MIN + Serial UDB Extra ALT_HOLD_PITCH_MAX + Serial UDB Extra ALT_HOLD_PITCH_HIGH + + + Backwards compatible version of SERIAL_UDB_EXTRA F13: format + Serial UDB Extra GPS Week Number + Serial UDB Extra MP Origin Latitude + Serial UDB Extra MP Origin Longitude + Serial UDB Extra MP Origin Altitude Above Sea Level + + + Backwards compatible version of SERIAL_UDB_EXTRA F14: format + Serial UDB Extra Wind Estimation Enabled + Serial UDB Extra Type of GPS Unit + Serial UDB Extra Dead Reckoning Enabled + Serial UDB Extra Type of UDB Hardware + Serial UDB Extra Type of Airframe + Serial UDB Extra Reboot Register of DSPIC + Serial UDB Extra Last dspic Trap Flags + Serial UDB Extra Type Program Address of Last Trap + Serial UDB Extra Number of Ocillator Failures + Serial UDB Extra UDB Internal Clock Configuration + Serial UDB Extra Type of Flight Plan + + + Backwards compatible version of SERIAL_UDB_EXTRA F15 format + Serial UDB Extra Model Name Of Vehicle + Serial UDB Extra Registraton Number of Vehicle + + + Backwards compatible version of SERIAL_UDB_EXTRA F16 format + Serial UDB Extra Name of Expected Lead Pilot + Serial UDB Extra URL of Lead Pilot or Team + + + The altitude measured by sensors and IMU + Timestamp (milliseconds since system boot) + GPS altitude (MSL) in meters, expressed as * 1000 (millimeters) + IMU altitude above ground in meters, expressed as * 1000 (millimeters) + barometeric altitude above ground in meters, expressed as * 1000 (millimeters) + Optical flow altitude above ground in meters, expressed as * 1000 (millimeters) + Rangefinder Altitude above ground in meters, expressed as * 1000 (millimeters) + Extra altitude above ground in meters, expressed as * 1000 (millimeters) + + + The airspeed measured by sensors and IMU + Timestamp (milliseconds since system boot) + Airspeed estimate from IMU, cm/s + Pitot measured forward airpseed, cm/s + Hot wire anenometer measured airspeed, cm/s + Ultrasonic measured airspeed, cm/s + Angle of attack sensor, degrees * 10 + Yaw angle sensor, degrees * 10 + + + Backwards compatible version of SERIAL_UDB_EXTRA F17 format + SUE Feed Forward Gain + SUE Max Turn Rate when Navigating + SUE Max Turn Rate in Fly By Wire Mode + + + Backwards compatible version of SERIAL_UDB_EXTRA F18 format + SUE Angle of Attack Normal + SUE Angle of Attack Inverted + SUE Elevator Trim Normal + SUE Elevator Trim Inverted + SUE reference_speed + + + Backwards compatible version of SERIAL_UDB_EXTRA F19 format + SUE aileron output channel + SUE aileron reversed + SUE elevator output channel + SUE elevator reversed + SUE throttle output channel + SUE throttle reversed + SUE rudder output channel + SUE rudder reversed + + + Backwards compatible version of SERIAL_UDB_EXTRA F20 format + SUE Number of Input Channels + SUE UDB PWM Trim Value on Input 1 + SUE UDB PWM Trim Value on Input 2 + SUE UDB PWM Trim Value on Input 3 + SUE UDB PWM Trim Value on Input 4 + SUE UDB PWM Trim Value on Input 5 + SUE UDB PWM Trim Value on Input 6 + SUE UDB PWM Trim Value on Input 7 + SUE UDB PWM Trim Value on Input 8 + SUE UDB PWM Trim Value on Input 9 + SUE UDB PWM Trim Value on Input 10 + SUE UDB PWM Trim Value on Input 11 + SUE UDB PWM Trim Value on Input 12 + + + Backwards compatible version of SERIAL_UDB_EXTRA F21 format + SUE X accelerometer offset + SUE Y accelerometer offset + SUE Z accelerometer offset + SUE X gyro offset + SUE Y gyro offset + SUE Z gyro offset + + + Backwards compatible version of SERIAL_UDB_EXTRA F22 format + SUE X accelerometer at calibration time + SUE Y accelerometer at calibration time + SUE Z accelerometer at calibration time + SUE X gyro at calibration time + SUE Y gyro at calibration time + SUE Z gyro at calibration time + + + diff --git a/bp_mavlink/minimal.xml b/bp_mavlink/minimal.xml new file mode 100644 index 0000000000000..6e371a690b002 --- /dev/null +++ b/bp_mavlink/minimal.xml @@ -0,0 +1,709 @@ + + + 3 + + + Micro air vehicle / autopilot classes. This identifies the individual model. + + Generic autopilot, full support for everything + + + Reserved for future use. + + + SLUGS autopilot, http://slugsuav.soe.ucsc.edu + + + ArduPilot - Plane/Copter/Rover/Sub/Tracker, https://ardupilot.org + + + OpenPilot, http://openpilot.org + + + Generic autopilot only supporting simple waypoints + + + Generic autopilot supporting waypoints and other simple navigation commands + + + Generic autopilot supporting the full mission command set + + + No valid autopilot, e.g. a GCS or other MAVLink component + + + PPZ UAV - http://nongnu.org/paparazzi + + + UAV Dev Board + + + FlexiPilot + + + PX4 Autopilot - http://px4.io/ + + + SMACCMPilot - http://smaccmpilot.org + + + AutoQuad -- http://autoquad.org + + + Armazila -- http://armazila.com + + + Aerob -- http://aerob.ru + + + ASLUAV autopilot -- http://www.asl.ethz.ch + + + SmartAP Autopilot - http://sky-drones.com + + + AirRails - http://uaventure.com + + + Fusion Reflex - https://fusion.engineering + + + + MAVLINK component type reported in HEARTBEAT message. Flight controllers must report the type of the vehicle on which they are mounted (e.g. MAV_TYPE_OCTOROTOR). All other components must report a value appropriate for their type (e.g. a camera must use MAV_TYPE_CAMERA). + + Generic micro air vehicle + + + Fixed wing aircraft. + + + Quadrotor + + + Coaxial helicopter + + + Normal helicopter with tail rotor. + + + Ground installation + + + Operator control unit / ground control station + + + Airship, controlled + + + Free balloon, uncontrolled + + + Rocket + + + Ground rover + + + Surface vessel, boat, ship + + + Submarine + + + Hexarotor + + + Octorotor + + + Tricopter + + + Flapping wing + + + Kite + + + Onboard companion controller + + + Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter. + + + Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter. + + + Tiltrotor VTOL + + + + VTOL reserved 2 + + + VTOL reserved 3 + + + VTOL reserved 4 + + + VTOL reserved 5 + + + Gimbal + + + ADSB system + + + Steerable, nonrigid airfoil + + + Dodecarotor + + + Camera + + + Charging station + + + FLARM collision avoidance system + + + Servo + + + Open Drone ID. See https://mavlink.io/en/services/opendroneid.html. + + + Decarotor + + + Battery + + + Parachute + + + Log + + + OSD + + + IMU + + + GPS + + + Winch + + + + These flags encode the MAV mode. + + 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. + + + 0b01000000 remote control input is enabled. + + + 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. + + + 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. + + + 0b00001000 guided mode enabled, system flies waypoints / mission items. + + + 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. + + + 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. + + + 0b00000001 Reserved for future use. + + + + These values encode the bit positions of the decode position. These values can be used to read the value of a flag bit by combining the base_mode variable with AND with the flag position value. The result will be either 0 or 1, depending on if the flag is set or not. + + First bit: 10000000 + + + Second bit: 01000000 + + + Third bit: 00100000 + + + Fourth bit: 00010000 + + + Fifth bit: 00001000 + + + Sixth bit: 00000100 + + + Seventh bit: 00000010 + + + Eighth bit: 00000001 + + + + + Uninitialized system, state is unknown. + + + System is booting up. + + + System is calibrating and not flight-ready. + + + System is grounded and on standby. It can be launched any time. + + + System is active and might be already airborne. Motors are engaged. + + + System is in a non-normal flight mode. It can however still navigate. + + + System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. + + + System just initialized its power-down sequence, will shut down now. + + + System is terminating itself. + + + + Component ids (values) for the different types and instances of onboard hardware/software that might make up a MAVLink system (autopilot, cameras, servos, GPS systems, avoidance systems etc.). + Components must use the appropriate ID in their source address when sending messages. Components can also use IDs to determine if they are the intended recipient of an incoming message. The MAV_COMP_ID_ALL value is used to indicate messages that must be processed by all components. + When creating new entries, components that can have multiple instances (e.g. cameras, servos etc.) should be allocated sequential values. An appropriate number of values should be left free after these components to allow the number of instances to be expanded. + + Target id (target_component) used to broadcast messages to all components of the receiving system. Components should attempt to process messages with this component ID and forward to components on any other interfaces. Note: This is not a valid *source* component id for a message. + + + System flight controller component ("autopilot"). Only one autopilot is expected in a particular system. + + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Telemetry radio (e.g. SiK radio, or other component that emits RADIO_STATUS messages). + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. + + + Camera #1. + + + Camera #2. + + + Camera #3. + + + Camera #4. + + + Camera #5. + + + Camera #6. + + + Servo #1. + + + Servo #2. + + + Servo #3. + + + Servo #4. + + + Servo #5. + + + Servo #6. + + + Servo #7. + + + Servo #8. + + + Servo #9. + + + Servo #10. + + + Servo #11. + + + Servo #12. + + + Servo #13. + + + Servo #14. + + + Gimbal #1. + + + Logging component. + + + Automatic Dependent Surveillance-Broadcast (ADS-B) component. + + + On Screen Display (OSD) devices for video links. + + + Generic autopilot peripheral component ID. Meant for devices that do not implement the parameter microservice. + + + All gimbals should use MAV_COMP_ID_GIMBAL. + Gimbal ID for QX1. + + + FLARM collision alert component. + + + Parachute component. + + + Gimbal #2. + + + Gimbal #3. + + + Gimbal #4 + + + Gimbal #5. + + + Gimbal #6. + + + Battery #1. + + + Battery #2. + + + CAN over MAVLink client. + + + Component that can generate/supply a mission flight plan (e.g. GCS or developer API). + + + Component that lives on the onboard computer (companion computer) and has some generic functionalities, such as settings system parameters and monitoring the status of some processes that don't directly speak mavlink and so on. + + + Component that lives on the onboard computer (companion computer) and has some generic functionalities, such as settings system parameters and monitoring the status of some processes that don't directly speak mavlink and so on. + + + Component that lives on the onboard computer (companion computer) and has some generic functionalities, such as settings system parameters and monitoring the status of some processes that don't directly speak mavlink and so on. + + + Component that lives on the onboard computer (companion computer) and has some generic functionalities, such as settings system parameters and monitoring the status of some processes that don't directly speak mavlink and so on. + + + Component that finds an optimal path between points based on a certain constraint (e.g. minimum snap, shortest path, cost, etc.). + + + Component that plans a collision free path between two points. + + + Component that provides position estimates using VIO techniques. + + + Component that manages pairing of vehicle and GCS. + + + Inertial Measurement Unit (IMU) #1. + + + Inertial Measurement Unit (IMU) #2. + + + Inertial Measurement Unit (IMU) #3. + + + GPS #1. + + + GPS #2. + + + Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). + + + Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). + + + Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). + + + Component to bridge MAVLink to UDP (i.e. from a UART). + + + Component to bridge to UART (i.e. from UDP). + + + Component handling TUNNEL messages (e.g. vendor specific GUI of a component). + + + System control does not require a separate component ID. + Component for handling system messages (e.g. to ARM, takeoff, etc.). + + + + + + The heartbeat message shows that a system or component is present and responding. The type and autopilot fields (along with the message component id), allow the receiving system to treat further messages from this system appropriately (e.g. by laying out the user interface based on the autopilot). This microservice is documented at https://mavlink.io/en/services/heartbeat.html + Vehicle or component type. For a flight controller component the vehicle type (quadrotor, helicopter, etc.). For other components the component type (e.g. camera, gimbal, etc.). This should be used in preference to component id for identifying the component type. + Autopilot type / class. Use MAV_AUTOPILOT_INVALID for components that are not flight controllers. + System mode bitmap. + A bitfield for use for autopilot-specific flags + System status flag. + MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version + + + diff --git a/bp_mavlink/olliw_dev.xml b/bp_mavlink/olliw_dev.xml new file mode 100644 index 0000000000000..140fc79a76b1d --- /dev/null +++ b/bp_mavlink/olliw_dev.xml @@ -0,0 +1,64 @@ + + + + + + RADIO_LINK_TYPE enum. + + Unknown radio link type. + + + Radio link is Crossfire. + + + Radio link is ExpressLRS. + + + Radio link is mLRS. + + + + + + + Addition to message AUTOPILOT_STATE_FOR_GIMBAL_DEVICE. + System ID. + Component ID. + Timestamp (time since system boot). + Wind X speed in NED (North,Est, Down). NAN if unknown. + Wind Y speed in NED (North, East, Down). NAN if unknown. + Correction angle due to wind. NaN if unknown. + + + + + Injected by a radio link endpoint into the MAVLink stream for purposes of flow control. Should be emitted only by components with component id MAV_COMP_ID_TELEMETRY_RADIO. + Transmitted bytes per second, UINT16_MAX: invalid/unknown. + Recieved bytes per second, UINT16_MAX: invalid/unknown. + Transmit bandwidth consumption. Values: 0..100, UINT8_MAX: invalid/unknown. + Receive bandwidth consumption. Values: 0..100, UINT8_MAX: invalid/unknown. + For compatibility with legacy method. UINT8_MAX: unknown. + + + + + Radio link information. Should be emitted only by components with component id MAV_COMP_ID_TELEMETRY_RADIO. + Radio link type. 0: unknown type. + Operation mode. Radio link dependent. + Rate in Hz. 0: unknown. + Transmit power of transmitter in dBm. INT8_MAX: unknown. + Transmit power of receiver in dBm. INT8_MAX: unknown. + + + diff --git a/bp_mavlink/paparazzi.xml b/bp_mavlink/paparazzi.xml new file mode 100644 index 0000000000000..45c9ec5542f96 --- /dev/null +++ b/bp_mavlink/paparazzi.xml @@ -0,0 +1,38 @@ + + + common.xml + 3 + + + + + + Message encoding a mission script item. This message is emitted upon a request for the next script item. + System ID + Component ID + Sequence + The name of the mission script, NULL terminated. + + + Request script item with the sequence number seq. The response of the system to this message should be a SCRIPT_ITEM message. + System ID + Component ID + Sequence + + + Request the overall list of mission items from the system/component. + System ID + Component ID + + + This message is emitted as response to SCRIPT_REQUEST_LIST by the MAV to get the number of mission scripts. + System ID + Component ID + Number of script items in the sequence + + + This message informs about the currently active SCRIPT. + Active Sequence + + + diff --git a/bp_mavlink/python_array_test.xml b/bp_mavlink/python_array_test.xml new file mode 100644 index 0000000000000..7e4b72e146792 --- /dev/null +++ b/bp_mavlink/python_array_test.xml @@ -0,0 +1,67 @@ + + + + common.xml + + + Array test #0. + Stub field + Value array + Value array + Value array + Value array + + + Array test #1. + Value array + + + Array test #3. + Stub field + Value array + + + Array test #4. + Value array + Stub field + + + Array test #5. + Value array + Value array + + + Array test #6. + Stub field + Stub field + Stub field + Value array + Value array + Value array + Value array + Value array + Value array + Value array + Value array + Value array + + + Array test #7. + Value array + Value array + Value array + Value array + Value array + Value array + Value array + Value array + Value array + + + Array test #8. + Stub field + Value array + Value array + + + diff --git a/bp_mavlink/standard.xml b/bp_mavlink/standard.xml new file mode 100644 index 0000000000000..4ca39607ad11a --- /dev/null +++ b/bp_mavlink/standard.xml @@ -0,0 +1,10 @@ + + + + common.xml + 0 + + + + + diff --git a/bp_mavlink/storm32.xml b/bp_mavlink/storm32.xml new file mode 100644 index 0000000000000..e385eb48895d3 --- /dev/null +++ b/bp_mavlink/storm32.xml @@ -0,0 +1,433 @@ + + + + ardupilotmega.xml + 1 + 1 + + + + + + + Registered for STorM32 gimbal controller. For communication with gimbal or camera. + + + Registered for STorM32 gimbal controller. For communication with gimbal or camera. + + + Registered for STorM32 gimbal controller. For communication with gimbal. + + + Registered for STorM32 gimbal controller. For communication with gimbal. + + + Registered for STorM32 gimbal controller. For communication with camera. + + + Registered for STorM32 gimbal controller. For communication with camera. + + + + + + STorM32 gimbal prearm check flags. + + STorM32 gimbal is in normal state. + + + The IMUs are healthy and working normally. + + + The motors are active and working normally. + + + The encoders are healthy and working normally. + + + A battery voltage is applied and is in range. + + + Virtual input channels are receiving data. + + + Mavlink messages are being received. + + + The STorM32Link data indicates QFix. + + + The STorM32Link is working. + + + The camera has been found and is connected. + + + The signal on the AUX0 input pin is low. + + + The signal on the AUX1 input pin is low. + + + The NTLogger is working normally. + + + + + STorM32 camera prearm check flags. + + The camera has been found and is connected. + + + + + + + Gimbal manager capability flags. + + The gimbal manager supports several profiles. + + + + + Flags for gimbal manager operation. Used for setting and reporting, unless specified otherwise. If a setting has been accepted by the gimbal manager is reported in the STORM32_GIMBAL_MANAGER_STATUS message. + + 0 = ignore. + + + Request to set RC input to active, or report RC input is active. Implies RC mixed. RC exclusive is achieved by setting all clients to inactive. + + + Request to set onboard/companion computer client to active, or report this client is active. + + + Request to set autopliot client to active, or report this client is active. + + + Request to set GCS client to active, or report this client is active. + + + Request to set camera client to active, or report this client is active. + + + Request to set GCS2 client to active, or report this client is active. + + + Request to set camera2 client to active, or report this client is active. + + + Request to set custom client to active, or report this client is active. + + + Request to set custom2 client to active, or report this client is active. + + + Request supervision. This flag is only for setting, it is not reported. + + + Release supervision. This flag is only for setting, it is not reported. + + + + + Gimbal manager client ID. In a prioritizing profile, the priorities are determined by the implementation; they could e.g. be custom1 > onboard > GCS > autopilot/camera > GCS2 > custom2. + + For convenience. + + + This is the onboard/companion computer client. + + + This is the autopilot client. + + + This is the GCS client. + + + This is the camera client. + + + This is the GCS2 client. + + + This is the camera2 client. + + + This is the custom client. + + + This is the custom2 client. + + + + + Gimbal manager profiles. Only standard profiles are defined. Any implementation can define its own profile(s) in addition, and should use enum values > 16. + + Default profile. Implementation specific. + + + Not supported/deprecated. + + + Profile with cooperative behavior. + + + Profile with exclusive behavior. + + + Profile with priority and cooperative behavior for equal priority. + + + Profile with priority and exclusive behavior for equal priority. + + + + + + Enumeration of possible shot modes. + + Undefined shot mode. Can be used to determine if qshots should be used or not. + + + Start normal gimbal operation. Is usually used to return back from a shot. + + + Load and keep safe gimbal position and stop stabilization. + + + Load neutral gimbal position and keep it while stabilizing. + + + Start mission with gimbal control. + + + Start RC gimbal control. + + + Start gimbal tracking the point specified by Lat, Lon, Alt. + + + Start gimbal tracking the system with specified system ID. + + + Start 2-point cable cam quick shot. + + + Start gimbal tracking the home location. + + + + + + + + Command to a gimbal manager to control the gimbal tilt and pan angles. It is possible to set combinations of the values below. E.g. an angle as well as a desired angular rate can be used to get to this angle at a certain angular rate, or an angular rate only will result in continuous turning. NaN is to be used to signal unset. A gimbal device is never to react to this command. + Pitch/tilt angle (positive: tilt up). NaN to be ignored. + Yaw/pan angle (positive: pan to the right). NaN to be ignored. The frame is determined by the GIMBAL_DEVICE_FLAGS_YAW_IN_xxx_FRAME flags. + Pitch/tilt rate (positive: tilt up). NaN to be ignored. + Yaw/pan rate (positive: pan to the right). NaN to be ignored. The frame is determined by the GIMBAL_DEVICE_FLAGS_YAW_IN_xxx_FRAME flags. + Gimbal device flags to be applied. + Gimbal manager flags to be applied. + Gimbal ID of the gimbal manager to address (component ID or 1-6 for non-MAVLink gimbal, 0 for all gimbals). Send command multiple times for more than one but not all gimbals. The client is copied into bits 8-15. + + + + + Command to configure a gimbal manager. A gimbal device is never to react to this command. The selected profile is reported in the STORM32_GIMBAL_MANAGER_STATUS message. + Gimbal manager profile (0 = default). + Gimbal ID of the gimbal manager to address (component ID or 1-6 for non-MAVLink gimbal, 0 for all gimbals). Send command multiple times for more than one but not all gimbals. + + + + + Command to set the shot manager mode. + Set shot mode. + Set shot state or command. The allowed values are specific to the selected shot mode. + + + + + RADIO_RC_CHANNELS flags (bitmask). + + Failsafe is active. + + + Indicates that the current frame has not been received. Channel values are frozen. + + + + RADIO_LINK_STATS flags (bitmask). + + Rssi are in negative dBm. Values 0..254 corresponds to 0..-254 dBm. + + + + + + + + + + Information about a gimbal manager. This message should be requested by a ground station using MAV_CMD_REQUEST_MESSAGE. It mirrors some fields of the GIMBAL_DEVICE_INFORMATION message, but not all. If the additional information is desired, also GIMBAL_DEVICE_INFORMATION should be requested. + Gimbal ID (component ID or 1-6 for non-MAVLink gimbal) that this gimbal manager is responsible for. + Gimbal device capability flags. Same flags as reported by GIMBAL_DEVICE_INFORMATION. The flag is only 16 bit wide, but stored in 32 bit, for backwards compatibility (high word is zero). + Gimbal manager capability flags. + Hardware minimum roll angle (positive: roll to the right). NaN if unknown. + Hardware maximum roll angle (positive: roll to the right). NaN if unknown. + Hardware minimum pitch/tilt angle (positive: tilt up). NaN if unknown. + Hardware maximum pitch/tilt angle (positive: tilt up). NaN if unknown. + Hardware minimum yaw/pan angle (positive: pan to the right, relative to the vehicle/gimbal base). NaN if unknown. + Hardware maximum yaw/pan angle (positive: pan to the right, relative to the vehicle/gimbal base). NaN if unknown. + + + + Message reporting the current status of a gimbal manager. This message should be broadcast at a low regular rate (e.g. 1 Hz, may be increase momentarily to e.g. 5 Hz for a period of 1 sec after a change). + Gimbal ID (component ID or 1-6 for non-MAVLink gimbal) that this gimbal manager is responsible for. + Client who is currently supervisor (0 = none). + Gimbal device flags currently applied. Same flags as reported by GIMBAL_DEVICE_ATTITUDE_STATUS. + Gimbal manager flags currently applied. + Profile currently applied (0 = default). + + + + Message to a gimbal manager to control the gimbal attitude. Angles and rates can be set to NaN according to use case. A gimbal device is never to react to this message. + System ID + Component ID + Gimbal ID of the gimbal manager to address (component ID or 1-6 for non-MAVLink gimbal, 0 for all gimbals). Send command multiple times for more than one but not all gimbals. + Client which is contacting the gimbal manager (must be set). + Gimbal device flags to be applied (UINT16_MAX to be ignored). Same flags as used in GIMBAL_DEVICE_SET_ATTITUDE. + Gimbal manager flags to be applied (0 to be ignored). + Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). Set first element to NaN to be ignored. The frame is determined by the GIMBAL_DEVICE_FLAGS_YAW_IN_xxx_FRAME flags. + X component of angular velocity (positive: roll to the right). NaN to be ignored. + Y component of angular velocity (positive: tilt up). NaN to be ignored. + Z component of angular velocity (positive: pan to the right). NaN to be ignored. The frame is determined by the GIMBAL_DEVICE_FLAGS_YAW_IN_xxx_FRAME flags. + + + + Message to a gimbal manager to control the gimbal tilt and pan angles. Angles and rates can be set to NaN according to use case. A gimbal device is never to react to this message. + System ID + Component ID + Gimbal ID of the gimbal manager to address (component ID or 1-6 for non-MAVLink gimbal, 0 for all gimbals). Send command multiple times for more than one but not all gimbals. + Client which is contacting the gimbal manager (must be set). + Gimbal device flags to be applied (UINT16_MAX to be ignored). Same flags as used in GIMBAL_DEVICE_SET_ATTITUDE. + Gimbal manager flags to be applied (0 to be ignored). + Pitch/tilt angle (positive: tilt up). NaN to be ignored. + Yaw/pan angle (positive: pan the right). NaN to be ignored. The frame is determined by the GIMBAL_DEVICE_FLAGS_YAW_IN_xxx_FRAME flags. + Pitch/tilt angular rate (positive: tilt up). NaN to be ignored. + Yaw/pan angular rate (positive: pan to the right). NaN to be ignored. The frame is determined by the GIMBAL_DEVICE_FLAGS_YAW_IN_xxx_FRAME flags. + + + + Message to a gimbal manager to correct the gimbal roll angle. This message is typically used to manually correct for a tilted horizon in operation. A gimbal device is never to react to this message. + System ID + Component ID + Gimbal ID of the gimbal manager to address (component ID or 1-6 for non-MAVLink gimbal, 0 for all gimbals). Send command multiple times for more than one but not all gimbals. + Client which is contacting the gimbal manager (must be set). + Roll angle (positive to roll to the right). + + + + + + Information about the shot operation. + Current shot mode. + Current state in the shot. States are specific to the selected shot mode. + + + + + Radio channels. Supports up to 24 channels. Channel values are in centerd 13 bit format. Range is [-4096,4096], center is 0. Conversion to PWM is x * 5/32 + 1500. Should be emitted only by components with component id MAV_COMP_ID_TELEMETRY_RADIO. + Total number of RC channels being received. This can be larger than 24, indicating that more channels are available but not given in this message. + Radio channels status flags. + + RC channels. Channels above count should be set to 0, to benefit from MAVLink's zero padding. + + + Radio link statistics. Should be emitted only by components with component id MAV_COMP_ID_TELEMETRY_RADIO. Per default, rssi values are in MAVLink units: 0 represents weakest signal, 254 represents maximum signal; can be changed to dBm with the flag RADIO_LINK_STATS_FLAGS_RSSI_DBM. + Radio link statistics flags. + Values: 0..100. UINT8_MAX: invalid/unknown. + Rssi of antenna1. UINT8_MAX: invalid/unknown. + Noise on antenna1. Radio dependent. INT8_MAX: invalid/unknown. + Rssi of antenna2. UINT8_MAX: ignore/unknown, use rx_rssi1. + Noise on antenna2. Radio dependent. INT8_MAX: ignore/unknown, use rx_snr1. + 0: antenna1, 1: antenna2, UINT8_MAX: ignore, no Rx receive diversity, use rx_rssi1, rx_snr1. + 0: antenna1, 1: antenna2, UINT8_MAX: ignore, no Rx transmit diversity. + Values: 0..100. UINT8_MAX: invalid/unknown. + Rssi of antenna1. UINT8_MAX: invalid/unknown. + Noise on antenna1. Radio dependent. INT8_MAX: invalid/unknown. + Rssi of antenna2. UINT8_MAX: ignore/unknown, use tx_rssi1. + Noise on antenna2. Radio dependent. INT8_MAX: ignore/unknown, use tx_snr1. + 0: antenna1, 1: antenna2, UINT8_MAX: ignore, no Tx receive diversity, use tx_rssi1, tx_snr1. + 0: antenna1, 1: antenna2, UINT8_MAX: ignore, no Tx transmit diversity. + + + + Frsky SPort passthrough multi packet container. + Timestamp (time since system boot). + Number of passthrough packets in this message. + Passthrough packet buffer. A packet has 6 bytes: uint16_t id + uint32_t data. The array has space for 40 packets. + + + + Parameter multi param value container. + Total number of onboard parameters. + Index of the first onboard parameter in this array. + Number of onboard parameters in this array. + Flags. + Parameters buffer. Contains a series of variable length parameter blocks, one per parameter, with format as specifed elsewhere. + + + + diff --git a/bp_mavlink/test.xml b/bp_mavlink/test.xml new file mode 100644 index 0000000000000..c680207462ddf --- /dev/null +++ b/bp_mavlink/test.xml @@ -0,0 +1,31 @@ + + + 3 + + + Test all field types + char + string + uint8_t + uint16_t + uint32_t + uint64_t + int8_t + int16_t + int32_t + int64_t + float + double + uint8_t_array + uint16_t_array + uint32_t_array + uint64_t_array + int8_t_array + int16_t_array + int32_t_array + int64_t_array + float_array + double_array + + + diff --git a/bp_mavlink/uAvionix.xml b/bp_mavlink/uAvionix.xml new file mode 100644 index 0000000000000..4a15425df10d7 --- /dev/null +++ b/bp_mavlink/uAvionix.xml @@ -0,0 +1,210 @@ + + + + + + + common.xml + + + State flags for ADS-B transponder dynamic report + + + + + + + + Transceiver RF control flags for ADS-B transponder dynamic reports + + + + + + Status for ADS-B transponder dynamic input + + + + + + + + + Status flags for ADS-B transponder dynamic output + + + + + + + Definitions for aircraft size + + + + + + + + + + + + + + + + + + + GPS lataral offset encoding + + + + + + + + + + + GPS longitudinal offset encoding + + + + + Emergency status encoding + + + + + + + + + + + State flags for ADS-B transponder dynamic report + + + + + + + + + + State flags for X-Bit and reserved fields. + + + + State flags for ADS-B transponder status report + + + + + + + + + + + State flags for ADS-B transponder status report + + + + + + + + + + + + + + + + + + + + + + + + + State flags for ADS-B transponder fault report + + + + + + + + + + Static data to configure the ADS-B transponder (send within 10 sec of a POR and every 10 sec thereafter) + Vehicle address (24 bit) + Vehicle identifier (8 characters, null terminated, valid characters are A-Z, 0-9, " " only) + Transmitting vehicle type. See ADSB_EMITTER_TYPE enum + Aircraft length and width encoding (table 2-35 of DO-282B) + GPS antenna lateral offset (table 2-36 of DO-282B) + GPS antenna longitudinal offset from nose [if non-zero, take position (in meters) divide by 2 and add one] (table 2-37 DO-282B) + Aircraft stall speed in cm/s + ADS-B transponder reciever and transmit enable flags + + + Dynamic data used to generate ADS-B out transponder data (send at 5Hz) + UTC time in seconds since GPS epoch (Jan 6, 1980). If unknown set to UINT32_MAX + Latitude WGS84 (deg * 1E7). If unknown set to INT32_MAX + Longitude WGS84 (deg * 1E7). If unknown set to INT32_MAX + Altitude (WGS84). UP +ve. If unknown set to INT32_MAX + 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK + Number of satellites visible. If unknown set to UINT8_MAX + Barometric pressure altitude (MSL) relative to a standard atmosphere of 1013.2 mBar and NOT bar corrected altitude (m * 1E-3). (up +ve). If unknown set to INT32_MAX + Horizontal accuracy in mm (m * 1E-3). If unknown set to UINT32_MAX + Vertical accuracy in cm. If unknown set to UINT16_MAX + Velocity accuracy in mm/s (m * 1E-3). If unknown set to UINT16_MAX + GPS vertical speed in cm/s. If unknown set to INT16_MAX + North-South velocity over ground in cm/s North +ve. If unknown set to INT16_MAX + East-West velocity over ground in cm/s East +ve. If unknown set to INT16_MAX + Emergency status + ADS-B transponder dynamic input state flags + Mode A code (typically 1200 [0x04B0] for VFR) + + + Transceiver heartbeat with health report (updated every 10s) + ADS-B transponder messages + + + Aircraft Registration. + Aircraft Registration (ASCII string A-Z, 0-9 only), e.g. "N8644B ". Trailing spaces (0x20) only. This is null-terminated. + + + Flight Identification for ADSB-Out vehicles. + Flight Identification: 8 ASCII characters, '0' through '9', 'A' through 'Z' or space. Spaces (0x20) used as a trailing pad character, or when call sign is unavailable. Reflects Control message setting. This is null-terminated. + + + Request messages. + Message ID to request. Supports any message in this 10000-10099 range + + + Control message with all data sent in UCP control message. + ADS-B transponder control state flags + Barometric pressure altitude (MSL) relative to a standard atmosphere of 1013.2 mBar and NOT bar corrected altitude (m * 1E-3). (up +ve). If unknown set to INT32_MAX + Mode A code (typically 1200 [0x04B0] for VFR) + Emergency status + Flight Identification: 8 ASCII characters, '0' through '9', 'A' through 'Z' or space. Spaces (0x20) used as a trailing pad character, or when call sign is unavailable. + X-Bit enable (military transponders only) + + + Status message with information from UCP Heartbeat and Status messages. + ADS-B transponder status state flags + Mode A code (typically 1200 [0x04B0] for VFR) + Integrity and Accuracy of traffic reported as a 4-bit value for each field (NACp 7:4, NIC 3:0) and encoded by Containment Radius (HPL) and Estimated Position Uncertainty (HFOM), respectively + Board temperature in C + ADS-B transponder fault flags + Flight Identification: 8 ASCII characters, '0' through '9', 'A' through 'Z' or space. Spaces (0x20) used as a trailing pad character, or when call sign is unavailable. + + + diff --git a/bp_mavlink/ualberta.xml b/bp_mavlink/ualberta.xml new file mode 100644 index 0000000000000..e0a6214a56214 --- /dev/null +++ b/bp_mavlink/ualberta.xml @@ -0,0 +1,76 @@ + + + common.xml + + + Available autopilot modes for ualberta uav + + Raw input pulse widts sent to output + + + Inputs are normalized using calibration, the converted back to raw pulse widths for output + + + dfsdfs + + + dfsfds + + + dfsdfsdfs + + + + Navigation filter mode + + + AHRS mode + + + INS/GPS initialization mode + + + INS/GPS mode + + + + Mode currently commanded by pilot + + sdf + + + dfs + + + Rotomotion mode + + + + + + Accelerometer and Gyro biases from the navigation filter + Timestamp (microseconds) + b_f[0] + b_f[1] + b_f[2] + b_f[0] + b_f[1] + b_f[2] + + + Complete set of calibration parameters for the radio + Aileron setpoints: left, center, right + Elevator setpoints: nose down, center, nose up + Rudder setpoints: nose left, center, nose right + Tail gyro mode/gain setpoints: heading hold, rate mode + Pitch curve setpoints (every 25%) + Throttle curve setpoints (every 25%) + + + System status specific to ualberta uav + System mode, see UALBERTA_AUTOPILOT_MODE ENUM + Navigation mode, see UALBERTA_NAV_MODE ENUM + Pilot mode, see UALBERTA_PILOT_MODE + + + diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index f3551bc5f19de..52b9b3f8f240d 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -661,4 +661,36 @@ AP_Camera *camera() } +//OW +bool AP_Camera::set_cam_mode(bool video_mode) +{ +/* XX ?? +#if HAL_MOUNT_ENABLED + if (get_trigger_type() == CamTrigType::mount) { + AP_Mount* mount = AP::mount(); + if (mount != nullptr) { + return mount->set_cam_mode(0, video_mode); + } + } +#endif +*/ + return false; +} + +bool AP_Camera::set_cam_photo_video(int8_t sw_flag) +{ +/* XX ?? +#if HAL_MOUNT_ENABLED + if (get_trigger_type() == CamTrigType::mount) { + AP_Mount* mount = AP::mount(); + if (mount != nullptr) { + return mount->set_cam_photo_video(0, sw_flag); + } + } +#endif +*/ + return false; +} +//OWEND + #endif diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 631651147f4ff..bd1055fd14593 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -146,6 +146,11 @@ class AP_Camera { // set if vehicle is in AUTO mode void set_is_auto_mode(bool enable) { _is_in_auto_mode = enable; } +//OW + bool set_cam_mode(bool video_mode); + bool set_cam_photo_video(int8_t sw_flag); +//OWEND + #if AP_CAMERA_SCRIPTING_ENABLED // structure and accessors for use by scripting backends typedef struct { diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index b3f8e4bf36137..920b4eebeb9c3 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -18,6 +18,10 @@ #include #include #include +//OW +#include +#include "BP_Mount_STorM32_MAVLink.h" +//OWEND const AP_Param::GroupInfo AP_Mount::var_info[] = { @@ -149,6 +153,14 @@ void AP_Mount::init() _num_instances++; break; #endif // HAL_MOUNT_VIEWPRO_ENABLED + +//OW + // check for STorM32_MAVLink mounts using MAVLink protocol + case Type::STorM32_MAVLink: + _backends[instance] = new BP_Mount_STorM32_MAVLink(*this, _params[instance], instance); + _num_instances++; + break; +//OWEND } // init new instance @@ -1015,4 +1027,42 @@ AP_Mount *mount() }; +//OW +bool AP_Mount::set_cam_mode(uint8_t instance, bool video_mode) +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return false; + } + return backend->set_cam_mode(video_mode); +} + +bool AP_Mount::set_cam_photo_video(uint8_t instance, int8_t sw_flag) +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return false; + } + return backend->set_cam_photo_video(sw_flag); +} + +void AP_Mount::handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg) +{ + for (uint8_t instance=0; instancehandle_msg(msg); + } + } +} + +void AP_Mount::send_banner(void) +{ + for (uint8_t instance=0; instancesend_banner(); + } + } +} +//OWEND + #endif /* HAL_MOUNT_ENABLED */ diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 55306d5efb0b5..8ecfd3022de35 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -46,6 +46,9 @@ class AP_Mount_Siyi; class AP_Mount_Scripting; class AP_Mount_Xacti; class AP_Mount_Viewpro; +//OW +class BP_Mount_STorM32_MAVLink; +//OWEND /* This is a workaround to allow the MAVLink backend access to the @@ -66,6 +69,9 @@ class AP_Mount friend class AP_Mount_Scripting; friend class AP_Mount_Xacti; friend class AP_Mount_Viewpro; +//OW + friend class BP_Mount_STorM32_MAVLink; +//OWEND public: AP_Mount(); @@ -114,6 +120,9 @@ class AP_Mount #if HAL_MOUNT_VIEWPRO_ENABLED Viewpro = 11, /// Viewpro gimbal using a custom serial protocol #endif +//OW + STorM32_MAVLink = 83 +//OWEND }; // init - detect and initialise all mounts @@ -250,6 +259,21 @@ class AP_Mount // parameter var table static const struct AP_Param::GroupInfo var_info[]; +//OW + // set photo or video mode + bool set_cam_mode(uint8_t instance, bool video_mode); + + // 3-way switch mode + bool set_cam_photo_video(uint8_t instance, int8_t sw_flag); + + // this is somewhat different to handle_message() in that it catches all messages + // with significant work it potentially could be combined, but let's play it safe and not introduce side effects + void handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg); + + // send banner + void send_banner(void); +//OWEND + protected: static AP_Mount *_singleton; diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 3481950fe6108..688a5ce59236d 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -75,7 +75,10 @@ class AP_Mount_Backend // set yaw_lock. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North) // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle - void set_yaw_lock(bool yaw_lock) { _yaw_lock = yaw_lock; } +//OW +// void set_yaw_lock(bool yaw_lock) { _yaw_lock = yaw_lock; } + virtual void set_yaw_lock(bool yaw_lock) { _yaw_lock = yaw_lock; } +//OWEND // set angle target in degrees // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame @@ -107,7 +110,10 @@ class AP_Mount_Backend void handle_mount_control(const mavlink_mount_control_t &packet); // send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS - void send_gimbal_device_attitude_status(mavlink_channel_t chan); +//OW +// void send_gimbal_device_attitude_status(mavlink_channel_t chan); + virtual void send_gimbal_device_attitude_status(mavlink_channel_t chan); +//OWEND // return gimbal capabilities sent to GCS in the GIMBAL_MANAGER_INFORMATION virtual uint32_t get_gimbal_manager_capability_flags() const; @@ -125,7 +131,10 @@ class AP_Mount_Backend virtual void handle_param_value(const mavlink_message_t &msg) {} // handle a GLOBAL_POSITION_INT message - bool handle_global_position_int(uint8_t msg_sysid, const mavlink_global_position_int_t &packet); +//OW +// bool handle_global_position_int(uint8_t msg_sysid, const mavlink_global_position_int_t &packet); + virtual bool handle_global_position_int(uint8_t msg_sysid, const mavlink_global_position_int_t &packet); +//OWEND // handle GIMBAL_DEVICE_INFORMATION message virtual void handle_gimbal_device_information(const mavlink_message_t &msg) {} @@ -178,6 +187,23 @@ class AP_Mount_Backend // send camera settings message to GCS virtual void send_camera_settings(mavlink_channel_t chan) const {} +//OW + // set photo or video mode + virtual bool set_cam_mode(bool video_mode) { return false; } + + // 3-way switch mode + virtual bool set_cam_photo_video(int8_t sw_flag) { return false; } + + // handle msg - allows to process a msg from a gimbal + virtual void handle_msg(const mavlink_message_t &msg) {} + + // send banner + virtual void send_banner(void) {} + + // frontend access + bool is_primary(void) { return (_instance == _frontend._primary); } +//OWEND + protected: enum class MountTargetType { diff --git a/libraries/AP_Mount/AP_Mount_Params.cpp b/libraries/AP_Mount/AP_Mount_Params.cpp index eab26b07d123f..ba44f3de76997 100644 --- a/libraries/AP_Mount/AP_Mount_Params.cpp +++ b/libraries/AP_Mount/AP_Mount_Params.cpp @@ -166,6 +166,10 @@ const AP_Param::GroupInfo AP_Mount_Params::var_info[] = { // @User: Advanced AP_GROUPINFO_FLAGS("_DEVID", 15, AP_Mount_Params, dev_id, 0, AP_PARAM_FLAG_INTERNAL_USE_ONLY), +//OW + AP_GROUPINFO("_ZFLAGS", 20, AP_Mount_Params, zflags, 0), +//OWEND + AP_GROUPEND }; diff --git a/libraries/AP_Mount/AP_Mount_Params.h b/libraries/AP_Mount/AP_Mount_Params.h index a3d7e12b01b6e..4ed0ae46de72e 100644 --- a/libraries/AP_Mount/AP_Mount_Params.h +++ b/libraries/AP_Mount/AP_Mount_Params.h @@ -31,4 +31,8 @@ class AP_Mount_Params { AP_Float pitch_stb_lead; // pitch lead control gain (only used by servo backend) AP_Int8 sysid_default; // target sysid for mount to follow AP_Int32 dev_id; // Device id taking into account bus + +//OW + AP_Int8 zflags; +//OWEND }; diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp new file mode 100644 index 0000000000000..f11849deeb68d --- /dev/null +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -0,0 +1,1534 @@ +//***************************************************** +//OW +// (c) olliw, www.olliw.eu, GPL3 +// STorM32 mount backend class +//***************************************************** + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "BP_Mount_STorM32_MAVLink.h" + +extern const AP_HAL::HAL& hal; + +//***************************************************** +/* +This is a simplified, heavily reworked version of what's in BetaPilot 4.2.3. +Adopts the gimbal changes in 4.3 as much as possible, and follows the Gremsy & SToRM32 drivers. +Limitations of the Gremsy driver are: +- captures & resends messages instead of relying on routing +- yaw lock and vehicle/earth frame are strictly tight together +- capabilities (i.e. limited capabilities) are not respected + +two modes/protocols of operation are supported +1. ArduPilot like, largely mimics Gremsy gimbal driver = PROTOCOL_ARDUPILOT_LIKE + we could mimic a super primitive v2 gimbal manager, primary is always autopilot, secondary always our GCS + - only sends GIMBAL_MANAGER_STATUS + - don' handle MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE + - handling of request is needed + - GIMBAL_MANAGER_SET_PITCHYAW, MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW ? ArduPilot's handling needs to be corrected, also doesn't respect pan +2. STorM32 is gimbal manger, using STorM32 gimbal manager messages = PROTOCOL_STORM32_GIMBAL_MANAGER +*/ +//***************************************************** + +/* + ZFLAGS + 0: protocol is auto determined + 1: protocol forced to PROTOCOL_ARDUPILOT_LIKE + 2: protocol forced to PROTOCOL_STORM32_GIMBAL_MANAGER + 8: only streaming + only sends out RC_CHANNLES, AUTOPILOT_STATE_FOR_GIMBAL for STorM32-Link + this mode could in principle be replaced by asking for the streams, but since AP isn't streaming reliably we don't + 16: do not send AUTOPILOT_STATE_FOR_GIMBAL_EXT + 64: do not use 3way photo-video switch mode for 'Camera Mode Toggle' aux function + 128: do not log + + in all modes sends MOUNT_STATUS to ground, so that "old" things like MP etc can see the gimbal orientation + listens to STORM32_GIMBAL_DEVICE_STATUS to send out MOUNT_STATUS in sync +*/ + +//****************************************************** +// Quaternion & Euler for Gimbal +//****************************************************** +// we do not use NED (roll-pitch-yaw) to convert received quaternion to Euler angles and vice versa +// we use pitch-roll-yaw instead +// when the roll angle is zero, both are equivalent, this should be the majority of cases anyhow +// also, for most gimbals pitch-roll-yaw is appropriate +// the issue with NED is the gimbal lock at pitch +-90�, but pitch +-90� is a common operation point for gimbals +// the angles we store in this lib are thus pitch-roll-yaw Euler + +class GimbalQuaternion : public Quaternion +{ +public: + // inherit constructors + using Quaternion::Quaternion; + + // create a quaternion from gimbal Euler angles + void from_gimbal_euler(float roll, float pitch, float yaw); + + // create gimbal Euler angles from a quaternion + void to_gimbal_euler(float &roll, float &pitch, float &yaw) const; +}; + + +void GimbalQuaternion::from_gimbal_euler(float roll, float pitch, float yaw) +{ + const float cr2 = cosf(roll*0.5f); + const float cp2 = cosf(pitch*0.5f); + const float cy2 = cosf(yaw*0.5f); + const float sr2 = sinf(roll*0.5f); + const float sp2 = sinf(pitch*0.5f); + const float sy2 = sinf(yaw*0.5f); + + q1 = cp2*cr2*cy2 - sp2*sr2*sy2; // -> cp2*cy2 + q2 = cp2*sr2*cy2 - sp2*cr2*sy2; // -> -sp2*sy2 + q3 = sp2*cr2*cy2 + cp2*sr2*sy2; // -> sp2*cy2 + q4 = sp2*sr2*cy2 + cp2*cr2*sy2; // -> cp2*sy2 +} + + +void GimbalQuaternion::to_gimbal_euler(float &roll, float &pitch, float &yaw) const +{ + pitch = atan2f(2.0f*(q1*q3 - q2*q4), 1.0f - 2.0f*(q2*q2 + q3*q3)); // -R31 / R33 = -(-spcr) / cpcr + roll = safe_asin(2.0f*(q1*q2 + q3*q4)); // R32 = sr + yaw = atan2f(2.0f*(q1*q4 - q2*q3), 1.0f - 2.0f*(q2*q2 + q4*q4)); // -R12 / R22 = -(-crsy) / crcy +} + + +//****************************************************** +// BP_Mount_STorM32_MAVLink, that's the main class +//****************************************************** + +// for reasons I really don't understand, calling them as log methods didn't work +// the MTC got mixed up with MTH, i.e., no MTC0 was there but a MTH0 with the MTC params... +// so done as macro in-place, which works fine +// units: +// { '-', "" }, // no units e.g. Pi, or a string +// { 'd', "deg" }, // of the angular variety, -180 to 180 +// { 'n', "m/s" }, // metres per second +// { 's', "s" }, // seconds +// { 'E', "rad/s" }, // radians per second +// { 'k', "deg/s" }, // degrees per second. Degrees are NOT SI, but is some situations more user-friendly than radians +// scales: +// { '-', 0 }, +// { 'F', 1e-6 }, +// data types: +// B : uint8_t +// H : uint16_t +// I : uint32_t +// f : float + +#define BP_LOG(m,h,...) if(_should_log){char logn[5] = m; logn[3] += _instance; AP::logger().Write(logn, h, AP_HAL::micros64(), __VA_ARGS__);} + + +// log incoming GIMBAL_DEVICE_ATTITUDE_STATUS +#define BP_LOG_MTS_ATTITUDESTATUS_HEADER \ + "TimeUS,Roll,Pitch,Yaw,dYaw,YawAhrs,Flags,FailFlags", \ + "sddddd--", \ + "F-------", \ + "QfffffHH" + +// log outgoing GIMBAL_DEVICE_SET_ATTITUDE, STORM32_GIMBAL_MANAGER_CONTROL +#define BP_LOG_MTC_GIMBALCONTROL_HEADER \ + "TimeUS,Type,Roll,Pitch,Yaw,GDFlags,GMFlags,TMode,QMode", \ + "s-ddd----", \ + "F--------", \ + "QBfffHHBB" + +// log outgoing AUTOPILOT_STATE_FOR_GIMBAL_DEVICE +#define BP_LOG_MTL_AUTOPILOTSTATE_HEADER \ + "TimeUS,q1,q2,q3,q4,vx,vy,vz,wz,YawRate,Est,Land,NavEst,NEst2,dtUS", \ + "s----nnnkk----s", \ + "F-------------F", \ + "QfffffffffHBHHI" + +// log outgoing AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_EXT +#define BP_LOG_MTLE_AUTOPILOTSTATEEXT_HEADER \ + "TimeUS,windx,windy,corrangle,vh,wh,gh,ah", \ + "snnddddd", \ + "F-------", \ + "Qfffffff" + + +//****************************************************** +// STorM32 states +//****************************************************** +enum STORM32STATEENUM { + STORM32STATE_STARTUP_MOTORS = 0, + STORM32STATE_STARTUP_SETTLE, + STORM32STATE_STARTUP_CALIBRATE, + STORM32STATE_STARTUP_LEVEL, + STORM32STATE_STARTUP_MOTORDIRDETECT, + STORM32STATE_STARTUP_RELEVEL, + STORM32STATE_NORMAL, + STORM32STATE_STARTUP_FASTLEVEL, +}; + + +//****************************************************** +// BP_Mount_STorM32_MAVLink, that's the main class +//****************************************************** + +BP_Mount_STorM32_MAVLink::BP_Mount_STorM32_MAVLink(AP_Mount &frontend, AP_Mount_Params ¶ms, uint8_t instance) : + AP_Mount_Backend(frontend, params, instance) +{ + _got_device_info = false; + _initialised = false; + _armed = false; + _prearmchecks_ok = false; + + _sysid = 0; + _compid = 0; // gimbal not yet discovered + _chan = MAVLINK_COMM_0; // this is a dummy, will be set correctly by find_gimbal() + + _armingchecks_enabled = false; + _prearmchecks_all_ok = false; + _prearmcheck.updated = false; + _prearmcheck.enabled_flags = 0; + _prearmcheck.fail_flags = 0; + _prearmcheck.fail_flags_last = UINT32_MAX; + + _device.received_flags = 0; + _device.received_failure_flags = 0; + + _yaw_lock = false; // STorM32 doesn't currently support earth frame, so we need to ensure this is false + _is_yaw_lock = false; + + _mode = MAV_MOUNT_MODE_RC_TARGETING; + + _sendonly = false; + _should_log = true; + _got_radio_rc_channels = false; // disable sending rc channels when RADIO_RC_CHANNELS messages are detected + _send_autopilotstateext = true; + _use_3way_photo_video = true; + + _protocol = PROTOCOL_UNDEFINED; + _protocol_auto_cntdown = PROTOCOL_AUTO_TIMEOUT_CNT; + _qshot.mode = MAV_QSHOT_MODE_UNDEFINED; + _qshot.mode_last = MAV_QSHOT_MODE_UNDEFINED; + + if (_params.zflags & 0x01) { // 1 is set + _protocol = PROTOCOL_ARDUPILOT_LIKE; + } else + if (_params.zflags & 0x02) { // 2 is set + _protocol = PROTOCOL_STORM32_GIMBAL_MANAGER; + } + if (_params.zflags & 0x08) _sendonly = true; + // we currently always do it if (_params.zflags & 0x10) _send_autopilotstateext = false; + if (_params.zflags & 0x40) _use_3way_photo_video = !_use_3way_photo_video; + if (_params.zflags & 0x80) _should_log = false; + + _camera_compid = 0; // camera not yet discovered + _camera_mode = CAMERA_MODE_UNDEFINED; +} + + +// called by all vehicles with 50 Hz, using the scheduler +// several vehicles do not support fast_update(), so let's go with this +// priority of update() not very high, so no idea how reliable that is, may be not so good +// we would have wanted updates with 20 Hz, especially for STorM32-Link +// however, since it's 50 Hz, we update at 25 Hz and 12.5 Hz respectively +// not soo nice, but best we can do +// not clear what it means for STorM32Link, probably not too bad, maybe even good +void BP_Mount_STorM32_MAVLink::update() +{ + switch (_task_counter) { + case TASK_SLOT0: + case TASK_SLOT2: + if (_compid) { // we send it as soon as we have found the gimbal + send_autopilot_state_for_gimbal_device(); + if (_send_autopilotstateext) send_autopilot_state_for_gimbal_device_ext(); + } + break; + + case TASK_SLOT1: + if (_sendonly) break; // don't do any control messages + if (_initialised) { // we do it when the startup sequence has been fully completed + set_and_send_target_angles(); // GRRRRRR this is used to determine hasmanager!! + } + break; + + case TASK_SLOT3: + if (_compid) { // we send it as soon as we have found the gimbal + if (!_got_radio_rc_channels) send_rc_channels(); + } + break; + } + + _task_counter++; + if (_task_counter > TASK_SLOT3) _task_counter = TASK_SLOT0; + + if (!_initialised) { + find_gimbal(); + return; + } + + uint32_t tnow_ms = AP_HAL::millis(); + + if ((tnow_ms - _send_system_time_tlast_ms) >= 5000) { // every 5 sec is really plenty + _send_system_time_tlast_ms = tnow_ms; + send_system_time(); + } +} + + +void BP_Mount_STorM32_MAVLink::set_and_send_target_angles(void) +{ + GimbalTarget gtarget; + + if (_qshot.mode == MAV_QSHOT_MODE_UNDEFINED) { + enum MAV_MOUNT_MODE mntmode = get_mode(); + if (!get_target_angles_mount(gtarget, mntmode)) return; // don't send + update_gimbal_device_flags_mount(mntmode); + } else { + if (!get_target_angles_qshot(gtarget)) return; // don't send + update_gimbal_device_flags_qshot(); + } + + _qshot.mode_last = _qshot.mode; + if (_qshot.mode == UINT8_MAX) return; // is in hold, don't send + + if (_protocol == PROTOCOL_ARDUPILOT_LIKE) { + send_gimbal_device_set_attitude(gtarget); + } else + if (_protocol == PROTOCOL_STORM32_GIMBAL_MANAGER) { + // only send when autopilot client is active, this reduces traffic + if (_manager.ap_client_is_active) { + update_gimbal_manager_flags(); + send_storm32_gimbal_manager_control(gtarget); + } + } +} + + +//------------------------------------------------------ +// V2 GIMBAL DEVICE, ArduPilot like +//------------------------------------------------------ + +bool BP_Mount_STorM32_MAVLink::get_target_angles_mount(GimbalTarget >arget, enum MAV_MOUNT_MODE mntmode) +{ + MountTarget mtarget_rad = {}; + + // update based on mount mode + switch (mntmode) { + + // move mount to a "retracted" position. We disable motors + case MAV_MOUNT_MODE_RETRACT: + gtarget.set(TARGET_MODE_RETRACT); + return true; + + // move mount to a neutral position, typically pointing forward + case MAV_MOUNT_MODE_NEUTRAL: { + const Vector3f &vec_deg = _params.neutral_angles.get(); + gtarget.set_from_vec_deg(vec_deg, TARGET_MODE_NEUTRAL); + return true; + } + + // use angle or rate targets provided by a mavlink message or mission command + case MAV_MOUNT_MODE_MAVLINK_TARGETING: + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + gtarget.set_angle(mnt_target.angle_rad); + return true; + case MountTargetType::RATE: + // we do not yet support rate, so do it SToRM32 driver like + MountTarget rate_mtarget_rad {}; + update_angle_target_from_rate(mnt_target.rate_rads, rate_mtarget_rad); + gtarget.set_angle(rate_mtarget_rad); + return true; + } + break; + + // RC radio manual angle control, but with stabilization from the AHRS + // update targets using pilot's RC inputs + case MAV_MOUNT_MODE_RC_TARGETING: +/* XX ?? + if (get_rc_rate_target(mtarget_rad)) { + // we do not yet support rate, so do it SToRM32 driver like + MountTarget rate_mtarget_rad {}; + update_angle_target_from_rate(mtarget_rad, rate_mtarget_rad); + gtarget.set_angle(rate_mtarget_rad); + return true; + } else + if (get_rc_angle_target(mtarget_rad)) { + gtarget.set_angle(mtarget_rad); + return true; + } +*/ + break; + + // point mount to a GPS point given by the mission planner + case MAV_MOUNT_MODE_GPS_POINT: + if (get_angle_target_to_roi(mtarget_rad)) { + gtarget.set_angle(mtarget_rad); + return true; + } + break; + + // point mount to home + case MAV_MOUNT_MODE_HOME_LOCATION: + if (get_angle_target_to_home(mtarget_rad)) { + gtarget.set_angle(mtarget_rad); + return true; + } + break; + + case MAV_MOUNT_MODE_SYSID_TARGET: + if (get_angle_target_to_sysid(mtarget_rad)) { + gtarget.set_angle(mtarget_rad); + return true; + } + break; + + default: + // unknown mode so do nothing + break; + } + + return false; +} + + +void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags_mount(enum MAV_MOUNT_MODE mntmode) +{ + _device.flags_for_gimbal = 0; + + switch (mntmode) { + case MAV_MOUNT_MODE_RETRACT: + _device.flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RETRACT; + break; + case MAV_MOUNT_MODE_NEUTRAL: + _device.flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_NEUTRAL; + break; + default: + break; + } + + _device.flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK; + if (_is_yaw_lock) _device.flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_LOCK; + + // set either YAW_IN_VEHICLE_FRAME or YAW_IN_EARTH_FRAME, to indicate new message format, STorM32 will reject otherwise + _device.flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME; +} + + +//------------------------------------------------------ +// STORM32 GIMBAL MANAGER, QShot +//------------------------------------------------------ + +bool BP_Mount_STorM32_MAVLink::get_target_angles_qshot(GimbalTarget >arget) +{ + switch (_qshot.mode) { + case MAV_QSHOT_MODE_UNDEFINED: + return false; + + case MAV_QSHOT_MODE_GIMBAL_RETRACT: + return get_target_angles_mount(gtarget, MAV_MOUNT_MODE_RETRACT); + + case MAV_QSHOT_MODE_GIMBAL_NEUTRAL: + return get_target_angles_mount(gtarget, MAV_MOUNT_MODE_NEUTRAL); + + case MAV_QSHOT_MODE_GIMBAL_RC_CONTROL: + return get_target_angles_mount(gtarget, MAV_MOUNT_MODE_RC_TARGETING); + + case MAV_QSHOT_MODE_POI_TARGETING: + return get_target_angles_mount(gtarget, MAV_MOUNT_MODE_GPS_POINT); + + case MAV_QSHOT_MODE_HOME_TARGETING: + return get_target_angles_mount(gtarget, MAV_MOUNT_MODE_HOME_LOCATION); + + case MAV_QSHOT_MODE_SYSID_TARGETING: + return get_target_angles_mount(gtarget, MAV_MOUNT_MODE_SYSID_TARGET); + + default: + // in all other modes we don't do nothing, i.e. just send out something + // it is the job of the supervisor to get things right by setting the activity + return get_target_angles_mount(gtarget, MAV_MOUNT_MODE_MAVLINK_TARGETING); + } + + return false; +} + + +void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags_qshot(void) +{ + switch (_qshot.mode) { + case MAV_QSHOT_MODE_GIMBAL_RETRACT: + update_gimbal_device_flags_mount(MAV_MOUNT_MODE_RETRACT); + break; + + case MAV_QSHOT_MODE_GIMBAL_NEUTRAL: + update_gimbal_device_flags_mount(MAV_MOUNT_MODE_NEUTRAL); + break; + + default: + // currently, anything not retract/neutral is good + update_gimbal_device_flags_mount(MAV_MOUNT_MODE_ENUM_END); + break; + } +} + + +void BP_Mount_STorM32_MAVLink::update_gimbal_manager_flags(void) +{ + // we play it simple and do not attempt to claim supervision nor activity + // we thus leave this to other components, e.g. a gcs, to set this + _manager.flags_for_gimbal = MAV_STORM32_GIMBAL_MANAGER_FLAGS_NONE; + + //TODO: we could claim supervisor and activity if the qshot mode suggests so + // what is then about missions ???? should qshots include a mission mode?? + // what about scripts? + // it's not bad as it is +} + + +//------------------------------------------------------ +// Health, Prearm, find +//------------------------------------------------------ + +// return true if healthy +bool BP_Mount_STorM32_MAVLink::healthy() const +{ + return const_cast(this)->prearmchecks_do(); // yes, ugly, but I have not overdesigned the backend +} + + +void BP_Mount_STorM32_MAVLink::find_gimbal(void) +{ + // search for gimbal until armed + if (hal.util->get_soft_armed()) { + return; + } + + uint32_t tnow_ms = AP_HAL::millis(); + + // search for gimbal in routing table + + if (!_compid) { + // we expect that instance 0 has compid = MAV_COMP_ID_GIMBAL, instance 1 has compid = MAV_COMP_ID_GIMBAL2, etc + uint8_t compid = (_instance == 0) ? MAV_COMP_ID_GIMBAL : MAV_COMP_ID_GIMBAL2 + (_instance - 1); + if (GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_GIMBAL, compid, _sysid, _chan) && (_sysid == mavlink_system.sysid)) { + _compid = compid; + _request_device_info_tlast_ms = (tnow_ms < 900) ? 0 : tnow_ms - 900; // start sending requests in 100 ms + } else { + // have not yet found a gimbal so return + return; + } + } +/* + + // search for a mavlink enabled gimbal + //if (_link == nullptr) { + if (!_compid) { + // we expect that instance 0 has compid = MAV_COMP_ID_GIMBAL, instance 1 has compid = MAV_COMP_ID_GIMBAL2, etc + uint8_t compid = (_instance == 0) ? MAV_COMP_ID_GIMBAL : MAV_COMP_ID_GIMBAL2 + (_instance - 1); + _link = GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_GIMBAL, compid, _sysid); + if (_link == nullptr || && (_sysid != mavlink_system.sysid)) { + // have not yet found a gimbal so return + return; + } + + _compid = compid; + _request_device_info_tlast_ms = (tnow_ms < 900) ? 0 : tnow_ms - 900; // start sending requests in 100 ms + } +*/ + + // request GIMBAL_DEVICE_INFORMATION + if (!_got_device_info) { + if (tnow_ms - _request_device_info_tlast_ms > 1000) { + _request_device_info_tlast_ms = tnow_ms; + send_request_gimbal_device_information(); + } + return; + } + + // we don't know yet what we should do + if (_protocol == PROTOCOL_UNDEFINED) { + return; + } + + _initialised = true; +} + + +void BP_Mount_STorM32_MAVLink::determine_protocol(const mavlink_message_t &msg) +{ + if (msg.sysid != _sysid || msg.compid != _compid) { // this msg is not from our gimbal + return; + } + + switch (msg.msgid) { + case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS: + if (_protocol == PROTOCOL_STORM32_GIMBAL_MANAGER) break; // do not allow switching from gimbal manager mode to gimbal device mode + if (_protocol_auto_cntdown) _protocol_auto_cntdown--; // delay switching to gimbal device mode, to give gimbal manager messages a chance + if (!_protocol_auto_cntdown) { + _protocol = PROTOCOL_ARDUPILOT_LIKE; + } + break; + + case MAVLINK_MSG_ID_STORM32_GIMBAL_MANAGER_STATUS: // 60011 + _protocol = PROTOCOL_STORM32_GIMBAL_MANAGER; + _protocol_auto_cntdown = PROTOCOL_AUTO_TIMEOUT_CNT; + break; + } +} + + +//------------------------------------------------------ +// MAVLink handle functions +//------------------------------------------------------ + +void BP_Mount_STorM32_MAVLink::handle_gimbal_device_information(const mavlink_message_t &msg) +{ + // this msg is not from our gimbal + if (msg.sysid != _sysid || msg.compid != _compid) { + return; + } + + mavlink_msg_gimbal_device_information_decode(&msg, &_device_info); + + // set parameter defaults from gimbal information + // Q: why default ?? why not actual value ?? I don't understand this +/* + if (!isnan(_device_info.roll_min)) _params.roll_angle_min.set_default(degrees(_device_info.roll_min)); + if (!isnan(_device_info.roll_max)) _params.roll_angle_max.set_default(degrees(_device_info.roll_max)); + if (!isnan(_device_info.pitch_min)) _params.pitch_angle_min.set_default(degrees(_device_info.pitch_min)); + if (!isnan(_device_info.pitch_max)) _params.pitch_angle_max.set_default(degrees(_device_info.pitch_max)); + if (!isnan(_device_info.yaw_min)) _params.yaw_angle_min.set_default(degrees(_device_info.yaw_min)); + if (!isnan(_device_info.yaw_max)) _params.yaw_angle_max.set_default(degrees(_device_info.yaw_max)); */ + + if (!isnan(_device_info.roll_min)) _params.roll_angle_min.set(degrees(_device_info.roll_min)); + if (!isnan(_device_info.roll_max)) _params.roll_angle_max.set(degrees(_device_info.roll_max)); + if (!isnan(_device_info.pitch_min)) _params.pitch_angle_min.set(degrees(_device_info.pitch_min)); + if (!isnan(_device_info.pitch_max)) _params.pitch_angle_max.set(degrees(_device_info.pitch_max)); + if (!isnan(_device_info.yaw_min)) _params.yaw_angle_min.set(degrees(_device_info.yaw_min)); + if (!isnan(_device_info.yaw_max)) _params.yaw_angle_max.set(degrees(_device_info.yaw_max)); + + // mark it as having been found + _got_device_info = true; + + // display gimbal info to user + send_banner(); +} + + +void BP_Mount_STorM32_MAVLink::handle_gimbal_device_attitude_status(const mavlink_message_t &msg) +{ + // this msg is not from our gimbal + if (msg.sysid != _sysid || msg.compid != _compid) { + return; + } + + // get relevant data + mavlink_gimbal_device_attitude_status_t payload; + mavlink_msg_gimbal_device_attitude_status_decode(&msg, &payload); + + _device.received_flags = payload.flags; + // TODO: handle case when received device_flags are not equal to those we send, set with update_gimbal_device_flags_for_gimbal() + + _device.received_failure_flags = payload.failure_flags; + + // used for health check + _device.received_tlast_ms = AP_HAL::millis(); + + // logging + float roll_rad, pitch_rad, yaw_rad; + GimbalQuaternion quat(payload.q[0], payload.q[1], payload.q[2], payload.q[3]); + quat.to_gimbal_euler(roll_rad, pitch_rad, yaw_rad); + + BP_LOG("MTS0", BP_LOG_MTS_ATTITUDESTATUS_HEADER, + degrees(roll_rad), + degrees(pitch_rad), + degrees(yaw_rad), + degrees(payload.delta_yaw), + degrees(AP::ahrs().yaw), + payload.flags, + payload.failure_flags); + + // forward to ground as MOUNT_STATUS message + if (payload.target_system) { // trigger sending of MOUNT_STATUS to ground only if target_sysid = 0 + return; + } + if (!is_primary()) { + return; + } + + MountStatus status = { + .roll_deg = degrees(roll_rad), + .pitch_deg = degrees(pitch_rad), + .yaw_deg = degrees(yaw_rad) }; + + send_mount_status_to_ground(status); // MissionPlaner now "understands" gimbal device attitude status, but doesn't use it for campoint, so we still need to send +} + + +void BP_Mount_STorM32_MAVLink::handle_msg(const mavlink_message_t &msg) +{ + if (!_initialised && _compid) { + if (_protocol == PROTOCOL_UNDEFINED) determine_protocol(msg); + return; + } + + switch (msg.msgid) { + // listen to qshot commands and messages to track changes in qshot mode + // these may come from anywhere + case MAVLINK_MSG_ID_COMMAND_LONG: { // 76 + mavlink_command_long_t payload; + mavlink_msg_command_long_decode(&msg, &payload); + switch (payload.command) { + case MAV_CMD_QSHOT_DO_CONFIGURE: // 60020 + uint8_t new_mode = payload.param1; + if (new_mode == MAV_QSHOT_MODE_UNDEFINED) { + _qshot.mode = MAV_QSHOT_MODE_UNDEFINED; + } + if (new_mode != _qshot.mode) { + _qshot.mode = UINT8_MAX; // mode change requested, so put it into hold, must be acknowledged by qshot status + } + break; + } + }break; + + case MAVLINK_MSG_ID_QSHOT_STATUS: { // 60020 + mavlink_qshot_status_t payload; + mavlink_msg_qshot_status_decode(&msg, &payload); + _qshot.mode = payload.mode; // also sets it if it was put on hold in the above + }break; + + // listen to RADIO_RC_CHANNELS messages to stop sending RC_CHANNELS + case MAVLINK_MSG_ID_RADIO_RC_CHANNELS: { // 60045 + _got_radio_rc_channels = true; + }break; + } + + // this msg is not from our system + if (msg.sysid != _sysid) { + return; + } + + // search for a MAVLink camera + // we are somewhat overly strict in that we require both the comp_id and the mav_type to be camera + if (!_camera_compid && (msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) && + (msg.compid >= MAV_COMP_ID_CAMERA) && (msg.compid <= MAV_COMP_ID_CAMERA6)) { + mavlink_heartbeat_t payload; + mavlink_msg_heartbeat_decode(&msg, &payload); + if ((payload.autopilot == MAV_AUTOPILOT_INVALID) && (payload.type == MAV_TYPE_CAMERA)) { + _camera_compid = msg.compid; + } + } + + // listen to STORM32_GIMBAL_MANGER_STATUS to detect activity of the autopilot client + switch (msg.msgid) { + case MAVLINK_MSG_ID_STORM32_GIMBAL_MANAGER_STATUS: { // 60011 + mavlink_storm32_gimbal_manager_status_t payload; + mavlink_msg_storm32_gimbal_manager_status_decode(&msg, &payload); + if (payload.gimbal_id != _compid) break; // not for our gimbal device + _manager.ap_client_is_active = + (payload.supervisor != MAV_STORM32_GIMBAL_MANAGER_CLIENT_NONE) && // a client is supervisor + (payload.manager_flags & MAV_STORM32_GIMBAL_MANAGER_FLAGS_CLIENT_AUTOPILOT_ACTIVE); // and autopilot is active + }break; + } + + // this msg is not from our gimbal + if (msg.sysid != _sysid || msg.compid != _compid) { + return; + } + + switch (msg.msgid) { + case MAVLINK_MSG_ID_HEARTBEAT: { + mavlink_heartbeat_t payload; + mavlink_msg_heartbeat_decode(&msg, &payload); + uint8_t storm32_state = (payload.custom_mode & 0xFF); + _armed = ((storm32_state == STORM32STATE_NORMAL) || (storm32_state == STORM32STATE_STARTUP_FASTLEVEL)); + if ((payload.custom_mode & 0x80000000) == 0) { // we don't follow all changes, but just toggle it to true once + _prearmchecks_ok = true; + } + if (_armingchecks_enabled && !_prearmchecks_all_ok && prearmchecks_all_ok()) { + _prearmchecks_all_ok = true; + gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); + } + }break; + + #define MAVGIMBAL_EVENT_ID(x) ((uint32_t)154 << 24) + (uint32_t)(x) + + case MAVLINK_MSG_ID_EVENT: { + mavlink_event_t payload; + mavlink_msg_event_decode(&msg, &payload); + if (payload.id != MAVGIMBAL_EVENT_ID(0)) break; // not our event id + struct PACKED tPrearmCheckEventArgument { + uint32_t enabled_flags; + uint32_t fail_flags; + }; + tPrearmCheckEventArgument* p = (tPrearmCheckEventArgument*)payload.arguments; + _prearmcheck.enabled_flags = p->enabled_flags; + _prearmcheck.fail_flags = p->fail_flags & p->enabled_flags; // only keep enabled flags + _prearmcheck.updated = true; +// gcs().send_text(MAV_SEVERITY_INFO, "Event: %d %d", (int)_prearmcheck.enabled_flags, (int)_prearmcheck.fail_flags); + if (_prearmcheck.fail_flags != _prearmcheck.fail_flags_last) { + _prearmcheck.fail_flags_last = _prearmcheck.fail_flags; + _prearmcheck.sendtext_tlast_ms = 0; // a change occurred, so force send immediately + } + }break; + } +} + + +//------------------------------------------------------ +// MAVLink send functions +//------------------------------------------------------ + +void BP_Mount_STorM32_MAVLink::send_request_gimbal_device_information(void) +{ + if (!HAVE_PAYLOAD_SPACE(_chan, COMMAND_LONG)) { + return; + } + + mavlink_msg_command_long_send( + _chan, + _sysid, _compid, + MAV_CMD_REQUEST_MESSAGE, 0, + MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, 0, 0, 0, 0, 0, 0); +} + + +void BP_Mount_STorM32_MAVLink::GimbalTarget::get_q_array(float* q_array) +{ + switch (mode) { + case TARGET_MODE_NEUTRAL: + case TARGET_MODE_ANGLE: { + Quaternion q; + q.from_euler(roll, pitch, yaw); + q_array[0] = q.q1; q_array[1] = q.q2; q_array[2] = q.q3; q_array[3] = q.q4; + break; + } + default: + q_array[0] = q_array[1] = q_array[2] = q_array[3] = NAN; + } +} + + +void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude(GimbalTarget >arget) +{ + if (!HAVE_PAYLOAD_SPACE(_chan, GIMBAL_DEVICE_SET_ATTITUDE)) { + return; + } + + float q_array[4]; + gtarget.get_q_array(q_array); + + mavlink_msg_gimbal_device_set_attitude_send( + _chan, + _sysid, _compid, + _device.flags_for_gimbal, // gimbal device flags + q_array, // attitude as a quaternion + NAN, NAN, NAN); // angular velocities + + BP_LOG("MTC0", BP_LOG_MTC_GIMBALCONTROL_HEADER, + (uint8_t)1, // GIMBAL_DEVICE_SET_ATTITUDE + degrees(gtarget.roll), degrees(gtarget.pitch), degrees(gtarget.yaw), + _device.flags_for_gimbal, (uint16_t)0, + gtarget.mode, + (uint8_t)0); +} + + +void BP_Mount_STorM32_MAVLink::send_storm32_gimbal_manager_control(GimbalTarget >arget) +{ + if (!HAVE_PAYLOAD_SPACE(_chan, STORM32_GIMBAL_MANAGER_CONTROL)) { + return; + } + + float q_array[4]; + gtarget.get_q_array(q_array); + + mavlink_msg_storm32_gimbal_manager_control_send( + _chan, + _sysid, _compid, + _compid, // gimbal_id + MAV_STORM32_GIMBAL_MANAGER_CLIENT_AUTOPILOT, + _device.flags_for_gimbal, _manager.flags_for_gimbal, + q_array, + NAN, NAN, NAN); // float angular_velocity_x, float angular_velocity_y, float angular_velocity_z + + BP_LOG("MTC0", BP_LOG_MTC_GIMBALCONTROL_HEADER, + (uint8_t)2, // STORM32_GIMBAL_MANAGER_CONTROL + degrees(gtarget.roll), degrees(gtarget.pitch), degrees(gtarget.yaw), + _device.flags_for_gimbal, _manager.flags_for_gimbal, + gtarget.mode, + _qshot.mode); +} + + +void BP_Mount_STorM32_MAVLink::send_autopilot_state_for_gimbal_device_ext(void) +{ + if (!HAVE_PAYLOAD_SPACE(_chan, AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_EXT)) { + return; + } + + const AP_AHRS &ahrs = AP::ahrs(); + +#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) + Vector3f airspeed_vec_bf; + if (!ahrs.airspeed_vector_true(airspeed_vec_bf)) { + // if we don't have an airspeed estimate then we don't have a valid wind estimate on copters + return; + } +#endif + +/* + https://github.com/ArduPilot/ardupilot/issues/6571 + v_ground = v_air + v_wind // note direction of wind + there are soo many different accessors, estimates, etc + vfr_hud_airspeed(), + ahrs.groundspeed() + ahrs.groundspeed_vector() + ahrs.yaw, ahrs.yaw_sensor + ahrs.airspeed_estimate(airspeed) + ahrs.airspeed_estimate_true(airspeed) + ahrs.airspeed_vector_true(airspeed_vec_bf) // gives it in BF, not NED! + AP::gps().ground_speed() + AP_Airspeed::get_singleton()->get_airspeed() + ???? + this gives some good insight!? + ahrs.groundspeed_vector() -> search for AP_AHRS_DCM::groundspeed_vector(void) + if airspeed_estimate_true(airspeed) then calculates it as gndVelADS = airspeed_vector + wind2d + else if gotGPS then gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * AP::gps().ground_speed() + => AP does indeed do vg = va + vw + => shows how ground speed is estimated + question: how does this relate to ahrs.get_velocity_NED(vground)? + ahrs.airspeed_estimate(airspeed) + does true_airspeed_vec = nav_vel - wind_vel + this suggests that nav_vel is a good ground speed + also suggests that airspeed, wind are in NED too +*/ + Vector3f wind; + wind = ahrs.wind_estimate(); + + float vehicle_heading = ahrs.yaw; + float wind_heading = atan2f(-wind.y, -wind.x); + float ground_heading = NAN; + float air_heading = NAN; + + float correction_angle = NAN; + Vector3f vground; + if (ahrs.get_velocity_NED(vground)) { // ??? is this correct, really ground speed ??? + Vector3f vair = vground - wind; + float vg2 = vground.x * vground.x + vground.y * vground.y; + float va2 = vair.x * vair.x + vair.y * vair.y; + float vgva = vground.x * vair.x + vground.y * vair.y; + correction_angle = acosf(vgva / sqrtf(vg2 * va2)); + if ((vground.x * vair.y - vground.y * vair.x) < 0.0f) correction_angle = -correction_angle; + + ground_heading = atan2f(vground.y, vground.x); + air_heading = atan2f(vair.y, vair.x); + } +/* we currently don't send, just log, we need to work it out what we want to do + mavlink_msg_autopilot_state_for_gimbal_device_ext_send( + _chan, + _sysid, _compid, + AP_HAL::micros64(), + wind.x, wind.y, correction_angle); */ + + BP_LOG("MTLE", BP_LOG_MTLE_AUTOPILOTSTATEEXT_HEADER, + wind.x, wind.y, degrees(correction_angle), + degrees(vehicle_heading), degrees(wind_heading), degrees(ground_heading), degrees(air_heading)); +} + + +enum THISWOULDBEGREATTOHAVE { + MAV_LANDED_STATE_PREPARING_FOR_TAKEOFF = 5, +}; + + +void BP_Mount_STorM32_MAVLink::send_autopilot_state_for_gimbal_device(void) +{ + if (!HAVE_PAYLOAD_SPACE(_chan, AUTOPILOT_STATE_FOR_GIMBAL_DEVICE)) { + return; + } + + const AP_AHRS &ahrs = AP::ahrs(); + + Quaternion quat; + if (!ahrs.get_quaternion(quat)) { // it returns a bool, so it's a good idea to consider it + quat.q1 = quat.q2 = quat.q3 = quat.q4 = NAN; + } + float q[4] = { quat.q1, quat.q2, quat.q3, quat.q4 }; + + // comment in AP_AHRS.cpp says "Must only be called if have_inertial_nav() is true", but probably not worth checking + Vector3f vel; + if (!ahrs.get_velocity_NED(vel)) { // it returns a bool, so it's a good idea to consider it + vel.x = vel.y = vel.z = 0.0f; // or NAN ??? + } + + float angular_velocity_z = ahrs.get_yaw_rate_earth(); // NAN; + + float yawrate = NAN; + // see https://github.com/ArduPilot/ardupilot/issues/22564 + const AP_Vehicle *vehicle = AP::vehicle(); + Vector3f rate_ef_targets; + if ((vehicle != nullptr) && vehicle->get_rate_ef_targets(rate_ef_targets)) { + yawrate = rate_ef_targets.z; + } + +// TODO: how do notify.flags.armed and hal.util->get_soft_armed() compare against each other, also across vehicles? +/* old: + bool ahrs_nav_status_horiz_vel = false; + nav_filter_status nav_status; + if (ahrs.get_filter_status(nav_status) && nav_status.flags.horiz_vel) { + ahrs_nav_status_horiz_vel = true; + } + + uint8_t status = 0; + if (ahrs.healthy()) { status |= STORM32LINK_FCSTATUS_AP_AHRSHEALTHY; } + if (ahrs.initialised()) { status |= STORM32LINK_FCSTATUS_AP_AHRSINITIALIZED; } + if (ahrs_nav_status_horiz_vel) { status |= STORM32LINK_FCSTATUS_AP_NAVHORIZVEL; } + if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { status |= STORM32LINK_FCSTATUS_AP_GPS3DFIX; } + if (notify.flags.armed) { status |= STORM32LINK_FCSTATUS_AP_ARMED; } + //if (hal.util->get_soft_armed()) { status |= STORM32LINK_FCSTATUS_AP_ARMED; } + // for copter this is !ap.land_complete, for plane this is new_is_flying + if (notify.flags.flying) { status |= STORM32LINK_FCSTATUS_AP_ISFLYING; } +*/ +/* +mavlink flags nav_filter_status +ESTIMATOR_ATTITUDE attitude : 1; // 0 - true if attitude estimate is valid +ESTIMATOR_VELOCITY_HORIZ horiz_vel : 1; // 1 - true if horizontal velocity estimate is valid +ESTIMATOR_VELOCITY_VERT vert_vel : 1; // 2 - true if the vertical velocity estimate is valid +ESTIMATOR_POS_HORIZ_REL horiz_pos_rel : 1; // 3 - true if the relative horizontal position estimate is valid +ESTIMATOR_POS_HORIZ_ABS horiz_pos_abs : 1; // 4 - true if the absolute horizontal position estimate is valid +ESTIMATOR_POS_VERT_ABS vert_pos : 1; // 5 - true if the vertical position estimate is valid +ESTIMATOR_POS_VERT_AGL terrain_alt : 1; // 6 - true if the terrain height estimate is valid +ESTIMATOR_CONST_POS_MODE const_pos_mode : 1; // 7 - true if we are in const position mode +ESTIMATOR_PRED_POS_HORIZ_REL pred_horiz_pos_rel : 1; // 8 - true if filter expects it can produce a good relative horizontal position estimate - used before takeoff +ESTIMATOR_PRED_POS_HORIZ_ABS pred_horiz_pos_abs : 1; // 9 - true if filter expects it can produce a good absolute horizontal position estimate - used before takeoff + +ESTIMATOR_GPS_GLITCH takeoff_detected : 1; // 10 - true if optical flow takeoff has been detected +ESTIMATOR_ACCEL_ERROR takeoff : 1; // 11 - true if filter is compensating for baro errors during takeoff + touchdown : 1; // 12 - true if filter is compensating for baro errors during touchdown + using_gps : 1; // 13 - true if we are using GPS position + gps_glitching : 1; // 14 - true if GPS glitching is affecting navigation accuracy + gps_quality_good : 1; // 15 - true if we can use GPS for navigation + initalized : 1; // 16 - true if the EKF has ever been healthy + rejecting_airspeed : 1; // 17 - true if we are rejecting airspeed data + dead_reckoning : 1; // 18 - true if we are dead reckoning (e.g. no position or velocity source) +*/ + uint16_t nav_estimator_status = 0; + uint16_t nav_estimator_status2 = 0; + + const uint32_t ESTIMATOR_MASK = ( + ESTIMATOR_ATTITUDE | + ESTIMATOR_VELOCITY_HORIZ | ESTIMATOR_VELOCITY_VERT | + ESTIMATOR_POS_HORIZ_REL | ESTIMATOR_POS_HORIZ_ABS | + ESTIMATOR_POS_VERT_ABS | ESTIMATOR_POS_VERT_AGL | + ESTIMATOR_CONST_POS_MODE | + ESTIMATOR_PRED_POS_HORIZ_REL | ESTIMATOR_PRED_POS_HORIZ_ABS); + + nav_filter_status nav_status; + if (ahrs.get_filter_status(nav_status)) { + nav_estimator_status = (uint16_t)(nav_status.value & ESTIMATOR_MASK); + nav_estimator_status2 = (uint16_t)(nav_status.value >> 10); + } + +/* estimator status + btw STorM32 only listens to ESTIMATOR_ATTITUDE and ESTIMATOR_VELOCITY_VERT + ahrs.healthy() becomes true during flip of quaternion => no-go + ahrs.initialised() is simply set after AP_AHRS_NAVEKF_SETTLE_TIME_MS 20000 !! + it becomes true some secs after ESTIMATOR_ATTITUDE|ESTIMATOR_VELOCITY_VERT are set + ESTIMATOR_ATTITUDE can be set during flip of quaternion => no-go + ESTIMATOR_VELOCITY_VERT are set briefly after the quaternion flip ??Q: really, couldn't it also be raised in the flip? + ESTIMATOR_VELOCITY_VERT are set however even if there is no gps or alike! I find it hard to trust the data + => we fake it so: + for ESTIMATOR_ATTITUDE we delay the flag being raised by few secs + for ESTIMATOR_VELOCITY_VERT we check for gps like in AP_AHRS_DCM::groundspeed_vector(void) + btw STorM32 does also check for non-zero velocities for AHRSFix +*/ + uint16_t estimator_status = 0; + static uint32_t tahrs_healthy_ms = 0; + const bool ahrs_healthy = ahrs.healthy(); // it's a bit costly + if (!tahrs_healthy_ms && ahrs_healthy) tahrs_healthy_ms = AP_HAL::millis(); + if (ahrs_healthy && (nav_estimator_status & ESTIMATOR_ATTITUDE) && ((AP_HAL::millis() - tahrs_healthy_ms) > 3000)) { + estimator_status |= ESTIMATOR_ATTITUDE; // -> QFix + if (ahrs.initialised() && (nav_estimator_status & ESTIMATOR_VELOCITY_VERT) && (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D)) { + estimator_status |= ESTIMATOR_VELOCITY_VERT; // -> AHRSFix + } + } + +/* landed state + GCS_Common.cpp: virtual MAV_LANDED_STATE landed_state() const { return MAV_LANDED_STATE_UNDEFINED; } + Copter has it: GCS_MAVLINK_Copter::landed_state(), yields ON_GROUND, TAKEOFF, IN_AIR, LANDING + Plane has it: GCS_MAVLINK_Plane::landed_state(), only yields ON_GROUND or IN_AIR + Blimp also has it, blimp not relevant for us + but is protected, so we needed to mock it up + we probably want to also take into account the arming state to mock something up + ugly as we will have vehicle dependency here +*/ + uint8_t landed_state = gcs().get_landed_state(); + +#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) +// for copter we modify the landed states so as to reflect the 2 sec pre-take-off period +// the way it is done leads to a PREPARING_FOR_TAKEOFF before takeoff, but also after landing! +// we somehow would have to catch that it was flying before to suppress it +// but we don't care, the gimbal will do inits when ON_GROUND, and apply them when transitioned to PREPARING_FOR_TAKEOFF +// it won't do it for other transitions, so e.g. also not for plane + const AP_Notify ¬ify = AP::notify(); + if ((landed_state == MAV_LANDED_STATE_ON_GROUND) && notify.flags.armed) landed_state = MAV_LANDED_STATE_PREPARING_FOR_TAKEOFF; +#endif + + static uint32_t tlast_us = 0; + uint32_t t_us = AP_HAL::micros(); + uint32_t dt_us = t_us - tlast_us; + tlast_us = t_us; + + mavlink_msg_autopilot_state_for_gimbal_device_send( + _chan, + _sysid, _compid, + AP_HAL::micros64(), + q, + 0, // uint32_t q_estimated_delay_us, + vel.x, vel.y, vel.z, + 0, // uint32_t v_estimated_delay_us, + yawrate, + estimator_status, landed_state, + angular_velocity_z); + + BP_LOG("MTL0", BP_LOG_MTL_AUTOPILOTSTATE_HEADER, + q[0],q[1],q[2],q[3], + vel.x, vel.y, vel.z, + degrees(angular_velocity_z), + degrees(yawrate), + estimator_status, + landed_state, + nav_estimator_status, + nav_estimator_status2, + dt_us); +} + + +void BP_Mount_STorM32_MAVLink::send_system_time(void) +{ + if (!HAVE_PAYLOAD_SPACE(_chan, SYSTEM_TIME)) { + return; + } + + uint64_t time_unix = 0; + AP::rtc().get_utc_usec(time_unix); // may fail, leaving time_unix at 0 + + if (!time_unix) return; // no unix time available, so no reason to send + + mavlink_msg_system_time_send( + _chan, + time_unix, + AP_HAL::millis()); +} + + +void BP_Mount_STorM32_MAVLink::send_rc_channels(void) +{ + if (!HAVE_PAYLOAD_SPACE(_chan, RC_CHANNELS)) { + return; + } + + // rc().channel(ch)->get_radio_in(), RC_Channels::get_radio_in(ch), and so on + // these are not the same as hal.rcin->read(), since radio_in can be set by override + // so we use hal.rcin->read() + + #define RCHALIN(ch_index) hal.rcin->read(ch_index) + + mavlink_msg_rc_channels_send( + _chan, + AP_HAL::millis(), + 16, + RCHALIN(0), RCHALIN(1), RCHALIN(2), RCHALIN(3), RCHALIN(4), RCHALIN(5), RCHALIN(6), RCHALIN(7), + RCHALIN(8), RCHALIN(9), RCHALIN(10), RCHALIN(11), RCHALIN(12), RCHALIN(13), RCHALIN(14), RCHALIN(15), + 0, 0, + 0); +} + + +void BP_Mount_STorM32_MAVLink::send_banner(void) +{ + if (_got_device_info) { + // we have lots of info + gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: gimbal at %u%s", _instance+1, _compid, (is_primary()) ? ", is primary" : ""); + + // we can convert the firmware version to STorM32 convention + char c = (_device_info.firmware_version & 0x00FF0000) >> 16; + if (c == '\0') c = ' '; else c += 'a' - 1; + + gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: %s %s v%u.%u%c", + _instance + 1, + _device_info.vendor_name, + _device_info.model_name, + (unsigned)(_device_info.firmware_version & 0x000000FF), + (unsigned)((_device_info.firmware_version & 0x0000FF00) >> 8), + c + ); + + gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: prearm checks %s", _instance+1, (_prearmchecks_all_ok) ? "passed" : "fail"); + + } else + if (_compid) { + // we have some info + gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: gimbal at %u%s", _instance+1, _compid, (is_primary()) ? ", is primary" : ""); + gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: prearm checks %s", _instance+1, (_prearmchecks_all_ok) ? "passed" : "fail"); + + } else { + // we don't know yet anything + gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: no gimbal yet", _instance+1); + } +} + + +//------------------------------------------------------ +// helper +//------------------------------------------------------ +const uint32_t FAILURE_FLAGS = + GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR | + GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR | + GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR | + GIMBAL_DEVICE_ERROR_FLAGS_SOFTWARE_ERROR | + GIMBAL_DEVICE_ERROR_FLAGS_COMMS_ERROR; + // GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER; + + +bool BP_Mount_STorM32_MAVLink::prearmchecks_all_ok(void) +{ + return (_prearmchecks_ok && _armed && ((_device.received_failure_flags & FAILURE_FLAGS) == 0)); +} + + +void BP_Mount_STorM32_MAVLink::send_prearmchecks_txt(void) +{ + if (_prearmcheck.available() && !_prearmchecks_ok) { // we got a new message, and it is still not ok + _prearmcheck.updated = false; + + char txt[255]; + strcpy(txt, ""); + if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_IS_NORMAL) strcat(txt, "arm,"); + if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_IMUS_WORKING) strcat(txt, "imu,"); + if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_MOTORS_WORKING) strcat(txt, "mot,"); + if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_ENCODERS_WORKING) strcat(txt, "enc,"); + if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_VOLTAGE_OK) strcat(txt, "volt,"); + if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_VIRTUALCHANNELS_RECEIVING) strcat(txt, "chan,"); + if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_MAVLINK_RECEIVING) strcat(txt, "mav,"); + if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_STORM32LINK_QFIX) strcat(txt, "qfix,"); + if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_STORM32LINK_WORKING) strcat(txt, "stl,"); + if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_CAMERA_CONNECTED) strcat(txt, "cam,"); + if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_AUX0_LOW) strcat(txt, "aux0,"); + if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_AUX1_LOW) strcat(txt, "aux1,"); + if (txt[0] != '\0') { + txt[strlen(txt)-1] = '\0'; + } else { + strcpy(txt, "unknown"); + } + gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: %s", _instance+1, txt); + } else + if (!_initialised || !_prearmchecks_ok || !_armed) { + gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: arm", _instance+1); + } else + if (_prearmchecks_ok && (_device.received_failure_flags & FAILURE_FLAGS)) { + char txt[255]; + strcpy(txt, ""); + if (_device.received_failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR) strcat(txt, "mot,"); + if (_device.received_failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR) strcat(txt, "enc,"); + if (_device.received_failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR) strcat(txt, "volt,"); + if (txt[0] != '\0') { + txt[strlen(txt)-1] = '\0'; + gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: %s", _instance+1, txt); + } else { + gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAILURE FLAGS", _instance+1); + } + } +} + + +// this is called when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA is set +bool BP_Mount_STorM32_MAVLink::prearmchecks_do(void) +{ + _armingchecks_enabled = true; + + if ((AP_HAL::millis() - _prearmcheck.sendtext_tlast_ms) > 30000) { // we haven't send it for a long time + _prearmcheck.sendtext_tlast_ms = AP_HAL::millis(); + send_prearmchecks_txt(); + } + + if (!_prearmchecks_all_ok && prearmchecks_all_ok()) { // has just changed + _prearmchecks_all_ok = true; + gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); + } + + // unhealthy until gimbal has fully passed the startup sequence + if (!_initialised || !_prearmchecks_ok || !_armed) { + return false; + } + + // unhealthy if attitude status is NOT received within the last second + if ((AP_HAL::millis() - _device.received_tlast_ms) > 1000) { + return false; + } + + // check failure flags + // we also check for GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER, it essentially only means that STorM32 got GIMBAL_DEVICE_SET_ATTITUDE messages + if ((_device.received_failure_flags & FAILURE_FLAGS) > 0) { + return false; + } + + // if we get this far the mount is healthy + return true; +} + + +//------------------------------------------------------ +// Camera +//------------------------------------------------------ + +bool BP_Mount_STorM32_MAVLink::take_picture() +{ + if (_use_3way_photo_video) return false; + + if (_camera_mode == CAMERA_MODE_UNDEFINED) { + _camera_mode = CAMERA_MODE_PHOTO; + send_cmd_do_digicam_configure(false); + } + + if (_camera_mode != CAMERA_MODE_PHOTO) return false; + + send_cmd_do_digicam_control(true); + +// gcs().send_text(MAV_SEVERITY_INFO, "cam take pic"); + + return true; +} + + +bool BP_Mount_STorM32_MAVLink::record_video(bool start_recording) +{ + if (_use_3way_photo_video) return false; + + if (_camera_mode == CAMERA_MODE_UNDEFINED) { + _camera_mode = CAMERA_MODE_VIDEO; + send_cmd_do_digicam_configure(true); + } + + if (_camera_mode != CAMERA_MODE_VIDEO) return false; + + send_cmd_do_digicam_control(start_recording); + +// gcs().send_text(MAV_SEVERITY_INFO, "cam rec video %u", start_recording); + + return true; +} + + +bool BP_Mount_STorM32_MAVLink::set_cam_mode(bool video_mode) +{ + if (_use_3way_photo_video) return false; + + _camera_mode = (video_mode) ? CAMERA_MODE_VIDEO : CAMERA_MODE_PHOTO; + send_cmd_do_digicam_configure(video_mode); + +// gcs().send_text(MAV_SEVERITY_INFO, "cam set mode %u", video_mode); + + return true; +} + + +bool BP_Mount_STorM32_MAVLink::set_cam_photo_video(int8_t sw_flag) +{ + if (!_use_3way_photo_video) return false; + + if (sw_flag > 0) { + if (_camera_mode != CAMERA_MODE_VIDEO) { + _camera_mode = CAMERA_MODE_VIDEO; + send_cmd_do_digicam_configure(true); + } + send_cmd_do_digicam_control(true); + } else + if (sw_flag < 0) { + if (_camera_mode != CAMERA_MODE_PHOTO) { + _camera_mode = CAMERA_MODE_PHOTO; + send_cmd_do_digicam_configure(false); + } + send_cmd_do_digicam_control(true); + } else { + if (_camera_mode == CAMERA_MODE_VIDEO) { + send_cmd_do_digicam_control(false); + } + } + + return true; +} + + +void BP_Mount_STorM32_MAVLink::send_cmd_do_digicam_configure(bool video_mode) +{ + if (!HAVE_PAYLOAD_SPACE(_chan, COMMAND_LONG)) { + return; + } + + float param1 = (video_mode) ? 1 : 0; + + mavlink_msg_command_long_send( + _chan, + _sysid, _compid, + MAV_CMD_DO_DIGICAM_CONFIGURE, 0, + param1, 0, 0, 0, 0, 0, 0); + +// gcs().send_text(MAV_SEVERITY_INFO, "cam digi config %u", video_mode); +} + + +void BP_Mount_STorM32_MAVLink::send_cmd_do_digicam_control(bool shoot) +{ + if (!HAVE_PAYLOAD_SPACE(_chan, COMMAND_LONG)) { + return; + } + + float param5 = (shoot) ? 1 : 0; + + mavlink_msg_command_long_send( + _chan, + _sysid, _compid, + MAV_CMD_DO_DIGICAM_CONTROL, 0, + 0, 0, 0, 0, param5, 0, 0); + +// gcs().send_text(MAV_SEVERITY_INFO, "cam digi cntrl %u", shoot); +} + + +//------------------------------------------------------ +// MAVLink MOUNT_STATUS forwarding +//------------------------------------------------------ + +// forward a MOUNT_STATUS message to ground, this is only to make MissionPlanner and alike happy +void BP_Mount_STorM32_MAVLink::send_mount_status_to_ground(MountStatus &status) +{ + // space is checked by send_to_ground() + + mavlink_mount_status_t msg = { + pointing_a : (int32_t)(status.pitch_deg * 100.0f), + pointing_b : (int32_t)(status.roll_deg * 100.0f), + pointing_c : (int32_t)(status.yaw_deg * 100.0f), + target_system : 0, + target_component : 0 }; + + send_to_ground(MAVLINK_MSG_ID_MOUNT_STATUS, (const char*)&msg); +} + + +// this is essentially GCS::send_to_active_channels(uint32_t msgid, const char *pkt) +// but exempts the gimbal channel +//TODO: Is dodgy as it assumes that ONLY the gimbal is on the link !! +// We actually only need to send to the ground, i.e., the gcs-es (but there could be more than one) +// This is what this achieves for gcs-ap-g or gcs-ap-cc-g topologies, but not for others +void BP_Mount_STorM32_MAVLink::send_to_ground(uint32_t msgid, const char *pkt) +{ + const mavlink_msg_entry_t* entry = mavlink_get_msg_entry(msgid); + + if (entry == nullptr) { + return; + } + + for (uint8_t i=0; imax_msg_len + GCS_MAVLINK::packet_overhead_chan(c.get_chan()) > c.get_uart()->txspace()) { + continue; // no space on this channel + } + c.send_message(pkt, entry); + } +} + + + +/* +STorM32-Link tests + +tests V4.3.2-rc1 +logging starts only ca 8-12 secs after power up +ahrs.healthy() becomes true during "flip" of quaternion !! +ahrs.initialised() is simply set after AP_AHRS_NAVEKF_SETTLE_TIME_MS 20000 !! + + 231 167 831 895 +1: ATTITUDE 1 1 1 1 +2: VELOCITY_HORIZ 1 1 1 1 +4: VELOCITY_VERT 1 1 1 1 +8: POS_HORIZ_REL 0 0 1 1 +16: POS_HORIZ_ABS 0 0 1 1 +32: POS_VERT_ABS 1 1 1 1 +64: POS_VERT_AGL 1 0 0 1 +128: CONST_POS_MODE 1 1 0 0 +256: PRED_POS_HORIZ_REL - - 1 1 +512: PRED_POS_HORIZ_ABS - - 1 1 + +test #1 (2022-12-17 10-28-00.bin): +t Est NavEst + 0 0 +26.0s 1 0 happens during "flip" of quaternion !! +26.8s 1 231 ATTITUDE|VELOCITY_HORIZ|VELOCITY_VERT|POS_VERT_ABS + more +31.3s 5 231 +47.4s 5 167 +62.6s 5 831 ATTITUDE|VELOCITY_HORIZ|VELOCITY_VERT|POS_VERT_ABS + POS_HORIZ_REL|POS_HORIZ_ABS +90.4s 5 895 happens briefly after landed went from 5 to 3 + +test #2 (2022-12-17 11-24-40.bin): +t Est NavEst + 0 0 +23.2s 1 0 +24.0s 1 167 +28.6s 5 167 +60.0s 5 831 +78.4s 5 895 happens briefly after landed went from 5 to 3 + +test #3 (2022-12-17 11-28-07.bin): +t Est NavEst + 0 0 +24.2s 1 0 +25.0s 1 167 +29.5s 5 167 +61.4s 5 831 +75.8s 5 895 happens briefly after landed went from 5 to 3 + +2.Jan.2023 +the quaternion flip can also be seen in MP in e.g. yaw, when one connects quickly, EKF dialog looks ok then +*/ + +/* +landed state: + this is not nice, but kind of the best we can currently do + copter & plane do support it, but has it private, and in GCS_MAVLINK + so we end up redoing it in vehicle dependent way, which however gives us also the chance to do it better + +copter's landed state + copter.ap.land_complete <-> MAV_LANDED_STATE_ON_GROUND + copter.flightmode->is_landing() <-> MAV_LANDED_STATE_LANDING + copter.flightmode->is_taking_off() <-> MAV_LANDED_STATE_TAKEOFF + else <-> MAV_LANDED_STATE_IN_AIR + +from tests 2021-08-28, in loiter with takeoff/land button, I conclude + get_landed_state(): + 1 = MAV_LANDED_STATE_ON_GROUND until motors ramp up + 3 = MAV_LANDED_STATE_TAKEOFF for a moment of gaining height + 2 = MAV_LANDED_STATE_IN_AIR during flight + 4 = MAV_LANDED_STATE_LANDING while landing + 1 = MAV_LANDED_STATE_ON_GROUND after landing + seems to exactly do what it is supposed to do, but doesn't reflect 2 sec pre-takeoff + SL status: + 143 = 0x8F until ca 4 sec before take off + 207 = 0xCF = ARMED for ca 4 sec until take off + 239 = 0xEF = 0x20 + ARMED at take off and in flight + 143 = 0x8F after landing + this thus allows to catch the 4 sec pre-takeoff period + ARMING_DELAY_SEC 2.0f, MOT_SAFE_TIME 1.0f per default + with taking-off & landing in loiter with sticks, we get the same behavior at takeoff, + but when landing the state MAV_LANDED_STATE_LANDING is not there, makes sense as we just drop to ground +*/ diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h new file mode 100644 index 0000000000000..f66d675c656b8 --- /dev/null +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -0,0 +1,258 @@ +//***************************************************** +//OW +// (c) olliw, www.olliw.eu, GPL3 +// STorM32 mount backend class +//***************************************************** +#pragma once + +#include "AP_Mount.h" +#include "AP_Mount_Backend.h" + + +#define PROTOCOL_AUTO_TIMEOUT_CNT 10 + + +class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend +{ + +public: + // Constructor + BP_Mount_STorM32_MAVLink(AP_Mount &frontend, AP_Mount_Params ¶ms, uint8_t instance); + + // init - performs any required initialisation for this instance + void init() override {} + + // update mount position - should be called periodically + void update() override; + + // used for gimbals that need to read INS data at full rate + void update_fast() override {} + + // return true if healthy + // is called by mount pre_arm_checks() (and nowhere else) + // which in turn is called in AP_Arming::pre_arm_checks() (and nowhere else) + // mount prearm checks are tied to camera, and enabled by setting + // thus Gremsy's method for e.g. checking attitude_status doesn't make much sense + bool healthy() const override; + + // returns true if this mount can control its pan (required for multicopters) + // STorM32: work it out later + // false: copter is rotated in yaw, not the gimbal + // this is evaluated in + // - GCS_MAVLINK_Copter::handle_command_mount(), cmd = MAV_CMD_DO_MOUNT_CONTROL + // - GCS_MAVLINK_Copter::handle_mount_message(), msg = MAVLINK_MSG_ID_MOUNT_CONTROL + // - Copter Mode::AutoYaw::set_roi() + // - Copter ModeAuto::do_mount_control() + bool has_pan_control() const override { return false; } + + // set yaw_lock + // STorM32: this needs to not set _yaw_lock, as + // (1) STorM32 doesn't (yet) support earth frame, it does support yaw lock in vehicle frame + // (2) _yaw_lock and vehicle or earth frame are not fundamentally related to each other + // in the backend, _yaw_lock is used as yaw_is_ef + void set_yaw_lock(bool yaw_lock) override { _is_yaw_lock = yaw_lock; } + + // handle GIMBAL_DEVICE_INFORMATION message + void handle_gimbal_device_information(const mavlink_message_t &msg) override; + + // handle GIMBAL_DEVICE_ATTITUDE_STATUS message + void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) override; + + // send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS + // STorM32: this needs to be empty, because the gimbal device is sending it itself + // we don't do any of ArduPilot's private mavlink channel nonsense + void send_gimbal_device_attitude_status(mavlink_channel_t chan) override {} + + // take a picture. returns true on success + // we do not need to do anything since AP_Camera::take_picture() will send a a CMD_LONG:DO_DIGICAM_CONTROL to all components + // we have modified this such that it is not send if it is CamTrigType::mount (CAM_TRIGG_TYPE = 3) + bool take_picture() override; + + // start or stop video recording. returns true on success + // set start_recording = true to start record, false to stop recording + bool record_video(bool start_recording) override; + + // set camera zoom step. returns true on success + // zoom out = -1, hold = 0, zoom in = 1 +//XX ?? bool set_zoom_step(int8_t zoom_step) override { return false; } + + // set photo or video mode + bool set_cam_mode(bool video_mode) override; + + // 3-way switch mode + bool set_cam_photo_video(int8_t sw_flag) override; + +protected: + + // get attitude as a quaternion. returns true on success + // this is used in send_gimbal_device_attitude_status() + // STorM32: empty it, send_gimbal_device_attitude_status() has been emptied also, so no use + bool get_attitude_quaternion(Quaternion& att_quat) override { return false; } + +private: + + // search for gimbal in GCS_MAVLink routing table + void find_gimbal(void); + + // request GIMBAL_DEVICE_INFORMATION from gimbal (holds vendor and model name, min/max angles) + void send_request_gimbal_device_information(void); + +// -- this so far is the interface provided by AP_Mount_Backend and as seen in the Gremsy driver +// -- here now come our methods +public: + + // handle msg - allows to process a msg from a gimbal + void handle_msg(const mavlink_message_t &msg) override; + + // send banner + void send_banner(void) override; + +private: + + // internal variables + + bool _got_device_info; // true once gimbal has provided device info + bool _initialised; // true once startup procedure has been fully completed + bool _armed; // true once the gimbal has reached normal state + bool _prearmchecks_ok; // true when the gimbal stops reporting prearm fail in the HEARTBEAT message + bool _got_radio_rc_channels; // true when a RADIO_RC_CHANNELS message has been received + + // internal MAVLink variables + + uint8_t _sysid; // system id of gimbal, supposedly equal to our flight controller sysid + uint8_t _compid; // component id of gimbal, 0 indicates gimbal not yet found + mavlink_channel_t _chan; // mavlink channel used to communicate with gimbal + + // initialization etc + + uint32_t _request_device_info_tlast_ms; + mavlink_gimbal_device_information_t _device_info; + + // flags + + bool _sendonly; + bool _send_autopilotstateext; + bool _should_log; + bool _use_3way_photo_video; + + enum PROTOCOLENUM { + PROTOCOL_UNDEFINED = 0, // we do not yet know + PROTOCOL_ARDUPILOT_LIKE, // gimbal uses V2 gimbal device messages + PROTOCOL_STORM32_GIMBAL_MANAGER, // gimbal is a STorM32 gimbal manager + }; + uint8_t _protocol; + + // internal task variables + + enum TASKENUM { + TASK_SLOT0 = 0, + TASK_SLOT1, + TASK_SLOT2, + TASK_SLOT3, + }; + + uint16_t _task_counter; + + // blabla + + uint8_t _protocol_auto_cntdown; + void determine_protocol(const mavlink_message_t &msg); + + bool _armingchecks_enabled; // true when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA set + bool _prearmchecks_all_ok; // becomes true when a number of conditions are full filled, track changes + bool prearmchecks_all_ok(void); + bool prearmchecks_do(void); // workaround needed since healthy() is const + void send_prearmchecks_txt(void); + + struct { + bool updated; // set to true when a new EVENT message is received + uint32_t enabled_flags; + uint32_t fail_flags; + uint32_t fail_flags_last; + uint32_t sendtext_tlast_ms; + bool available(void) { return updated && (enabled_flags > 0) && (fail_flags > 0); } + } _prearmcheck; // for component prearm status handling + + bool _is_yaw_lock; + + void set_and_send_target_angles(void); + void send_autopilot_state_for_gimbal_device(void); + void send_autopilot_state_for_gimbal_device_ext(void); + + enum GIMBALTARGETMODEENUM { + TARGET_MODE_RETRACT = 0, + TARGET_MODE_NEUTRAL, + TARGET_MODE_ANGLE, + }; + + struct GimbalTarget { + float roll; + float pitch; + float yaw; + bool yaw_is_ef; + uint8_t mode; + + void set(MountTarget &t, uint8_t m) { roll = t.roll; pitch = t.pitch; yaw = t.yaw; yaw_is_ef = t.yaw_is_ef; mode = m; } + void set(uint8_t m) { roll = pitch = yaw = 0.0f; yaw_is_ef = false; mode = m; } + void set_angle(MountTarget &t) { set(t, TARGET_MODE_ANGLE); } + void set_from_vec_deg(const Vector3f &v_deg, uint8_t m) { roll = ToRad(v_deg.x); pitch = ToRad(v_deg.y); yaw = ToRad(v_deg.z); yaw_is_ef = false; mode = m; } + void get_q_array(float* q_array); + }; + + struct { + uint16_t received_flags; // obtained from GIMBAL_DEVICE_ATTITUDE_STATUS + uint32_t received_failure_flags; // obtained from GIMBAL_DEVICE_ATTITUDE_STATUS + uint32_t received_tlast_ms; // time last GIMBAL_DEVICE_ATTITUDE_STATUS was received (used for health reporting) + uint16_t flags_for_gimbal; + } _device; + + struct { + bool ap_client_is_active; // true when autopilot client is active + uint16_t flags_for_gimbal; + } _manager; + + struct { + uint8_t mode; + uint8_t mode_last; + } _qshot; + + bool get_target_angles_mount(GimbalTarget >arget, enum MAV_MOUNT_MODE mmode); + bool get_target_angles_qshot(GimbalTarget >arget); + void update_gimbal_device_flags_mount(enum MAV_MOUNT_MODE mmode); + void update_gimbal_device_flags_qshot(void); + void update_gimbal_manager_flags(void); + + void send_gimbal_device_set_attitude(GimbalTarget >arget); + void send_storm32_gimbal_manager_control(GimbalTarget >arget); + + uint32_t _send_system_time_tlast_ms; + void send_system_time(void); + + void send_rc_channels(void); + + // camera + + uint8_t _camera_compid; // component id of a mavlink camera, 0 indicates camera not yet found + + enum CAMERAMODEENUM { + CAMERA_MODE_UNDEFINED = 0, // we do not yet know + CAMERA_MODE_PHOTO, + CAMERA_MODE_VIDEO, + }; + uint8_t _camera_mode; // current camera mode + + void send_cmd_do_digicam_configure(bool video_mode); + void send_cmd_do_digicam_control(bool shoot); + + // mount_status forwarding + + struct MountStatus { + float roll_deg; + float pitch_deg; + float yaw_deg; + }; + + void send_mount_status_to_ground(MountStatus &status); + void send_to_ground(uint32_t msgid, const char *pkt); +}; + diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 4b02171695b58..25c1db6c1558a 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -34,6 +34,9 @@ #include "AP_RCProtocol_FPort2.h" #include #include +//OW RADIOLINK +#include "AP_RCProtocol_MavlinkRadio.h" +//OWEND #include @@ -78,6 +81,9 @@ void AP_RCProtocol::init() #if AP_RCPROTOCOL_FPORT_ENABLED backend[AP_RCProtocol::FPORT] = new AP_RCProtocol_FPort(*this, true); #endif +//OW RADIOLINK + backend[AP_RCProtocol::MAVLINK_RADIO] = new AP_RCProtocol_MavlinkRadio(*this); +//OWEND } AP_RCProtocol::~AP_RCProtocol() @@ -481,6 +487,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol) case FPORT2: return "FPORT2"; #endif +//OW RADIOLINK + case MAVLINK_RADIO: + return "MavRadio"; +//OWEND case NONE: break; } @@ -523,4 +533,59 @@ namespace AP { } }; +//OW RADIOLINK +void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) +{ + // this message is also used to check if the receiver is present + // so let's first do the receiver detection + if (_detected_protocol == AP_RCProtocol::NONE) { // we are still searching +//#ifndef IOMCU_FW +//? we need this, but originally it is enclosed by an #ifndef IOMCU_FW +// how does this work? + rc_protocols_mask = rc().enabled_protocols(); +//#endif + if (!protocol_enabled(MAVLINK_RADIO)) return; // not our turn + _detected_protocol = AP_RCProtocol::MAVLINK_RADIO; + } + + // here now comes the message handling itself + + if (_detected_protocol != AP_RCProtocol::MAVLINK_RADIO) { + return; + } + + // update the field, so that the backend can see it + memcpy(&(mavlink_radio.rc_channels), packet, sizeof(mavlink_radio_rc_channels_t)); + mavlink_radio.rc_channels_updated = true; + + // now update the backend + backend[_detected_protocol]->update(); + + // now we can ask the backend if it got a new input + if (backend[_detected_protocol]->new_input()) { + _new_input = true; + _last_input_ms = AP_HAL::millis(); + } +}; + +void AP_RCProtocol::handle_radio_link_stats(mavlink_radio_link_stats_t* packet) +{ +// can be handled like CRSF (= receiver) or like RADIO_STATUS (= telemetry) +// the user does it via RssiType::RECEIVER or RssiType::TELEMETRY_RADIO_RSSI setting +// so isn't decided here, but is decide somewhere higher up in the outside +// this here is needed only in case the user wants RssiType::RECEIVER + + if (_detected_protocol != AP_RCProtocol::MAVLINK_RADIO) { + return; + } + + // update the field, so that the backend can see it + memcpy(&(mavlink_radio.link_stats), packet, sizeof(mavlink_radio_link_stats_t)); + mavlink_radio.link_stats_updated = true; + + // now update the backend + backend[_detected_protocol]->update(); +} +//OWEND + #endif // AP_RCPROTOCOL_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index ca219f5d8580d..00193b0131255 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -20,6 +20,9 @@ #include #include +//OW RADIOLINK +#include +//OWEND #define MAX_RCIN_CHANNELS 18 #define MIN_RCIN_CHANNELS 5 @@ -67,6 +70,9 @@ class AP_RCProtocol { #if AP_RCPROTOCOL_FASTSBUS_ENABLED FASTSBUS = 12, #endif +//OW RADIOLINK + MAVLINK_RADIO = 13, // RC_PROTOCOLS +2^14 = 16384 +//OWEND NONE //last enum always is None }; @@ -143,6 +149,9 @@ class AP_RCProtocol { #if AP_RCPROTOCOL_ST24_ENABLED case ST24: #endif +//OW RADIOLINK + case MAVLINK_RADIO: +//OWEND case NONE: return false; } @@ -189,6 +198,18 @@ class AP_RCProtocol { return _detected_with_bytes; } +//OW RADIOLINK + void handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet); + void handle_radio_link_stats(mavlink_radio_link_stats_t* packet); + + struct { + mavlink_radio_rc_channels_t rc_channels; + bool rc_channels_updated = false; + mavlink_radio_link_stats_t link_stats; + bool link_stats_updated = false; + } mavlink_radio; +//OWEND + private: void check_added_uart(void); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp new file mode 100644 index 0000000000000..1b42ccca98131 --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp @@ -0,0 +1,49 @@ +//***************************************************** +//OW +// (c) olliw, www.olliw.eu, GPL3 +// for mLRS +//***************************************************** + +#include "AP_RCProtocol_MavlinkRadio.h" +#include + + +AP_RCProtocol_MavlinkRadio::AP_RCProtocol_MavlinkRadio(AP_RCProtocol &_frontend) : + AP_RCProtocol_Backend(_frontend) +{ +} + + +void AP_RCProtocol_MavlinkRadio::update(void) +{ + if (frontend.mavlink_radio.rc_channels_updated) { + frontend.mavlink_radio.rc_channels_updated = false; + + uint8_t count = frontend.mavlink_radio.rc_channels.count; + if (count >= MAX_RCIN_CHANNELS) count = MAX_RCIN_CHANNELS; + + uint16_t rc_chan[MAX_RCIN_CHANNELS]; + for (uint8_t i = 0; i < count; i++) { + rc_chan[i] = ((int32_t)frontend.mavlink_radio.rc_channels.channels[i] * 5) / 32 + 1500; + } + + bool failsafe = (frontend.mavlink_radio.rc_channels.flags & RADIO_RC_CHANNELS_FLAGS_FAILSAFE); + + add_input(count, rc_chan, failsafe, rssi, link_quality); + } + + if (frontend.mavlink_radio.link_stats_updated) { + frontend.mavlink_radio.link_stats_updated = false; + + link_quality = frontend.mavlink_radio.link_stats.rx_LQ; + if (frontend.mavlink_radio.link_stats.rx_receive_antenna == UINT8_MAX || + frontend.mavlink_radio.link_stats.rx_rssi2 == UINT8_MAX) { + rssi = frontend.mavlink_radio.link_stats.rx_rssi1; + } else + if (frontend.mavlink_radio.link_stats.rx_receive_antenna == 1) { + rssi = frontend.mavlink_radio.link_stats.rx_rssi2; + } else { + rssi = frontend.mavlink_radio.link_stats.rx_rssi1; + } + } +} diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h new file mode 100644 index 0000000000000..c7b0a7fed1dbb --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h @@ -0,0 +1,22 @@ +//***************************************************** +//OW +// (c) olliw, www.olliw.eu, GPL3 +// for mLRS +//***************************************************** +#pragma once + +#include "AP_RCProtocol.h" + + +class AP_RCProtocol_MavlinkRadio : public AP_RCProtocol_Backend { +public: + + AP_RCProtocol_MavlinkRadio(AP_RCProtocol &_frontend); + + void update(void) override; + +private: + + int16_t rssi = -1; // TODO: can't we just use the backend's fields??? + int16_t link_quality = -1; +}; diff --git a/libraries/AP_RSSI/AP_RSSI.h b/libraries/AP_RSSI/AP_RSSI.h index f46438480013d..a8b2e70eb1f14 100644 --- a/libraries/AP_RSSI/AP_RSSI.h +++ b/libraries/AP_RSSI/AP_RSSI.h @@ -45,6 +45,9 @@ class AP_RSSI // return true if rssi reading is enabled bool enabled() const { return RssiType(rssi_type.get()) != RssiType::TYPE_DISABLED; } +//OW RADIOLINK + bool enabled(RssiType type) const { return RssiType(rssi_type.get()) == type; } +//OWEND // Read the receiver RSSI value as a float 0.0f - 1.0f. // 0.0 represents weakest signal, 1.0 represents maximum signal. diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 3a10a3feabf78..bf3715e30bc44 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4114,11 +4114,13 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) #if AP_EFI_MAV_ENABLED case MAVLINK_MSG_ID_EFI_STATUS: - AP_EFI *efi = AP::EFI(); - if (efi) { - efi->handle_EFI_message(msg); - } - break; + { + AP_EFI *efi = AP::EFI(); + if (efi) { + efi->handle_EFI_message(msg); + } + break; + } #endif //OW RADIOLINK @@ -6867,29 +6869,3 @@ void GCS_MAVLINK::handle_radio_rc_channels(const mavlink_message_t &msg) AP::RC().handle_radio_rc_channels(&packet); } //OWEND -//OW FRPT -// this is tentative, just demo -// we probably want some timing, some packets do not need to be send so often -// maybe we also want to make which packets are send dependent on which streams are enabled -// or vice versa, modify the streams depending on whether frsky passthorugh is send -// one also could make it dependent on which rate is higher - -void GCS_MAVLINK::send_frsky_passthrough_array() -{ - AP_Frsky_SPort_Protocol* pt = AP::frsky_sport_protocol(); - if (pt == nullptr) return; - - uint8_t count = 0; - uint8_t packet_buf[MAVLINK_MSG_FRSKY_PASSTHROUGH_ARRAY_FIELD_PACKET_BUF_LEN] = {0}; // max 40 packets! - - pt->assemble_array(packet_buf, &count, 21, AP_HAL::millis()); - - if (count == 0) return; // nothing to send - - mavlink_msg_frsky_passthrough_array_send( - chan, - AP_HAL::millis(), // time since system boot - count, - packet_buf); -} -//OWEND diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.h b/libraries/GCS_MAVLink/GCS_MAVLink.h index dd4ab4271a553..5d2943f62af2d 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.h +++ b/libraries/GCS_MAVLink/GCS_MAVLink.h @@ -31,7 +31,10 @@ #pragma GCC diagnostic ignored "-Waddress-of-packed-member" #endif -#include "include/mavlink/v2.0/all/version.h" +//OW +//#include "include/mavlink/v2.0/all/version.h" +#include "include/mavlink/v2.0/betapilot/version.h" +//OWEND #define MAVLINK_MAX_PAYLOAD_LEN 255 @@ -67,7 +70,10 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len); uint16_t comm_get_txspace(mavlink_channel_t chan); #define MAVLINK_USE_CONVENIENCE_FUNCTIONS -#include "include/mavlink/v2.0/all/mavlink.h" +//OW +//#include "include/mavlink/v2.0/all/mavlink.h" +#include "include/mavlink/v2.0/betapilot/mavlink.h" +//OWEND // lock and unlock a channel, for multi-threaded mavlink send void comm_send_lock(mavlink_channel_t chan, uint16_t size); diff --git a/libraries/GCS_MAVLink/MAVLink_routing.cpp b/libraries/GCS_MAVLink/MAVLink_routing.cpp index 8015102a472fc..3cb85490f0b8f 100644 --- a/libraries/GCS_MAVLink/MAVLink_routing.cpp +++ b/libraries/GCS_MAVLink/MAVLink_routing.cpp @@ -127,6 +127,17 @@ bool MAVLink_routing::check_and_forward(GCS_MAVLINK &in_link, const mavlink_mess int16_t target_component = -1; get_targets(msg, target_system, target_component); +//OW RADIOLINK + // we want to handle messages from a radio as if they had target_system = our system, target_component = 0 + // we currently narrow it down to "our" messages to play it safe + if ((msg.compid == MAV_COMP_ID_TELEMETRY_RADIO) && + (msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_FLOW_CONTROL || msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_STATS || + msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS)) { + target_system = mavlink_system.sysid; + target_component = 0; + } +//OWEND + bool broadcast_system = (target_system == 0 || target_system == -1); bool broadcast_component = (target_component == 0 || target_component == -1); bool match_system = broadcast_system || (target_system == mavlink_system.sysid); @@ -405,3 +416,32 @@ void MAVLink_routing::get_targets(const mavlink_message_t &msg, int16_t &sysid, } } + +//OW +void MAVLink_routing::send_to_all(uint32_t msgid, const char *pkt) +{ + const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid); + if (entry == nullptr) { + return; + } + + bool sent_to_chan[MAVLINK_COMM_NUM_BUFFERS] {}; + + for (uint8_t i=0; imax_msg_len) + GCS_MAVLINK::packet_overhead_chan(routes[i].channel)) { + continue; + } + _mav_finalize_message_chan_send(routes[i].channel, + entry->msgid, + pkt, + entry->min_msg_len, + entry->max_msg_len, + entry->crc_extra); + sent_to_chan[routes[i].channel] = true; + } +} +//OWEND diff --git a/libraries/GCS_MAVLink/MAVLink_routing.h b/libraries/GCS_MAVLink/MAVLink_routing.h index 0f9fcc4299676..a6a9de7963f33 100644 --- a/libraries/GCS_MAVLink/MAVLink_routing.h +++ b/libraries/GCS_MAVLink/MAVLink_routing.h @@ -49,6 +49,10 @@ class MAVLink_routing */ bool find_by_mavtype_and_compid(uint8_t mavtype, uint8_t compid, uint8_t &sysid, mavlink_channel_t &channel) const; +//OW + void send_to_all(uint32_t msgid, const char *pkt); +//OWEND + private: // a simple linear routing table. We don't expect to have a lot of // routes, so a scalable structure isn't worthwhile yet. diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 8b275ecf46742..af41523605b09 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -1487,6 +1487,14 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos camera->cam_mode_toggle(); break; } +//OW + switch (ch_flag) { + case AuxSwitchPos::HIGH: camera->set_cam_photo_video(1); break; + case AuxSwitchPos::MIDDLE: camera->set_cam_photo_video(0); break; + case AuxSwitchPos::LOW: camera->set_cam_photo_video(-1); break; + } + camera->set_cam_mode(ch_flag == AuxSwitchPos::HIGH); +//OWEND break; } case AUX_FUNC::CAMERA_REC_VIDEO: @@ -1514,12 +1522,21 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos if (mount == nullptr) { break; } +//OW +static enum MAV_MOUNT_MODE mode_last = MAV_MOUNT_MODE_RETRACT; +//OWEND switch (ch_flag) { case AuxSwitchPos::HIGH: +//OW + if (mount->get_mode(0) > MAV_MOUNT_MODE_NEUTRAL) mode_last = mount->get_mode(0); +//OWEND mount->set_mode(0,MAV_MOUNT_MODE_RETRACT); break; case AuxSwitchPos::MIDDLE: // nothing +//OW + if (mode_last > MAV_MOUNT_MODE_NEUTRAL) mount->set_mode(0, mode_last); +//OWEND break; case AuxSwitchPos::LOW: mount->set_mode_to_default(0); diff --git a/libraries/bp_version.h b/libraries/bp_version.h index d77984b8a8b32..4c930a3c4c7f5 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -5,6 +5,9 @@ /* search for //OW to find all changes +2023.08.10: v058 + upgraded to ArduPilot master 4.5.0-dev + 2023.03.10: v058 upgraded to Copter4.3.6 stable 2023.03.10: v057.3 diff --git a/wscript b/wscript index 262cdd8b23fa6..7406b80b3f102 100644 --- a/wscript +++ b/wscript @@ -681,7 +681,10 @@ def _build_dynamic_sources(bld): if not bld.env.BOOTLOADER: bld( features='mavgen', - source='modules/mavlink/message_definitions/v1.0/all.xml', + # //OW + #source='modules/mavlink/message_definitions/v1.0/all.xml', + source='bp_mavlink/betapilot.xml', + # //OWEND output_dir='libraries/GCS_MAVLink/include/mavlink/v2.0/', name='mavlink', # this below is not ideal, mavgen tool should set this, but that's not From c3c3dcc7c7fb2629e89e8a434fe2048fd90adc5a Mon Sep 17 00:00:00 2001 From: olliw42 Date: Thu, 10 Aug 2023 13:04:02 +0200 Subject: [PATCH 003/125] some tidy --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 16 ++++----- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 2 +- libraries/GCS_MAVLink/GCS.h | 1 + libraries/GCS_MAVLink/GCS_Common.cpp | 34 +++++++++---------- libraries/GCS_MAVLink/MAVLink_routing.cpp | 4 +-- 5 files changed, 28 insertions(+), 29 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index f11849deeb68d..7606693d6cbc1 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -526,8 +526,7 @@ void BP_Mount_STorM32_MAVLink::find_gimbal(void) /* // search for a mavlink enabled gimbal - //if (_link == nullptr) { - if (!_compid) { + if (_link == nullptr) { // we expect that instance 0 has compid = MAV_COMP_ID_GIMBAL, instance 1 has compid = MAV_COMP_ID_GIMBAL2, etc uint8_t compid = (_instance == 0) ? MAV_COMP_ID_GIMBAL : MAV_COMP_ID_GIMBAL2 + (_instance - 1); _link = GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_GIMBAL, compid, _sysid); @@ -1425,25 +1424,26 @@ void BP_Mount_STorM32_MAVLink::send_mount_status_to_ground(MountStatus &status) // this is essentially GCS::send_to_active_channels(uint32_t msgid, const char *pkt) // but exempts the gimbal channel //TODO: Is dodgy as it assumes that ONLY the gimbal is on the link !! -// We actually only need to send to the ground, i.e., the gcs-es (but there could be more than one) +// We actually only need to send to the ground, i.e., to the gcs-es (there could be more than one) // This is what this achieves for gcs-ap-g or gcs-ap-cc-g topologies, but not for others void BP_Mount_STorM32_MAVLink::send_to_ground(uint32_t msgid, const char *pkt) { const mavlink_msg_entry_t* entry = mavlink_get_msg_entry(msgid); - if (entry == nullptr) { return; } - for (uint8_t i=0; imax_msg_len + GCS_MAVLINK::packet_overhead_chan(c.get_chan()) > c.get_uart()->txspace()) { - continue; // no space on this channel + if (!c.is_active()) { + continue; + } + if (!c.is_active()) { + continue; } + // size checks done by this method: c.send_message(pkt, entry); } } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 25c1db6c1558a..6f94ecb8f908f 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -571,7 +571,7 @@ void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_t* void AP_RCProtocol::handle_radio_link_stats(mavlink_radio_link_stats_t* packet) { // can be handled like CRSF (= receiver) or like RADIO_STATUS (= telemetry) -// the user does it via RssiType::RECEIVER or RssiType::TELEMETRY_RADIO_RSSI setting +// the user does decide it via RssiType::RECEIVER or RssiType::TELEMETRY_RADIO_RSSI setting // so isn't decided here, but is decide somewhere higher up in the outside // this here is needed only in case the user wants RssiType::RECEIVER diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 1fc4c5d3ece9f..7bd72e04983c7 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -485,6 +485,7 @@ class GCS_MAVLINK //OW RADIOLINK // called from vehicle class handler + mavlink_radio_t _mavlink_radio_packet; void handle_radio_link_stats(const mavlink_message_t &msg, bool log_radio); void handle_radio_link_flow_control(const mavlink_message_t &msg, bool log_radio); // called from common handler diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index bf3715e30bc44..7be9ce73627a9 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -6371,8 +6371,8 @@ bool GCS_MAVLINK::accept_packet(const mavlink_status_t &status, // we want to handle messages from a radio // we currently narrow it down to "ours" to play it safe if ((msg.compid == MAV_COMP_ID_TELEMETRY_RADIO) && - (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS || - msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_FLOW_CONTROL || msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_STATS)) { + (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS || msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_STATS || + msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_FLOW_CONTROL)) { return true; } //OWEND @@ -6751,24 +6751,22 @@ MAV_RESULT GCS_MAVLINK::handle_control_high_latency(const mavlink_command_long_t #endif // HAL_HIGH_LATENCY2_ENABLED //OW RADIOLINK -static mavlink_radio_t ow_mavlink_radio_packet; - void GCS_MAVLINK::handle_radio_link_stats(const mavlink_message_t &msg, bool log_radio) { mavlink_radio_link_stats_t packet; mavlink_msg_radio_link_stats_decode(&msg, &packet); - // let the AP_RCProtocol do it's thing - // is doing a thing if AP_RCProtocol::MAVLINK_RADIO is selected by the user + // let AP_RCProtocol do its thing + // is doing something if AP_RCProtocol::MAVLINK_RADIO is selected by the user // will provide rssi if also AP_RSSI::RssiType::RECEIVER is selected by the user AP::RC().handle_radio_link_stats(&packet); // handle the rssi info - // is doing a thing if AP_RSSI::RssiType::TELEMETRY_RADIO_RSSI is selected by the user + // jump out if AP_RSSI::RssiType::TELEMETRY_RADIO_RSSI is not selected by the user AP_RSSI* rssi = AP::rssi(); if ((rssi != nullptr) && !rssi->enabled(AP_RSSI::RssiType::TELEMETRY_RADIO_RSSI)) return; - // convert it to what we would get from RADIO_STATUS + // convert it to what one would get from RADIO_STATUS uint8_t _rssi = packet.rx_rssi1; uint8_t _remrssi = (packet.tx_rssi1 == UINT8_MAX) ? 0 : packet.tx_rssi1; uint8_t _noise = 0; @@ -6776,7 +6774,7 @@ void GCS_MAVLINK::handle_radio_link_stats(const mavlink_message_t &msg, bool log if (packet.flags & RADIO_LINK_STATS_FLAGS_RSSI_DBM) { // the rssi values are not in MAVLink standard, but negative dBm - // so we convert using ArduPilot's CRSF conversion, see AP_RCProtocol_CRSF::process_link_stats_frame() + // so convert using ArduPilot's CRSF conversion, see AP_RCProtocol_CRSF::process_link_stats_frame() if (_rssi < 50) { _rssi = 254; } else if (_rssi > 120) { @@ -6804,15 +6802,15 @@ void GCS_MAVLINK::handle_radio_link_stats(const mavlink_message_t &msg, bool log last_radio_status.remrssi_ms = now; } - // we fake it for logging - ow_mavlink_radio_packet.rssi = _rssi; - ow_mavlink_radio_packet.remrssi = _remrssi; - ow_mavlink_radio_packet.noise = _noise; - ow_mavlink_radio_packet.remnoise = _remnoise; + // prepare for logging + _mavlink_radio_packet.rssi = _rssi; + _mavlink_radio_packet.remrssi = _remrssi; + _mavlink_radio_packet.noise = _noise; + _mavlink_radio_packet.remnoise = _remnoise; // log rssi, noise, etc if logging Performance monitoring data if (log_radio) { - AP::logger().Write_Radio(ow_mavlink_radio_packet); + AP::logger().Write_Radio(_mavlink_radio_packet); } } @@ -6852,12 +6850,12 @@ void GCS_MAVLINK::handle_radio_link_flow_control(const mavlink_message_t &msg, b } #endif - // we fake it for logging - ow_mavlink_radio_packet.txbuf = _txbuf; + // prepare for logging + _mavlink_radio_packet.txbuf = _txbuf; // log rssi, noise, etc if logging Performance monitoring data if (log_radio) { - AP::logger().Write_Radio(ow_mavlink_radio_packet); + AP::logger().Write_Radio(_mavlink_radio_packet); } } diff --git a/libraries/GCS_MAVLink/MAVLink_routing.cpp b/libraries/GCS_MAVLink/MAVLink_routing.cpp index 3cb85490f0b8f..467a88d10850f 100644 --- a/libraries/GCS_MAVLink/MAVLink_routing.cpp +++ b/libraries/GCS_MAVLink/MAVLink_routing.cpp @@ -131,8 +131,8 @@ bool MAVLink_routing::check_and_forward(GCS_MAVLINK &in_link, const mavlink_mess // we want to handle messages from a radio as if they had target_system = our system, target_component = 0 // we currently narrow it down to "our" messages to play it safe if ((msg.compid == MAV_COMP_ID_TELEMETRY_RADIO) && - (msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_FLOW_CONTROL || msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_STATS || - msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS)) { + (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS || msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_STATS || + msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_FLOW_CONTROL)) { target_system = mavlink_system.sysid; target_component = 0; } From fdc72d70d643d465586df949058949c07bf7e46c Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 1 Nov 2023 10:13:03 +0100 Subject: [PATCH 004/125] cc --- libraries/bp_version.h | 26 ++++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 4c930a3c4c7f5..d2a9eb2ff11c4 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -61,7 +61,7 @@ search for //OW to find all changes upgraded to storm32_lib.h v 17. Nov. 2020 ArduCopter specific -- GCS_Mavlink.cpp: (2 comments, no change) +- GCS_Mavlink.cpp: (2 comments, no change) 1x RADIOLINK - version.h: 1x ArduPlane specific @@ -69,18 +69,28 @@ ArduPlane specific - version.h: 1x Libraries: -- AP_Mount_Backend.cpp: 4x (+3 comments) -- AP_Mount_Backend.h: 1x (+1 comment) -- AP_Mount.cpp: 7x (+1 comment) -- AP_Mount.h: 6x -- GCS_Common.cpp: 2x -- GCS.h: 2x +- AP_Camera.cpp: 1x +- AP_Camera.h: 1x +- AP_Mount_Backend.cpp: ? +- AP_Mount_Backend.h: 1x +- AP_Mount_Params.cpp: 1x +- AP_Mount_Params.h: 1x +- AP_Mount.cpp: 3x +- AP_Mount.h: 8x +- AP_RCProtocol.cpp: 4x RADIOLINK +- AP_RCProtocol.h: 4x RADIOLINK +- AP_RSSI.h: 1x RADIOLINK +- GCS_Common.cpp: 2x 3x RADIOLINK +- GCS_MAVLink.h: 2x +- GCS.h: 2x 1x RADIOLINK +- MAVLink_routing.cpp: 1x 1x RADIOLINK +- MAVLink_routing.h: 1x +- RC_Channel.cpp: 4x Additional Files in library: - bp_version.h - AP_Mount/BP_Mount_STorM32_MAVLink.cpp - AP_Mount/BP_Mount_STorM32_MAVLink.h -- AP_Mount/STorM32_lib.h Effect of SYSID_MYGCS parameter value: From b06e4c648c48b96375c513b383959ee938fb3eaf Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 1 Nov 2023 10:33:20 +0100 Subject: [PATCH 005/125] cc --- libraries/AP_RCProtocol/AP_RCProtocol.h | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index a7f1269ded751..42f62a7fcf6d0 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -70,15 +70,12 @@ class AP_RCProtocol { #if AP_RCPROTOCOL_FASTSBUS_ENABLED FASTSBUS = 12, #endif -<<<<<<< HEAD #if AP_RCPROTOCOL_DRONECAN_ENABLED DRONECAN = 13, #endif -======= //OW RADIOLINK - MAVLINK_RADIO = 13, // RC_PROTOCOLS +2^14 = 16384 + MAVLINK_RADIO = 14, // RC_PROTOCOLS +2^14 = 16384 //OWEND ->>>>>>> BetaPilot NONE //last enum always is None }; @@ -155,15 +152,12 @@ class AP_RCProtocol { #if AP_RCPROTOCOL_ST24_ENABLED case ST24: #endif -<<<<<<< HEAD #if AP_RCPROTOCOL_DRONECAN_ENABLED case DRONECAN: #endif -======= //OW RADIOLINK case MAVLINK_RADIO: //OWEND ->>>>>>> BetaPilot case NONE: return false; } From d9bfea10984e59e701893782592709698a172bed Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 1 Nov 2023 10:56:23 +0100 Subject: [PATCH 006/125] sync bp_mavlink --- bp_mavlink/all.xml | 9 +++++ bp_mavlink/ardupilotmega.xml | 5 +-- bp_mavlink/common.xml | 66 +++++++++++++++++++++++++++++++----- bp_mavlink/csAirLink.xml | 29 ++++++++++++++++ 4 files changed, 97 insertions(+), 12 deletions(-) create mode 100644 bp_mavlink/csAirLink.xml diff --git a/bp_mavlink/all.xml b/bp_mavlink/all.xml index 0dd38915a7caf..1c5725258b09c 100644 --- a/bp_mavlink/all.xml +++ b/bp_mavlink/all.xml @@ -39,5 +39,14 @@ commands: 50000 - 50099 --> cubepilot.xml + csAirLink.xml + + diff --git a/bp_mavlink/ardupilotmega.xml b/bp_mavlink/ardupilotmega.xml index 46fe2d721bbfb..be538df89e901 100644 --- a/bp_mavlink/ardupilotmega.xml +++ b/bp_mavlink/ardupilotmega.xml @@ -6,6 +6,7 @@ icarous.xml loweheiser.xml cubepilot.xml + csAirLink.xml 2 @@ -23,10 +24,6 @@ - - - - diff --git a/bp_mavlink/common.xml b/bp_mavlink/common.xml index 0d566d5d0e77e..e2236472d4711 100644 --- a/bp_mavlink/common.xml +++ b/bp_mavlink/common.xml @@ -1042,8 +1042,8 @@ Empty - Change speed and/or throttle set points. - Speed type (0=Airspeed, 1=Ground Speed, 2=Climb Speed, 3=Descent Speed) + Change speed and/or throttle set points + Speed type of value set in param2 (such as airspeed, ground speed, and so on) Speed (-1 indicates no change, -2 indicates return to default vehicle speed) Throttle (-1 indicates no change, -2 indicates return to default vehicle throttle value) 0: absolute, 1: relative @@ -1417,7 +1417,7 @@ 0: Stop engine, 1:Start Engine 0: Warm start, 1:Cold start. Controls use of choke where applicable Height delay. This is for commanding engine start only after the vehicle has gained the specified height. Used in VTOL vehicles during takeoff to start engine after the aircraft is off the ground. Zero for no delay. - Empty + A bitmask of options for engine control Empty Empty Empty @@ -1668,8 +1668,21 @@ Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). - Start image capture sequence. Sends CAMERA_IMAGE_CAPTURED after each capture. Use NaN for reserved values. - Reserved (Set to 0) + Start image capture sequence. CAMERA_IMAGE_CAPTURED must be emitted after each capture. + + Param1 (id) may be used to specify the target camera: 0: all cameras, 1 to 6: autopilot-connected cameras, 7-255: MAVLink camera component ID. + It is needed in order to target specific cameras connected to the autopilot, or specific sensors in a multi-sensor camera (neither of which have a distinct MAVLink component ID). + It is also needed to specify the target camera in missions. + + When used in a mission, an autopilot should execute the MAV_CMD for a specified local camera (param1 = 1-6), or resend it as a command if it is intended for a MAVLink camera (param1 = 7 - 255), setting the command's target_component as the param1 value (and setting param1 in the command to zero). + If the param1 is 0 the autopilot should do both. + + When sent in a command the target MAVLink address is set using target_component. + If addressed specifically to an autopilot: param1 should be used in the same way as it is for missions (though command should NACK with MAV_RESULT_DENIED if a specified local camera does not exist). + If addressed to a MAVLink camera, param 1 can be used to address all cameras (0), or to separately address 1 to 7 individual sensors. Other values should be NACKed with MAV_RESULT_DENIED. + If the command is broadcast (target_component is 0) then param 1 should be set to 0 (any other value should be NACKED with MAV_RESULT_DENIED). An autopilot would trigger any local cameras and forward the command to all channels. + + Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras that don't have a distinct component id (such as autopilot-attached cameras). 0: all cameras. This is used to specifically target autopilot-connected cameras or individual sensors in a multi-sensor MAVLink camera. It is also used to target specific cameras when the MAV_CMD is used in a mission Desired elapsed time between two consecutive pictures (in seconds). Minimum values depend on hardware (typically greater than 2 seconds). Total number of images to capture. 0 to capture forever/until MAV_CMD_IMAGE_STOP_CAPTURE. Capture sequence number starting from 1. This is only valid for single-capture (param3 == 1), otherwise set to 0. Increment the capture ID for each capture command to prevent double captures when a command is re-transmitted. @@ -1678,8 +1691,21 @@ - Stop image capture sequence Use NaN for reserved values. - Reserved (Set to 0) + Stop image capture sequence. + + Param1 (id) may be used to specify the target camera: 0: all cameras, 1 to 6: autopilot-connected cameras, 7-255: MAVLink camera component ID. + It is needed in order to target specific cameras connected to the autopilot, or specific sensors in a multi-sensor camera (neither of which have a distinct MAVLink component ID). + It is also needed to specify the target camera in missions. + + When used in a mission, an autopilot should execute the MAV_CMD for a specified local camera (param1 = 1-6), or resend it as a command if it is intended for a MAVLink camera (param1 = 7 - 255), setting the command's target_component as the param1 value (and setting param1 in the command to zero). + If the param1 is 0 the autopilot should do both. + + When sent in a command the target MAVLink address is set using target_component. + If addressed specifically to an autopilot: param1 should be used in the same way as it is for missions (though command should NACK with MAV_RESULT_DENIED if a specified local camera does not exist). + If addressed to a MAVLink camera, param1 can be used to address all cameras (0), or to separately address 1 to 7 individual sensors. Other values should be NACKed with MAV_RESULT_DENIED. + If the command is broadcast (target_component is 0) then param 1 should be set to 0 (any other value should be NACKED with MAV_RESULT_DENIED). An autopilot would trigger any local cameras and forward the command to all channels. + + Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras that don't have a distinct component id (such as autopilot-attached cameras). 0: all cameras. This is used to specifically target autopilot-connected cameras or individual sensors in a multi-sensor MAVLink camera. It is also used to target specific cameras when the MAV_CMD is used in a mission @@ -2929,6 +2955,21 @@ The aircraft should immediately transition into guided. This should not be set for follow me applications + + Speed setpoint types used in MAV_CMD_DO_CHANGE_SPEED + + Airspeed + + + Groundspeed + + + Climb speed + + + Descent speed + + Flags in ESTIMATOR_STATUS message @@ -3339,6 +3380,12 @@ Spektrum DSMX + + Engine control options + + Allow starting the engine once while disarmed + + Bitmap to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 9 is set the floats afx afy afz should be interpreted as force instead of acceleration. @@ -4986,6 +5033,9 @@ True velocity in north direction in earth-fixed NED frame True velocity in east direction in earth-fixed NED frame True velocity in down direction in earth-fixed NED frame + + Latitude (higher precision). If 0, recipients should use the lat field value (otherwise this field is preferred). + Longitude (higher precision). If 0, recipients should use the lon field value (otherwise this field is preferred). Status generated by radio and injected into MAVLink stream. @@ -5930,7 +5980,7 @@ Current status about a high level gimbal manager. This message should be broadcast at a low regular rate (e.g. 5Hz). Timestamp (time since system boot). - High level gimbal manager flags currently applied. + High level gimbal manager flags currently applied. Gimbal device ID that this gimbal manager is responsible for. Component ID of gimbal device (or 1-6 for non-MAVLink gimbal). System ID of MAVLink component with primary control, 0 for none. Component ID of MAVLink component with primary control, 0 for none. diff --git a/bp_mavlink/csAirLink.xml b/bp_mavlink/csAirLink.xml new file mode 100644 index 0000000000000..2f6c7da7d8978 --- /dev/null +++ b/bp_mavlink/csAirLink.xml @@ -0,0 +1,29 @@ + + + + + + + 3 + + + + Login or password error + + + Auth successful + + + + + + Authorization package + Login + Password + + + Response to the authorization request + Response type + + + From 9e1ed39cbc9d4112e1ea1ca9f29135e84d399c94 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 4 Nov 2023 00:18:06 +0100 Subject: [PATCH 007/125] remove some obsolete modifications, functions, text --- libraries/AP_Mount/AP_Mount.h | 2 +- libraries/AP_Mount/AP_Mount_Backend.h | 7 +----- libraries/GCS_MAVLink/GCS.h | 5 ---- libraries/GCS_MAVLink/MAVLink_routing.cpp | 28 ----------------------- libraries/GCS_MAVLink/MAVLink_routing.h | 4 ---- libraries/bp_version.h | 23 +------------------ 6 files changed, 3 insertions(+), 66 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index f4f50d9167253..bc6d7f237a22c 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -278,7 +278,7 @@ class AP_Mount bool set_cam_photo_video(uint8_t instance, int8_t sw_flag); // this is somewhat different to handle_message() in that it catches all messages - // with significant work it potentially could be combined, but let's play it safe and not introduce side effects + // with significant work it potentially could be combined, but let's not introduce side effects void handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg); // send banner diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 23d9592c6fe30..3231a4cf59727 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -135,10 +135,7 @@ class AP_Mount_Backend virtual void handle_param_value(const mavlink_message_t &msg) {} // handle a GLOBAL_POSITION_INT message -//OW -// bool handle_global_position_int(uint8_t msg_sysid, const mavlink_global_position_int_t &packet); - virtual bool handle_global_position_int(uint8_t msg_sysid, const mavlink_global_position_int_t &packet); -//OWEND + bool handle_global_position_int(uint8_t msg_sysid, const mavlink_global_position_int_t &packet); // handle GIMBAL_DEVICE_INFORMATION message virtual void handle_gimbal_device_information(const mavlink_message_t &msg) {} @@ -204,8 +201,6 @@ class AP_Mount_Backend // send banner virtual void send_banner(void) {} - // frontend access - bool is_primary(void) { return (_instance == _frontend._primary); } //OWEND // diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index c46d889b3c0a9..10c6d0d8e5956 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -480,10 +480,6 @@ class GCS_MAVLINK MAV_RESULT set_message_interval(uint32_t msg_id, int32_t interval_us); -//OW - static void send_to_all(uint32_t msgid, const char *pkt) { routing.send_to_all(msgid, pkt); } -//OWEND - protected: //OW RADIOLINK @@ -1263,7 +1259,6 @@ class GCS //OW uint8_t get_landed_state(void) const { return num_gcs() > 0 ? chan(0)->landed_state() : MAV_LANDED_STATE_UNDEFINED; } - uint8_t sysid_my_gcs() { return num_gcs() > 0 ? chan(0)->sysid_my_gcs() : 0; } //OWEND protected: diff --git a/libraries/GCS_MAVLink/MAVLink_routing.cpp b/libraries/GCS_MAVLink/MAVLink_routing.cpp index 593586d36d27e..d9e7db9fa1313 100644 --- a/libraries/GCS_MAVLink/MAVLink_routing.cpp +++ b/libraries/GCS_MAVLink/MAVLink_routing.cpp @@ -436,33 +436,5 @@ void MAVLink_routing::get_targets(const mavlink_message_t &msg, int16_t &sysid, } } -//OW -void MAVLink_routing::send_to_all(uint32_t msgid, const char *pkt) -{ - const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid); - if (entry == nullptr) { - return; - } - - bool sent_to_chan[MAVLINK_COMM_NUM_BUFFERS] {}; - - for (uint8_t i=0; imax_msg_len) + GCS_MAVLINK::packet_overhead_chan(routes[i].channel)) { - continue; - } - _mav_finalize_message_chan_send(routes[i].channel, - entry->msgid, - pkt, - entry->min_msg_len, - entry->max_msg_len, - entry->crc_extra); - sent_to_chan[routes[i].channel] = true; - } -} -//OWEND #endif // HAL_GCS_ENABLED diff --git a/libraries/GCS_MAVLink/MAVLink_routing.h b/libraries/GCS_MAVLink/MAVLink_routing.h index b9ff3f3dc0a0d..b680f405a2031 100644 --- a/libraries/GCS_MAVLink/MAVLink_routing.h +++ b/libraries/GCS_MAVLink/MAVLink_routing.h @@ -49,10 +49,6 @@ class MAVLink_routing */ bool find_by_mavtype_and_compid(uint8_t mavtype, uint8_t compid, uint8_t &sysid, mavlink_channel_t &channel) const; -//OW - void send_to_all(uint32_t msgid, const char *pkt); -//OWEND - private: // a simple linear routing table. We don't expect to have a lot of // routes, so a scalable structure isn't worthwhile yet. diff --git a/libraries/bp_version.h b/libraries/bp_version.h index d2a9eb2ff11c4..d4fff4babd1e1 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -37,28 +37,7 @@ search for //OW to find all changes frsky_passthrough_array support, new param SRx_FRPT added to set stream rate radio_rc_channels, radio_link_flow_control, radio_link_stats support added upgraded to Copter4.3.0-beta1 -2022.09.19: v054 - upgraded to Copter4.2.3 - upgraded to Copter4.2.1 - upgraded to Copter4.2.1-rc1 - upgraded to Copter4.2.0 - upgraded to Plane4.2.1 - Ardu is unfortunately still on storm32.xml v 4. Feb. 2021 -2021.10.08: v052 - upgraded to Copter4.1.0 - upgraded to storm32.xml v 6. Okt. 2021 - more zflags, logging - reverted correction to CMD_DO_MOUNT_CONTROL bug, it's not going to change and it's acknowledged -2021.04.05: - upgraded to Copter4.0.7 -2021.02.05: - upgraded to Copter4.0.6 - upgraded to storm32.xml v 4. Feb. 2021, 600xx ranges - dual mount support if enabled -2020.11.25: - upgraded to Copter4.0.5 - upgraded to storm32.xml v 25. Nov. 2020 - upgraded to storm32_lib.h v 17. Nov. 2020 + ArduCopter specific - GCS_Mavlink.cpp: (2 comments, no change) 1x RADIOLINK From a1a7c7310c025eb07cf7d5fe9fd7f69d6fbbc4a9 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 4 Nov 2023 07:37:25 +0100 Subject: [PATCH 008/125] some tidies --- libraries/AP_Camera/AP_Camera.cpp | 4 ++-- libraries/AP_Camera/AP_Camera.h | 2 +- libraries/AP_Mount/AP_Mount.cpp | 4 ++-- libraries/AP_Mount/AP_Mount.h | 2 +- libraries/AP_Mount/AP_Mount_Backend.cpp | 2 +- libraries/AP_Mount/AP_Mount_Backend.h | 2 +- libraries/RC_Channel/RC_Channel.cpp | 6 +++--- 7 files changed, 11 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 5f7556d9b8962..d6607e4666d9a 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -771,14 +771,14 @@ bool AP_Camera::set_cam_mode(bool video_mode) return false; } -bool AP_Camera::set_cam_photo_video(int8_t sw_flag) +bool AP_Camera::set_cam_photo_video_mode(int8_t sw_flag) { /* XX ?? #if HAL_MOUNT_ENABLED if (get_trigger_type() == CamTrigType::mount) { AP_Mount* mount = AP::mount(); if (mount != nullptr) { - return mount->set_cam_photo_video(0, sw_flag); + return mount->set_cam_photo_video_mode(0, sw_flag); } } #endif diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index a952a4cb666c7..6fbb11fd2a35f 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -157,7 +157,7 @@ class AP_Camera { //OW bool set_cam_mode(bool video_mode); - bool set_cam_photo_video(int8_t sw_flag); + bool set_cam_photo_video_mode(int8_t sw_flag); //OWEND #if AP_CAMERA_SCRIPTING_ENABLED diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index c40101a5f4f54..82b0f2d6b0f75 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -1065,13 +1065,13 @@ bool AP_Mount::set_cam_mode(uint8_t instance, bool video_mode) return backend->set_cam_mode(video_mode); } -bool AP_Mount::set_cam_photo_video(uint8_t instance, int8_t sw_flag) +bool AP_Mount::set_cam_photo_video_mode(uint8_t instance, int8_t sw_flag) { auto *backend = get_instance(instance); if (backend == nullptr) { return false; } - return backend->set_cam_photo_video(sw_flag); + return backend->set_cam_photo_video_mode(sw_flag); } void AP_Mount::handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index bc6d7f237a22c..970e5ada505ca 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -275,7 +275,7 @@ class AP_Mount bool set_cam_mode(uint8_t instance, bool video_mode); // 3-way switch mode - bool set_cam_photo_video(uint8_t instance, int8_t sw_flag); + bool set_cam_photo_video_mode(uint8_t instance, int8_t sw_flag); // this is somewhat different to handle_message() in that it catches all messages // with significant work it potentially could be combined, but let's not introduce side effects diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index c50ab5a5d5e7b..98908434e5424 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -93,7 +93,7 @@ void AP_Mount_Backend::clear_roi_target() _roi_target_set = false; // reset the mode if in GPS tracking mode - if (_mode == MAV_MOUNT_MODE_GPS_POINT) { + if (get_mode() == MAV_MOUNT_MODE_GPS_POINT) { MAV_MOUNT_MODE default_mode = (MAV_MOUNT_MODE)_params.default_mode.get(); set_mode(default_mode); } diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 3231a4cf59727..24b348719d50d 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -193,7 +193,7 @@ class AP_Mount_Backend virtual bool set_cam_mode(bool video_mode) { return false; } // 3-way switch mode - virtual bool set_cam_photo_video(int8_t sw_flag) { return false; } + virtual bool set_cam_photo_video_mode(int8_t sw_flag) { return false; } // handle msg - allows to process a msg from a gimbal virtual void handle_msg(const mavlink_message_t &msg) {} diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index dc015c2836634..e0a82b7f12a7f 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -1493,9 +1493,9 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos } //OW switch (ch_flag) { - case AuxSwitchPos::HIGH: camera->set_cam_photo_video(1); break; - case AuxSwitchPos::MIDDLE: camera->set_cam_photo_video(0); break; - case AuxSwitchPos::LOW: camera->set_cam_photo_video(-1); break; + case AuxSwitchPos::HIGH: camera->set_cam_photo_video_mode(1); break; + case AuxSwitchPos::MIDDLE: camera->set_cam_photo_video_mode(0); break; + case AuxSwitchPos::LOW: camera->set_cam_photo_video_mode(-1); break; } camera->set_cam_mode(ch_flag == AuxSwitchPos::HIGH); //OWEND From 80aa8039ca5b5f0fce40fb444b1050106a35c5bb Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 4 Nov 2023 09:39:52 +0100 Subject: [PATCH 009/125] move landed_state to vehicle class --- ArduCopter/Copter.cpp | 16 ++++++++++++++++ ArduCopter/Copter.h | 3 +++ ArduCopter/GCS_Mavlink.cpp | 4 ++++ ArduCopter/GCS_Mavlink.h | 4 +++- ArduPlane/ArduPlane.cpp | 12 ++++++++++++ ArduPlane/GCS_Mavlink.cpp | 5 ++++- ArduPlane/GCS_Mavlink.h | 5 +++-- ArduPlane/Plane.h | 3 +++ Blimp/Blimp.cpp | 16 ++++++++++++++++ Blimp/Blimp.h | 3 +++ Blimp/GCS_Mavlink.cpp | 5 ++++- Blimp/GCS_Mavlink.h | 5 +++-- libraries/AP_Vehicle/AP_Vehicle.h | 5 +++++ libraries/GCS_MAVLink/GCS.h | 13 +++++++------ 14 files changed, 86 insertions(+), 13 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 8bbbc186c19e1..651473abb9072 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -459,6 +459,22 @@ bool Copter::is_taking_off() const return flightmode->is_taking_off(); } +//OW +MAV_LANDED_STATE Copter::get_landed_state() const +{ + if (ap.land_complete) { + return MAV_LANDED_STATE_ON_GROUND; + } + if (flightmode->is_landing()) { + return MAV_LANDED_STATE_LANDING; + } + if (flightmode->is_taking_off()) { + return MAV_LANDED_STATE_TAKEOFF; + } + return MAV_LANDED_STATE_IN_AIR; +} +//OWEND + bool Copter::current_mode_requires_mission() const { #if MODE_AUTO_ENABLED == ENABLED diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 2d620deb8305c..2c1d60ed78742 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -700,6 +700,9 @@ class Copter : public AP_Vehicle { #endif // AP_SCRIPTING_ENABLED bool is_landing() const override; bool is_taking_off() const override; +//OW + MAV_LANDED_STATE get_landed_state() const override; +//OWEND void rc_loop(); void throttle_loop(); void update_batt_compass(void); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index d86efc6c0591c..dd3c97109e344 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1544,6 +1544,8 @@ uint64_t GCS_MAVLINK_Copter::capabilities() const GCS_MAVLINK::capabilities()); } +//OW +/* MAV_LANDED_STATE GCS_MAVLINK_Copter::landed_state() const { if (copter.ap.land_complete) { @@ -1557,6 +1559,8 @@ MAV_LANDED_STATE GCS_MAVLINK_Copter::landed_state() const } return MAV_LANDED_STATE_IN_AIR; } +*/ +//OWEND void GCS_MAVLINK_Copter::send_wind() const { diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index 98de6b502f386..35a8222a0db72 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -53,7 +53,9 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK uint64_t capabilities() const override; virtual MAV_VTOL_STATE vtol_state() const override { return MAV_VTOL_STATE_MC; }; - virtual MAV_LANDED_STATE landed_state() const override; +//OW +// virtual MAV_LANDED_STATE landed_state() const override; +//OWEND void handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow) override; diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 61d7642533327..4d41877747089 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -894,6 +894,18 @@ bool Plane::is_taking_off() const return control_mode->is_taking_off(); } +//OW +MAV_LANDED_STATE Plane::get_landed_state() const +{ + if (plane.is_flying()) { + // note that Q-modes almost always consider themselves as flying + return MAV_LANDED_STATE_IN_AIR; + } + + return MAV_LANDED_STATE_ON_GROUND; +} +//OWEND + // correct AHRS pitch for TRIM_PITCH_CD in non-VTOL modes, and return VTOL view in VTOL void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const { diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 71df773b681fd..f46a7cba5333c 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1508,6 +1508,8 @@ MAV_VTOL_STATE GCS_MAVLINK_Plane::vtol_state() const #endif }; +//OW +/* MAV_LANDED_STATE GCS_MAVLINK_Plane::landed_state() const { if (plane.is_flying()) { @@ -1517,4 +1519,5 @@ MAV_LANDED_STATE GCS_MAVLINK_Plane::landed_state() const return MAV_LANDED_STATE_ON_GROUND; } - +*/ +//OWEND diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index b30ac87287669..aef55f2eb2c9e 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -94,6 +94,7 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK #endif MAV_VTOL_STATE vtol_state() const override; - MAV_LANDED_STATE landed_state() const override; - +//OW +// MAV_LANDED_STATE landed_state() const override; +//OWEND }; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b8d6e21db733c..220b33bb31cb4 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1244,6 +1244,9 @@ class Plane : public AP_Vehicle { void failsafe_check(void); bool is_landing() const override; bool is_taking_off() const override; +//OW + MAV_LANDED_STATE get_landed_state() const override; +//OWEND #if AP_SCRIPTING_ENABLED bool set_target_location(const Location& target_loc) override; bool get_target_location(Location& target_loc) override; diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index ff5327006738d..cabacf8764746 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -272,6 +272,22 @@ void Blimp::rotate_NE_to_BF(Vector2f &vec) } +//OW +MAV_LANDED_STATE Blimp::get_landed_state() const +{ + if (ap.land_complete) { + return MAV_LANDED_STATE_ON_GROUND; + } + if (flightmode->is_landing()) { + return MAV_LANDED_STATE_LANDING; + } + // if (flightmode->is_taking_off()) { + // return MAV_LANDED_STATE_TAKEOFF; + // } + return MAV_LANDED_STATE_IN_AIR; +} +//OWEND + /* constructor for main Blimp class */ diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index 32ac39b5bddf4..c1536c0c2c46c 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -309,6 +309,9 @@ class Blimp : public AP_Vehicle void update_altitude(); void rotate_NE_to_BF(Vector2f &vec); void rotate_BF_to_NE(Vector2f &vec); +//OW + MAV_LANDED_STATE get_landed_state() const override; +//OWEND // commands.cpp void update_home_from_EKF(); diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index c96624649184a..082c2157b1194 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -549,7 +549,8 @@ uint64_t GCS_MAVLINK_Blimp::capabilities() const MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET | GCS_MAVLINK::capabilities()); } - +//OW +/* MAV_LANDED_STATE GCS_MAVLINK_Blimp::landed_state() const { if (blimp.ap.land_complete) { @@ -563,6 +564,8 @@ MAV_LANDED_STATE GCS_MAVLINK_Blimp::landed_state() const // } return MAV_LANDED_STATE_IN_AIR; } +*/ +//OWEND void GCS_MAVLINK_Blimp::send_wind() const { diff --git a/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h index c248ae0886e2b..14899074e01fb 100644 --- a/Blimp/GCS_Mavlink.h +++ b/Blimp/GCS_Mavlink.h @@ -40,8 +40,9 @@ class GCS_MAVLINK_Blimp : public GCS_MAVLINK { return MAV_VTOL_STATE_MC; }; - virtual MAV_LANDED_STATE landed_state() const override; - +//OW +// virtual MAV_LANDED_STATE landed_state() const override; +//OWEND private: void handleMessage(const mavlink_message_t &msg) override; diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 18b43fc0209ca..9fa057d34606d 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -225,6 +225,11 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { // returns true if vehicle is in the process of taking off virtual bool is_taking_off() const { return false; } +//OW + // returns landed state + virtual MAV_LANDED_STATE get_landed_state() const { return MAV_LANDED_STATE_UNDEFINED; } +//OWEND + // zeroing the RC outputs can prevent unwanted motor movement: virtual bool should_zero_rc_outputs_on_reboot() const { return false; } diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 10c6d0d8e5956..07cf52eb88e9c 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -24,7 +24,9 @@ #include #include #include - +//OW +#include +//OWEND #include "ap_message.h" #define GCS_DEBUG_SEND_MESSAGE_TIMINGS 0 @@ -504,7 +506,10 @@ class GCS_MAVLINK virtual MAV_STATE vehicle_system_status() const = 0; virtual MAV_VTOL_STATE vtol_state() const { return MAV_VTOL_STATE_UNDEFINED; } - virtual MAV_LANDED_STATE landed_state() const { return MAV_LANDED_STATE_UNDEFINED; } +//OW +// virtual MAV_LANDED_STATE landed_state() const { return MAV_LANDED_STATE_UNDEFINED; } + MAV_LANDED_STATE landed_state() const { return AP::vehicle()->get_landed_state(); } +//OWEND // return a MAVLink parameter type given a AP_Param type static MAV_PARAM_TYPE mav_param_type(enum ap_var_type t); @@ -1257,10 +1262,6 @@ class GCS virtual uint8_t sysid_this_mav() const = 0; -//OW - uint8_t get_landed_state(void) const { return num_gcs() > 0 ? chan(0)->landed_state() : MAV_LANDED_STATE_UNDEFINED; } -//OWEND - protected: virtual GCS_MAVLINK *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters ¶ms, From dc0b4777c4a575ca7d364ee276e45f616cfa02f0 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 4 Nov 2023 09:41:37 +0100 Subject: [PATCH 010/125] attempt, looks good without test --- ArduCopter/version.h | 2 +- libraries/AP_Mount/AP_Mount.cpp | 27 + libraries/AP_Mount/AP_Mount_Backend.cpp | 33 +- libraries/AP_Mount/AP_Mount_Backend.h | 14 +- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 1198 ++++++++--------- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 323 +++-- libraries/bp_version.h | 19 +- 7 files changed, 798 insertions(+), 818 deletions(-) diff --git a/ArduCopter/version.h b/ArduCopter/version.h index c671c4555dc04..2811befe67b88 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -11,7 +11,7 @@ //OW #undef THISFIRMWARE #include "../libraries/bp_version.h" -#define THISFIRMWARE "BetaCopter V4.5.0-dev" BETAPILOTVERSION " 20230810" +#define THISFIRMWARE "BetaCopter V4.5.0-dev" BETAPILOTVERSION " 20231101" //OWEND // the following line is parsed by the autotest scripts diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 82b0f2d6b0f75..f15c6ff7e3c18 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -339,6 +339,8 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com return MAV_RESULT_FAILED; } +//OW +/* // check flags for change to RETRACT const uint32_t flags = packet.x; if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) { @@ -350,6 +352,13 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com backend->set_mode(MAV_MOUNT_MODE_NEUTRAL); return MAV_RESULT_ACCEPTED; } +*/ + const uint32_t flags = packet.x; + + if (!backend->handle_gimbal_manager_flags(flags)) { + return MAV_RESULT_ACCEPTED; + } +//OWEND // param1 : pitch_angle (in degrees) // param2 : yaw angle (in degrees) @@ -411,6 +420,8 @@ void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg){ return; } +//OW +/* // check flags for change to RETRACT const uint32_t flags = packet.flags; if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) { @@ -423,6 +434,13 @@ void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg){ backend->set_mode(MAV_MOUNT_MODE_NEUTRAL); return; } +*/ + const uint32_t flags = packet.flags; + + if (!backend->handle_gimbal_manager_flags(flags)) { + return; + } +//OWEND const Quaternion att_quat{packet.q}; const Vector3f att_rate_degs { @@ -477,6 +495,8 @@ void AP_Mount::handle_command_gimbal_manager_set_pitchyaw(const mavlink_message_ return; } +//OW +/* // check flags for change to RETRACT uint32_t flags = (uint32_t)packet.flags; if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) { @@ -488,6 +508,13 @@ void AP_Mount::handle_command_gimbal_manager_set_pitchyaw(const mavlink_message_ backend->set_mode(MAV_MOUNT_MODE_NEUTRAL); return; } +*/ + const uint32_t flags = packet.flags; + + if (!backend->handle_gimbal_manager_flags(flags)) { + return; + } +//OWEND // Do not allow both angle and rate to be specified at the same time if (!isnan(packet.pitch) && !isnan(packet.yaw) && !isnan(packet.pitch_rate) && !isnan(packet.yaw_rate)) { diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 98908434e5424..87621396f9889 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -197,14 +197,45 @@ void AP_Mount_Backend::send_gimbal_manager_information(mavlink_channel_t chan) radians(_params.yaw_angle_max)); // yaw_max in radians } +//OW +// return gimbal manager flags used by GIMBAL_MANAGER_STATUS message +uint32_t AP_Mount_Backend::get_gimbal_manager_flags() const +{ + uint32_t flags = GIMBAL_MANAGER_FLAGS_ROLL_LOCK | GIMBAL_MANAGER_FLAGS_PITCH_LOCK; + if (_yaw_lock) { + flags |= GIMBAL_MANAGER_FLAGS_YAW_LOCK; + } + return flags; +} + +// set gimbal manager flags, called from frontend's gimbal manager handlers +bool AP_Mount_Backend::handle_gimbal_manager_flags(uint32_t flags) +{ + // check flags for change to RETRACT + if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) { + set_mode(MAV_MOUNT_MODE_RETRACT); + } else + // check flags for change to NEUTRAL + if ((flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) { + set_mode(MAV_MOUNT_MODE_NEUTRAL); + } + return true; +} +//OWEND + + // send a GIMBAL_MANAGER_STATUS message to GCS void AP_Mount_Backend::send_gimbal_manager_status(mavlink_channel_t chan) { +//OW +/* uint32_t flags = GIMBAL_MANAGER_FLAGS_ROLL_LOCK | GIMBAL_MANAGER_FLAGS_PITCH_LOCK; if (_yaw_lock) { flags |= GIMBAL_MANAGER_FLAGS_YAW_LOCK; - } + } */ + uint32_t flags = get_gimbal_manager_flags(); +//OWEND mavlink_msg_gimbal_manager_status_send(chan, AP_HAL::millis(), // autopilot system time diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 24b348719d50d..408c24f0d48bb 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -122,8 +122,11 @@ class AP_Mount_Backend // return gimbal capabilities sent to GCS in the GIMBAL_MANAGER_INFORMATION virtual uint32_t get_gimbal_manager_capability_flags() const; +//OW // send a GIMBAL_MANAGER_INFORMATION message to GCS - void send_gimbal_manager_information(mavlink_channel_t chan); +// void send_gimbal_manager_information(mavlink_channel_t chan); + virtual void send_gimbal_manager_information(mavlink_channel_t chan); +//OWEND // send a GIMBAL_MANAGER_STATUS message to GCS void send_gimbal_manager_status(mavlink_channel_t chan); @@ -201,6 +204,15 @@ class AP_Mount_Backend // send banner virtual void send_banner(void) {} + // return gimbal manager flags used by GIMBAL_MANAGER_STATUS message + virtual uint32_t get_gimbal_manager_flags() const; + + // handle gimbal manager flags received from gimbal manager messages + // GIMBAL_MANAGER_FLAGS_RETRACT, GIMBAL_MANAGER_FLAGS_NEUTRAL are handled in frontend + // and sets mode accordingly. Not so nice but it's not my cup of tea. + // The function may modify the flags according to its capabilities. + // Return false to abort angle/rate processing. + virtual bool handle_gimbal_manager_flags(uint32_t flags); //OWEND // diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 7606693d6cbc1..49c47d91a0dcd 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -19,51 +19,16 @@ extern const AP_HAL::HAL& hal; -//***************************************************** -/* -This is a simplified, heavily reworked version of what's in BetaPilot 4.2.3. -Adopts the gimbal changes in 4.3 as much as possible, and follows the Gremsy & SToRM32 drivers. -Limitations of the Gremsy driver are: -- captures & resends messages instead of relying on routing -- yaw lock and vehicle/earth frame are strictly tight together -- capabilities (i.e. limited capabilities) are not respected - -two modes/protocols of operation are supported -1. ArduPilot like, largely mimics Gremsy gimbal driver = PROTOCOL_ARDUPILOT_LIKE - we could mimic a super primitive v2 gimbal manager, primary is always autopilot, secondary always our GCS - - only sends GIMBAL_MANAGER_STATUS - - don' handle MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE - - handling of request is needed - - GIMBAL_MANAGER_SET_PITCHYAW, MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW ? ArduPilot's handling needs to be corrected, also doesn't respect pan -2. STorM32 is gimbal manger, using STorM32 gimbal manager messages = PROTOCOL_STORM32_GIMBAL_MANAGER -*/ -//***************************************************** - -/* - ZFLAGS - 0: protocol is auto determined - 1: protocol forced to PROTOCOL_ARDUPILOT_LIKE - 2: protocol forced to PROTOCOL_STORM32_GIMBAL_MANAGER - 8: only streaming - only sends out RC_CHANNLES, AUTOPILOT_STATE_FOR_GIMBAL for STorM32-Link - this mode could in principle be replaced by asking for the streams, but since AP isn't streaming reliably we don't - 16: do not send AUTOPILOT_STATE_FOR_GIMBAL_EXT - 64: do not use 3way photo-video switch mode for 'Camera Mode Toggle' aux function - 128: do not log - - in all modes sends MOUNT_STATUS to ground, so that "old" things like MP etc can see the gimbal orientation - listens to STORM32_GIMBAL_DEVICE_STATUS to send out MOUNT_STATUS in sync -*/ //****************************************************** // Quaternion & Euler for Gimbal //****************************************************** -// we do not use NED (roll-pitch-yaw) to convert received quaternion to Euler angles and vice versa -// we use pitch-roll-yaw instead -// when the roll angle is zero, both are equivalent, this should be the majority of cases anyhow -// also, for most gimbals pitch-roll-yaw is appropriate -// the issue with NED is the gimbal lock at pitch +-90�, but pitch +-90� is a common operation point for gimbals -// the angles we store in this lib are thus pitch-roll-yaw Euler +// It is inappropriate to use NED (roll-pitch-yaw) to convert received quaternion to Euler angles and vice versa. +// For most gimbals pitch-roll-yaw is appropriate. +// When the roll angle is zero, both are equivalent, which should be the majority of cases anyhow. +// The issue with NED is the gimbal lock at pitch +-90 deg, but pitch +-90 deg is a common operation point for +// gimbals. +// Tthe angles in this driver are thus pitch-roll-yaw Euler. class GimbalQuaternion : public Quaternion { @@ -104,7 +69,7 @@ void GimbalQuaternion::to_gimbal_euler(float &roll, float &pitch, float &yaw) co //****************************************************** -// BP_Mount_STorM32_MAVLink, that's the main class +// BP_Mount_STorM32_MAVLink, main class //****************************************************** // for reasons I really don't understand, calling them as log methods didn't work @@ -161,6 +126,7 @@ void GimbalQuaternion::to_gimbal_euler(float &roll, float &pitch, float &yaw) co //****************************************************** // STorM32 states //****************************************************** + enum STORM32STATEENUM { STORM32STATE_STARTUP_MOTORS = 0, STORM32STATE_STARTUP_SETTLE, @@ -174,59 +140,37 @@ enum STORM32STATEENUM { //****************************************************** -// BP_Mount_STorM32_MAVLink, that's the main class +// BP_Mount_STorM32_MAVLink, main class //****************************************************** BP_Mount_STorM32_MAVLink::BP_Mount_STorM32_MAVLink(AP_Mount &frontend, AP_Mount_Params ¶ms, uint8_t instance) : AP_Mount_Backend(frontend, params, instance) { + _sysid = 0; + _compid = 0; // gimbal not yet discovered + _chan = MAVLINK_COMM_0; // this is a dummy, will be set correctly by find_gimbal() + _got_device_info = false; - _initialised = false; _armed = false; _prearmchecks_ok = false; + _initialised = false; - _sysid = 0; - _compid = 0; // gimbal not yet discovered - _chan = MAVLINK_COMM_0; // this is a dummy, will be set correctly by find_gimbal() + _mode = MAV_MOUNT_MODE_RC_TARGETING; - _armingchecks_enabled = false; - _prearmchecks_all_ok = false; - _prearmcheck.updated = false; - _prearmcheck.enabled_flags = 0; - _prearmcheck.fail_flags = 0; - _prearmcheck.fail_flags_last = UINT32_MAX; + _protocol = PROTOCOL_UNDEFINED; - _device.received_flags = 0; - _device.received_failure_flags = 0; + _armingchecks_enabled = false; + _prearmchecks_done = false; - _yaw_lock = false; // STorM32 doesn't currently support earth frame, so we need to ensure this is false - _is_yaw_lock = false; + _device_status = {}; + flags_for_gimbal = 0; + _current_angles = { 0.0f, 0.0f, 0.0f, NAN}; // the NAN is important! + _script_control_angles = {}; - _mode = MAV_MOUNT_MODE_RC_TARGETING; + _yaw_lock = false; // can't be currently supported, so we need to ensure this is false. This is important! - _sendonly = false; - _should_log = true; _got_radio_rc_channels = false; // disable sending rc channels when RADIO_RC_CHANNELS messages are detected - _send_autopilotstateext = true; _use_3way_photo_video = true; - - _protocol = PROTOCOL_UNDEFINED; - _protocol_auto_cntdown = PROTOCOL_AUTO_TIMEOUT_CNT; - _qshot.mode = MAV_QSHOT_MODE_UNDEFINED; - _qshot.mode_last = MAV_QSHOT_MODE_UNDEFINED; - - if (_params.zflags & 0x01) { // 1 is set - _protocol = PROTOCOL_ARDUPILOT_LIKE; - } else - if (_params.zflags & 0x02) { // 2 is set - _protocol = PROTOCOL_STORM32_GIMBAL_MANAGER; - } - if (_params.zflags & 0x08) _sendonly = true; - // we currently always do it if (_params.zflags & 0x10) _send_autopilotstateext = false; - if (_params.zflags & 0x40) _use_3way_photo_video = !_use_3way_photo_video; - if (_params.zflags & 0x80) _should_log = false; - - _camera_compid = 0; // camera not yet discovered _camera_mode = CAMERA_MODE_UNDEFINED; } @@ -234,10 +178,7 @@ BP_Mount_STorM32_MAVLink::BP_Mount_STorM32_MAVLink(AP_Mount &frontend, AP_Mount_ // called by all vehicles with 50 Hz, using the scheduler // several vehicles do not support fast_update(), so let's go with this // priority of update() not very high, so no idea how reliable that is, may be not so good -// we would have wanted updates with 20 Hz, especially for STorM32-Link -// however, since it's 50 Hz, we update at 25 Hz and 12.5 Hz respectively -// not soo nice, but best we can do -// not clear what it means for STorM32Link, probably not too bad, maybe even good +// STorM32-Link wants 25 Hz, so we update at 25 Hz and 12.5 Hz respectively void BP_Mount_STorM32_MAVLink::update() { switch (_task_counter) { @@ -245,20 +186,20 @@ void BP_Mount_STorM32_MAVLink::update() case TASK_SLOT2: if (_compid) { // we send it as soon as we have found the gimbal send_autopilot_state_for_gimbal_device(); - if (_send_autopilotstateext) send_autopilot_state_for_gimbal_device_ext(); } break; case TASK_SLOT1: - if (_sendonly) break; // don't do any control messages if (_initialised) { // we do it when the startup sequence has been fully completed - set_and_send_target_angles(); // GRRRRRR this is used to determine hasmanager!! + send_target_angles(); } break; case TASK_SLOT3: if (_compid) { // we send it as soon as we have found the gimbal - if (!_got_radio_rc_channels) send_rc_channels(); + if (!_got_radio_rc_channels) { // don't send it if we have seen RADIO_RC_CHANNELS messages + send_rc_channels(); + } } break; } @@ -266,6 +207,8 @@ void BP_Mount_STorM32_MAVLink::update() _task_counter++; if (_task_counter > TASK_SLOT3) _task_counter = TASK_SLOT0; + update_target_angles(); // update at 50 Hz + if (!_initialised) { find_gimbal(); return; @@ -280,230 +223,197 @@ void BP_Mount_STorM32_MAVLink::update() } -void BP_Mount_STorM32_MAVLink::set_and_send_target_angles(void) +//------------------------------------------------------ +// Mode handling and targeting functions +//------------------------------------------------------ + +// flags coming from gimbal manager messages and commands +// Called from AP_Mount's gimbal manager handlers, but only if source is in control. +// The front-end calls set_angle_target() and/or set_rate_target() depending +// on the info in the gimbal manager messages. +// Return false to not do this angle/rate processing. +bool BP_Mount_STorM32_MAVLink::handle_gimbal_manager_flags(uint32_t flags) { - GimbalTarget gtarget; + // check flags for change to RETRACT + if (flags & GIMBAL_MANAGER_FLAGS_RETRACT) { + set_mode(MAV_MOUNT_MODE_RETRACT); + } else + // check flags for change to NEUTRAL + if (flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) { + set_mode(MAV_MOUNT_MODE_NEUTRAL); + } - if (_qshot.mode == MAV_QSHOT_MODE_UNDEFINED) { - enum MAV_MOUNT_MODE mntmode = get_mode(); - if (!get_target_angles_mount(gtarget, mntmode)) return; // don't send - update_gimbal_device_flags_mount(mntmode); - } else { - if (!get_target_angles_qshot(gtarget)) return; // don't send - update_gimbal_device_flags_qshot(); + update_gimbal_device_flags(get_mode()); + + // we currently do not support LOCK + // front-end is digesting GIMBAL_MANAGER_FLAGS_YAW_LOCK to determine yaw_is_earth_frame + // we could make it to modify the flag, but for moment let's be happy. + if (flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK) { + return false; // don't accept angle/rate setting } - _qshot.mode_last = _qshot.mode; - if (_qshot.mode == UINT8_MAX) return; // is in hold, don't send + // STorM32 expects one of them to be set, otherwise it rejects + if (!(flags & GIMBAL_MANAGER_FLAGS_YAW_IN_VEHICLE_FRAME) && !(flags & GIMBAL_MANAGER_FLAGS_YAW_IN_EARTH_FRAME)) { + return false; // don't accept angle/rate setting + } - if (_protocol == PROTOCOL_ARDUPILOT_LIKE) { - send_gimbal_device_set_attitude(gtarget); - } else - if (_protocol == PROTOCOL_STORM32_GIMBAL_MANAGER) { - // only send when autopilot client is active, this reduces traffic - if (_manager.ap_client_is_active) { - update_gimbal_manager_flags(); - send_storm32_gimbal_manager_control(gtarget); - } + // STorM32 currently only supports GIMBAL_MANAGER_FLAGS_YAW_IN_VEHICLE_FRAME + if (flags & GIMBAL_MANAGER_FLAGS_YAW_IN_EARTH_FRAME) { + return false; // don't accept angle/rate setting } -} + return true; // accept angle/rate setting +} -//------------------------------------------------------ -// V2 GIMBAL DEVICE, ArduPilot like -//------------------------------------------------------ -bool BP_Mount_STorM32_MAVLink::get_target_angles_mount(GimbalTarget >arget, enum MAV_MOUNT_MODE mntmode) +void BP_Mount_STorM32_MAVLink::send_target_angles(void) { - MountTarget mtarget_rad = {}; + // just send stupidly at 12.5 Hz if (!get_target_angles()) return; // if false don't send - // update based on mount mode - switch (mntmode) { + update_gimbal_device_flags(get_mode()); - // move mount to a "retracted" position. We disable motors - case MAV_MOUNT_MODE_RETRACT: - gtarget.set(TARGET_MODE_RETRACT); - return true; + if (_protocol == PROTOCOL_GIMBAL_DEVICE) { + send_gimbal_device_set_attitude(); + } else { + send_cmd_do_mount_control(); + } +} + + +enum MountModeThisWouldBeGreatToHave { + MAV_MOUNT_MODE_SCRIPT = 7, +}; + + +// update_angle_target_from_rate() assumes a 50hz update rate! +void BP_Mount_STorM32_MAVLink::update_target_angles(void) +{ + // update based on mount mode + switch (get_mode()) { + + // move mount to a "retracted" position + // -> ANGLE + case MAV_MOUNT_MODE_RETRACT: { + const Vector3f &target = _params.retract_angles.get(); + mnt_target.target_type = MountTargetType::ANGLE; + mnt_target.angle_rad.set(target*DEG_TO_RAD, false); + break; + } // move mount to a neutral position, typically pointing forward + // -> ANGLE case MAV_MOUNT_MODE_NEUTRAL: { - const Vector3f &vec_deg = _params.neutral_angles.get(); - gtarget.set_from_vec_deg(vec_deg, TARGET_MODE_NEUTRAL); - return true; + const Vector3f &target = _params.neutral_angles.get(); + mnt_target.target_type = MountTargetType::ANGLE; + mnt_target.angle_rad.set(target*DEG_TO_RAD, false); + break; } - // use angle or rate targets provided by a mavlink message or mission command + // point to the angles given by a mavlink message or mission command + // -> ANGLE case MAV_MOUNT_MODE_MAVLINK_TARGETING: - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - gtarget.set_angle(mnt_target.angle_rad); - return true; - case MountTargetType::RATE: - // we do not yet support rate, so do it SToRM32 driver like - MountTarget rate_mtarget_rad {}; - update_angle_target_from_rate(mnt_target.rate_rads, rate_mtarget_rad); - gtarget.set_angle(rate_mtarget_rad); - return true; + // mnt_target should have already been populated by set_angle_target() or set_rate_target(). + // SToRM32 doesn't support rate, so update target angle from rate if necessary. + if (mnt_target.target_type == MountTargetType::RATE) { + update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); } break; // RC radio manual angle control, but with stabilization from the AHRS // update targets using pilot's RC inputs - case MAV_MOUNT_MODE_RC_TARGETING: -/* XX ?? - if (get_rc_rate_target(mtarget_rad)) { - // we do not yet support rate, so do it SToRM32 driver like - MountTarget rate_mtarget_rad {}; - update_angle_target_from_rate(mtarget_rad, rate_mtarget_rad); - gtarget.set_angle(rate_mtarget_rad); - return true; - } else - if (get_rc_angle_target(mtarget_rad)) { - gtarget.set_angle(mtarget_rad); - return true; + // -> can be RATE, will be ignored + case MAV_MOUNT_MODE_RC_TARGETING: { + MountTarget rc_target; + get_rc_target(mnt_target.target_type, rc_target); + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + mnt_target.angle_rad = rc_target; + break; + case MountTargetType::RATE: + mnt_target.rate_rads = rc_target; + break; } -*/ break; + } // point mount to a GPS point given by the mission planner + // ATTENTION: angle_rad.yaw_is_ef = true + // -> ANGLE case MAV_MOUNT_MODE_GPS_POINT: - if (get_angle_target_to_roi(mtarget_rad)) { - gtarget.set_angle(mtarget_rad); - return true; + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - // point mount to home + // point mount to Home location + // ATTENTION: angle_rad.yaw_is_ef = true + // -> ANGLE case MAV_MOUNT_MODE_HOME_LOCATION: - if (get_angle_target_to_home(mtarget_rad)) { - gtarget.set_angle(mtarget_rad); - return true; + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; + // point mount to another vehicle + // ATTENTION: angle_rad.yaw_is_ef = true + // -> ANGLE case MAV_MOUNT_MODE_SYSID_TARGET: - if (get_angle_target_to_sysid(mtarget_rad)) { - gtarget.set_angle(mtarget_rad); - return true; + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; + // point mount to where a script wants it to point + // -> ANGLE + case MAV_MOUNT_MODE_SCRIPT: + mnt_target.target_type = MountTargetType::ANGLE; + mnt_target.angle_rad.roll = _script_control_angles.roll; + mnt_target.angle_rad.pitch = _script_control_angles.pitch; + mnt_target.angle_rad.yaw = _script_control_angles.yaw_bf; + mnt_target.angle_rad.yaw_is_ef = false; + break; + default: - // unknown mode so do nothing + // we do not know this mode so do nothing break; } - - return false; } -void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags_mount(enum MAV_MOUNT_MODE mntmode) +void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags(enum MAV_MOUNT_MODE mntmode) { - _device.flags_for_gimbal = 0; + flags_for_gimbal = 0; switch (mntmode) { case MAV_MOUNT_MODE_RETRACT: - _device.flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RETRACT; + flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RETRACT; break; case MAV_MOUNT_MODE_NEUTRAL: - _device.flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_NEUTRAL; + flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_NEUTRAL; break; default: break; } - _device.flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK; - if (_is_yaw_lock) _device.flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_LOCK; - - // set either YAW_IN_VEHICLE_FRAME or YAW_IN_EARTH_FRAME, to indicate new message format, STorM32 will reject otherwise - _device.flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME; -} - - -//------------------------------------------------------ -// STORM32 GIMBAL MANAGER, QShot -//------------------------------------------------------ - -bool BP_Mount_STorM32_MAVLink::get_target_angles_qshot(GimbalTarget >arget) -{ - switch (_qshot.mode) { - case MAV_QSHOT_MODE_UNDEFINED: - return false; - - case MAV_QSHOT_MODE_GIMBAL_RETRACT: - return get_target_angles_mount(gtarget, MAV_MOUNT_MODE_RETRACT); - - case MAV_QSHOT_MODE_GIMBAL_NEUTRAL: - return get_target_angles_mount(gtarget, MAV_MOUNT_MODE_NEUTRAL); - - case MAV_QSHOT_MODE_GIMBAL_RC_CONTROL: - return get_target_angles_mount(gtarget, MAV_MOUNT_MODE_RC_TARGETING); - - case MAV_QSHOT_MODE_POI_TARGETING: - return get_target_angles_mount(gtarget, MAV_MOUNT_MODE_GPS_POINT); - - case MAV_QSHOT_MODE_HOME_TARGETING: - return get_target_angles_mount(gtarget, MAV_MOUNT_MODE_HOME_LOCATION); - - case MAV_QSHOT_MODE_SYSID_TARGETING: - return get_target_angles_mount(gtarget, MAV_MOUNT_MODE_SYSID_TARGET); - - default: - // in all other modes we don't do nothing, i.e. just send out something - // it is the job of the supervisor to get things right by setting the activity - return get_target_angles_mount(gtarget, MAV_MOUNT_MODE_MAVLINK_TARGETING); - } - - return false; -} - - -void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags_qshot(void) -{ - switch (_qshot.mode) { - case MAV_QSHOT_MODE_GIMBAL_RETRACT: - update_gimbal_device_flags_mount(MAV_MOUNT_MODE_RETRACT); - break; + // we currently only support pitch,roll lock, not pitch,roll follow + flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK; - case MAV_QSHOT_MODE_GIMBAL_NEUTRAL: - update_gimbal_device_flags_mount(MAV_MOUNT_MODE_NEUTRAL); - break; + // we currently do not support yaw lock +// if (_is_yaw_lock) flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_LOCK; - default: - // currently, anything not retract/neutral is good - update_gimbal_device_flags_mount(MAV_MOUNT_MODE_ENUM_END); - break; - } + // set either YAW_IN_VEHICLE_FRAME or YAW_IN_EARTH_FRAME, to indicate new message format, STorM32 will reject otherwise + flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME; } -void BP_Mount_STorM32_MAVLink::update_gimbal_manager_flags(void) -{ - // we play it simple and do not attempt to claim supervision nor activity - // we thus leave this to other components, e.g. a gcs, to set this - _manager.flags_for_gimbal = MAV_STORM32_GIMBAL_MANAGER_FLAGS_NONE; - - //TODO: we could claim supervisor and activity if the qshot mode suggests so - // what is then about missions ???? should qshots include a mission mode?? - // what about scripts? - // it's not bad as it is -} - //------------------------------------------------------ -// Health, Prearm, find +// Gimbal and protocol discovery //------------------------------------------------------ -// return true if healthy -bool BP_Mount_STorM32_MAVLink::healthy() const -{ - return const_cast(this)->prearmchecks_do(); // yes, ugly, but I have not overdesigned the backend -} - - void BP_Mount_STorM32_MAVLink::find_gimbal(void) { - // search for gimbal until armed + // search for gimbal only until vehicle is armed if (hal.util->get_soft_armed()) { return; } @@ -511,26 +421,11 @@ void BP_Mount_STorM32_MAVLink::find_gimbal(void) uint32_t tnow_ms = AP_HAL::millis(); // search for gimbal in routing table - if (!_compid) { // we expect that instance 0 has compid = MAV_COMP_ID_GIMBAL, instance 1 has compid = MAV_COMP_ID_GIMBAL2, etc uint8_t compid = (_instance == 0) ? MAV_COMP_ID_GIMBAL : MAV_COMP_ID_GIMBAL2 + (_instance - 1); - if (GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_GIMBAL, compid, _sysid, _chan) && (_sysid == mavlink_system.sysid)) { - _compid = compid; - _request_device_info_tlast_ms = (tnow_ms < 900) ? 0 : tnow_ms - 900; // start sending requests in 100 ms - } else { - // have not yet found a gimbal so return - return; - } - } -/* - - // search for a mavlink enabled gimbal - if (_link == nullptr) { - // we expect that instance 0 has compid = MAV_COMP_ID_GIMBAL, instance 1 has compid = MAV_COMP_ID_GIMBAL2, etc - uint8_t compid = (_instance == 0) ? MAV_COMP_ID_GIMBAL : MAV_COMP_ID_GIMBAL2 + (_instance - 1); - _link = GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_GIMBAL, compid, _sysid); - if (_link == nullptr || && (_sysid != mavlink_system.sysid)) { + bool found = GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_GIMBAL, compid, _sysid, _chan); + if (!found || (_sysid != mavlink_system.sysid)) { // have not yet found a gimbal so return return; } @@ -538,13 +433,12 @@ void BP_Mount_STorM32_MAVLink::find_gimbal(void) _compid = compid; _request_device_info_tlast_ms = (tnow_ms < 900) ? 0 : tnow_ms - 900; // start sending requests in 100 ms } -*/ // request GIMBAL_DEVICE_INFORMATION if (!_got_device_info) { if (tnow_ms - _request_device_info_tlast_ms > 1000) { _request_device_info_tlast_ms = tnow_ms; - send_request_gimbal_device_information(); + send_cmd_request_gimbal_device_information(); } return; } @@ -565,22 +459,109 @@ void BP_Mount_STorM32_MAVLink::determine_protocol(const mavlink_message_t &msg) } switch (msg.msgid) { - case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS: - if (_protocol == PROTOCOL_STORM32_GIMBAL_MANAGER) break; // do not allow switching from gimbal manager mode to gimbal device mode - if (_protocol_auto_cntdown) _protocol_auto_cntdown--; // delay switching to gimbal device mode, to give gimbal manager messages a chance - if (!_protocol_auto_cntdown) { - _protocol = PROTOCOL_ARDUPILOT_LIKE; - } + case MAVLINK_MSG_ID_MOUNT_STATUS: + _protocol = PROTOCOL_MOUNT; break; - - case MAVLINK_MSG_ID_STORM32_GIMBAL_MANAGER_STATUS: // 60011 - _protocol = PROTOCOL_STORM32_GIMBAL_MANAGER; - _protocol_auto_cntdown = PROTOCOL_AUTO_TIMEOUT_CNT; + case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS: + _protocol = PROTOCOL_GIMBAL_DEVICE; break; } } +//------------------------------------------------------ +// Prearm & healthy functions +//------------------------------------------------------ + +const uint32_t FAILURE_FLAGS = + GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR | + GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR | + GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR | + GIMBAL_DEVICE_ERROR_FLAGS_SOFTWARE_ERROR | + GIMBAL_DEVICE_ERROR_FLAGS_COMMS_ERROR; + // GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER; + + +bool BP_Mount_STorM32_MAVLink::prearmchecks_all_ok(void) +{ + return (_armingchecks_enabled && // healthy() called at least once + _prearmchecks_ok && // HEARTBEAT reported ok + _armed && // gimbal is in NORMAL state + ((_device_status.received_failure_flags & FAILURE_FLAGS) == 0));// gimbal has no errors +} + + +void BP_Mount_STorM32_MAVLink::send_prearmchecks_txt(void) +{ + if (!_initialised || !_prearmchecks_ok || !_armed) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: arm", _instance+1); + } else + if (_prearmchecks_ok && (_device_status.received_failure_flags & FAILURE_FLAGS)) { + char txt[255]; + strcpy(txt, ""); + if (_device_status.received_failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR) strcat(txt, "mot,"); + if (_device_status.received_failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR) strcat(txt, "enc,"); + if (_device_status.received_failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR) strcat(txt, "volt,"); + if (txt[0] != '\0') { + txt[strlen(txt)-1] = '\0'; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: %s", _instance+1, txt); + } else { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAILURE FLAGS", _instance+1); + } + } +} + + +// return true if healthy +// this is called when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA is set, else not +// workaround to that healthy() is const +bool BP_Mount_STorM32_MAVLink::healthy() const +{ + return const_cast(this)->prearmchecks_do(); // yes, ugly, but I have not overdesigned the backend +} + + +bool BP_Mount_STorM32_MAVLink::prearmchecks_do(void) +{ + _armingchecks_enabled = true; + + if (prearmchecks_all_ok() && !_prearmchecks_done) { // has just changed + _prearmchecks_done = true; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); + } else + if ((AP_HAL::millis() - _prearmcheck_sendtext_tlast_ms) > 30000) { // we haven't send it for a long time + _prearmcheck_sendtext_tlast_ms = AP_HAL::millis(); + send_prearmchecks_txt(); + } + + // unhealthy until gimbal has fully passed the startup sequence + // implies gimbal has been found and replied with device info, i.e. _compid != 0 and _got_device_info = true + if (!_initialised || !_prearmchecks_ok || !_armed) { + return false; + } + + // if we get this far and mount is not gimbal device then mount is healthy + if (_protocol == PROTOCOL_MOUNT) { + return true; + } + + // unhealthy if attitude status is NOT received within the last second + if ((AP_HAL::millis() - _device_status.received_tlast_ms) > 1000) { + return false; + } + + // check failure flags + // We also check for GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER + // which essentially only means that gimbal got GIMBAL_DEVICE_SET_ATTITUDE messages. + if ((_device_status.received_failure_flags & FAILURE_FLAGS) > 0) { + return false; + } + + // if we get this far the mount is healthy + return true; +} + + //------------------------------------------------------ // MAVLink handle functions //------------------------------------------------------ @@ -594,6 +575,16 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_information(const mavlink_me mavlink_msg_gimbal_device_information_decode(&msg, &_device_info); + if (_device_info.gimbal_device_id != 0) { // something is wrong here, so reject + // some few STorM32 firmware versions have a bug in that they set this field to the gimbal's id + // so reject only if not a gimbal id + // shall be removed after some time + if (!(_device_info.gimbal_device_id == MAV_COMP_ID_GIMBAL) && + !(_device_info.gimbal_device_id >= MAV_COMP_ID_GIMBAL2 && _device_info.gimbal_device_id <= MAV_COMP_ID_GIMBAL6)) { + return; + } + } + // set parameter defaults from gimbal information // Q: why default ?? why not actual value ?? I don't understand this /* @@ -626,113 +617,70 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_attitude_status(const mavlin return; } - // get relevant data mavlink_gimbal_device_attitude_status_t payload; mavlink_msg_gimbal_device_attitude_status_decode(&msg, &payload); - _device.received_flags = payload.flags; + if (payload.gimbal_device_id != 0) { // something is wrong here, so reject + // some few STorM32 firmware versions have a bug in that they set this field to the gimbal's id + // so reject only if not a gimbal id + // shall be removed after some time + if (!(_device_info.gimbal_device_id == MAV_COMP_ID_GIMBAL) && + !(_device_info.gimbal_device_id >= MAV_COMP_ID_GIMBAL2 && _device_info.gimbal_device_id <= MAV_COMP_ID_GIMBAL6)) { + return; + } + } + + // get relevant data + + _device_status.received_flags = payload.flags; // TODO: handle case when received device_flags are not equal to those we send, set with update_gimbal_device_flags_for_gimbal() - _device.received_failure_flags = payload.failure_flags; + _device_status.received_failure_flags = payload.failure_flags; // used for health check - _device.received_tlast_ms = AP_HAL::millis(); + _device_status.received_tlast_ms = AP_HAL::millis(); + + // Euler angles + GimbalQuaternion q(payload.q[0], payload.q[1], payload.q[2], payload.q[3]); + q.to_gimbal_euler(_current_angles.roll, _current_angles.pitch, _current_angles.yaw_bf); + + _current_angles.delta_yaw = payload.delta_yaw; // logging - float roll_rad, pitch_rad, yaw_rad; - GimbalQuaternion quat(payload.q[0], payload.q[1], payload.q[2], payload.q[3]); - quat.to_gimbal_euler(roll_rad, pitch_rad, yaw_rad); BP_LOG("MTS0", BP_LOG_MTS_ATTITUDESTATUS_HEADER, - degrees(roll_rad), - degrees(pitch_rad), - degrees(yaw_rad), - degrees(payload.delta_yaw), + degrees(_current_angles.roll), + degrees(_current_angles.pitch), + degrees(_current_angles.yaw_bf), + degrees(_current_angles.delta_yaw), degrees(AP::ahrs().yaw), payload.flags, payload.failure_flags); // forward to ground as MOUNT_STATUS message - if (payload.target_system) { // trigger sending of MOUNT_STATUS to ground only if target_sysid = 0 + + if (_compid != MAV_COMP_ID_GIMBAL) { // do it only for the 1st gimbal return; } - if (!is_primary()) { + + if (payload.target_system) { // send MOUNT_STATUS to ground only if target_sysid = 0 return; } - MountStatus status = { - .roll_deg = degrees(roll_rad), - .pitch_deg = degrees(pitch_rad), - .yaw_deg = degrees(yaw_rad) }; - - send_mount_status_to_ground(status); // MissionPlaner now "understands" gimbal device attitude status, but doesn't use it for campoint, so we still need to send + send_mount_status_to_ground(); } void BP_Mount_STorM32_MAVLink::handle_msg(const mavlink_message_t &msg) { - if (!_initialised && _compid) { - if (_protocol == PROTOCOL_UNDEFINED) determine_protocol(msg); - return; - } - - switch (msg.msgid) { - // listen to qshot commands and messages to track changes in qshot mode - // these may come from anywhere - case MAVLINK_MSG_ID_COMMAND_LONG: { // 76 - mavlink_command_long_t payload; - mavlink_msg_command_long_decode(&msg, &payload); - switch (payload.command) { - case MAV_CMD_QSHOT_DO_CONFIGURE: // 60020 - uint8_t new_mode = payload.param1; - if (new_mode == MAV_QSHOT_MODE_UNDEFINED) { - _qshot.mode = MAV_QSHOT_MODE_UNDEFINED; - } - if (new_mode != _qshot.mode) { - _qshot.mode = UINT8_MAX; // mode change requested, so put it into hold, must be acknowledged by qshot status - } - break; - } - }break; - - case MAVLINK_MSG_ID_QSHOT_STATUS: { // 60020 - mavlink_qshot_status_t payload; - mavlink_msg_qshot_status_decode(&msg, &payload); - _qshot.mode = payload.mode; // also sets it if it was put on hold in the above - }break; - - // listen to RADIO_RC_CHANNELS messages to stop sending RC_CHANNELS - case MAVLINK_MSG_ID_RADIO_RC_CHANNELS: { // 60045 - _got_radio_rc_channels = true; - }break; - } - - // this msg is not from our system - if (msg.sysid != _sysid) { + if (_protocol == PROTOCOL_UNDEFINED) { // implies !_initialised && _compid + determine_protocol(msg); return; } - // search for a MAVLink camera - // we are somewhat overly strict in that we require both the comp_id and the mav_type to be camera - if (!_camera_compid && (msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) && - (msg.compid >= MAV_COMP_ID_CAMERA) && (msg.compid <= MAV_COMP_ID_CAMERA6)) { - mavlink_heartbeat_t payload; - mavlink_msg_heartbeat_decode(&msg, &payload); - if ((payload.autopilot == MAV_AUTOPILOT_INVALID) && (payload.type == MAV_TYPE_CAMERA)) { - _camera_compid = msg.compid; - } - } - - // listen to STORM32_GIMBAL_MANGER_STATUS to detect activity of the autopilot client - switch (msg.msgid) { - case MAVLINK_MSG_ID_STORM32_GIMBAL_MANAGER_STATUS: { // 60011 - mavlink_storm32_gimbal_manager_status_t payload; - mavlink_msg_storm32_gimbal_manager_status_decode(&msg, &payload); - if (payload.gimbal_id != _compid) break; // not for our gimbal device - _manager.ap_client_is_active = - (payload.supervisor != MAV_STORM32_GIMBAL_MANAGER_CLIENT_NONE) && // a client is supervisor - (payload.manager_flags & MAV_STORM32_GIMBAL_MANAGER_FLAGS_CLIENT_AUTOPILOT_ACTIVE); // and autopilot is active - }break; + // listen to RADIO_RC_CHANNELS messages to stop sending RC_CHANNELS + if (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS) { // 60045 + _got_radio_rc_channels = true; } // this msg is not from our gimbal @@ -749,30 +697,9 @@ void BP_Mount_STorM32_MAVLink::handle_msg(const mavlink_message_t &msg) if ((payload.custom_mode & 0x80000000) == 0) { // we don't follow all changes, but just toggle it to true once _prearmchecks_ok = true; } - if (_armingchecks_enabled && !_prearmchecks_all_ok && prearmchecks_all_ok()) { - _prearmchecks_all_ok = true; - gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); - } - }break; - - #define MAVGIMBAL_EVENT_ID(x) ((uint32_t)154 << 24) + (uint32_t)(x) - - case MAVLINK_MSG_ID_EVENT: { - mavlink_event_t payload; - mavlink_msg_event_decode(&msg, &payload); - if (payload.id != MAVGIMBAL_EVENT_ID(0)) break; // not our event id - struct PACKED tPrearmCheckEventArgument { - uint32_t enabled_flags; - uint32_t fail_flags; - }; - tPrearmCheckEventArgument* p = (tPrearmCheckEventArgument*)payload.arguments; - _prearmcheck.enabled_flags = p->enabled_flags; - _prearmcheck.fail_flags = p->fail_flags & p->enabled_flags; // only keep enabled flags - _prearmcheck.updated = true; -// gcs().send_text(MAV_SEVERITY_INFO, "Event: %d %d", (int)_prearmcheck.enabled_flags, (int)_prearmcheck.fail_flags); - if (_prearmcheck.fail_flags != _prearmcheck.fail_flags_last) { - _prearmcheck.fail_flags_last = _prearmcheck.fail_flags; - _prearmcheck.sendtext_tlast_ms = 0; // a change occurred, so force send immediately + if (prearmchecks_all_ok()) { + _prearmchecks_done = true; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); } }break; } @@ -783,7 +710,7 @@ void BP_Mount_STorM32_MAVLink::handle_msg(const mavlink_message_t &msg) // MAVLink send functions //------------------------------------------------------ -void BP_Mount_STorM32_MAVLink::send_request_gimbal_device_information(void) +void BP_Mount_STorM32_MAVLink::send_cmd_request_gimbal_device_information(void) { if (!HAVE_PAYLOAD_SPACE(_chan, COMMAND_LONG)) { return; @@ -792,156 +719,93 @@ void BP_Mount_STorM32_MAVLink::send_request_gimbal_device_information(void) mavlink_msg_command_long_send( _chan, _sysid, _compid, - MAV_CMD_REQUEST_MESSAGE, 0, - MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, 0, 0, 0, 0, 0, 0); -} - - -void BP_Mount_STorM32_MAVLink::GimbalTarget::get_q_array(float* q_array) -{ - switch (mode) { - case TARGET_MODE_NEUTRAL: - case TARGET_MODE_ANGLE: { - Quaternion q; - q.from_euler(roll, pitch, yaw); - q_array[0] = q.q1; q_array[1] = q.q2; q_array[2] = q.q3; q_array[3] = q.q4; - break; - } - default: - q_array[0] = q_array[1] = q_array[2] = q_array[3] = NAN; - } + MAV_CMD_REQUEST_MESSAGE, // command + 0, // confirmation + MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, 0, 0, 0, 0, 0, 0 // param1 .. param7 + ); } -void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude(GimbalTarget >arget) +// called by send_target_angles() +void BP_Mount_STorM32_MAVLink::send_cmd_do_mount_control(void) { - if (!HAVE_PAYLOAD_SPACE(_chan, GIMBAL_DEVICE_SET_ATTITUDE)) { + if (!HAVE_PAYLOAD_SPACE(_chan, COMMAND_LONG)) { return; } - float q_array[4]; - gtarget.get_q_array(q_array); - - mavlink_msg_gimbal_device_set_attitude_send( - _chan, - _sysid, _compid, - _device.flags_for_gimbal, // gimbal device flags - q_array, // attitude as a quaternion - NAN, NAN, NAN); // angular velocities - - BP_LOG("MTC0", BP_LOG_MTC_GIMBALCONTROL_HEADER, - (uint8_t)1, // GIMBAL_DEVICE_SET_ATTITUDE - degrees(gtarget.roll), degrees(gtarget.pitch), degrees(gtarget.yaw), - _device.flags_for_gimbal, (uint16_t)0, - gtarget.mode, - (uint8_t)0); -} - - -void BP_Mount_STorM32_MAVLink::send_storm32_gimbal_manager_control(GimbalTarget >arget) -{ - if (!HAVE_PAYLOAD_SPACE(_chan, STORM32_GIMBAL_MANAGER_CONTROL)) { + if (mnt_target.target_type == MountTargetType::RATE) { + // we ignore it. We may think to just send angle_rad, but if yaw is eath frame + // this oculd result in pretty strange behavior. So better ignore. return; } - float q_array[4]; - gtarget.get_q_array(q_array); - - mavlink_msg_storm32_gimbal_manager_control_send( + // send command_long command containing a do_mount_control command + // Note: pitch and yaw are reversed + // ATTENTION: uses get_bf_yaw() to ensure body frame, which uses ahrs.yaw, not delta_yaw!!! + mavlink_msg_command_long_send( _chan, _sysid, _compid, - _compid, // gimbal_id - MAV_STORM32_GIMBAL_MANAGER_CLIENT_AUTOPILOT, - _device.flags_for_gimbal, _manager.flags_for_gimbal, - q_array, - NAN, NAN, NAN); // float angular_velocity_x, float angular_velocity_y, float angular_velocity_z - - BP_LOG("MTC0", BP_LOG_MTC_GIMBALCONTROL_HEADER, - (uint8_t)2, // STORM32_GIMBAL_MANAGER_CONTROL - degrees(gtarget.roll), degrees(gtarget.pitch), degrees(gtarget.yaw), - _device.flags_for_gimbal, _manager.flags_for_gimbal, - gtarget.mode, - _qshot.mode); + MAV_CMD_DO_MOUNT_CONTROL, // command + 0, // confirmation + -degrees(mnt_target.angle_rad.pitch), // param1 + degrees(mnt_target.angle_rad.roll), // param2 + -degrees(mnt_target.angle_rad.get_bf_yaw()), // param3 + 0, 0, 0, // param4 .. param6 + get_mode() // param7 + ); } -void BP_Mount_STorM32_MAVLink::send_autopilot_state_for_gimbal_device_ext(void) +// called by send_target_angles() +// flags_for_gimbal were just updated, so are correct for sure +void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude(void) { - if (!HAVE_PAYLOAD_SPACE(_chan, AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_EXT)) { + if (!HAVE_PAYLOAD_SPACE(_chan, GIMBAL_DEVICE_SET_ATTITUDE)) { return; } - const AP_AHRS &ahrs = AP::ahrs(); - -#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) - Vector3f airspeed_vec_bf; - if (!ahrs.airspeed_vector_true(airspeed_vec_bf)) { - // if we don't have an airspeed estimate then we don't have a valid wind estimate on copters + if (mnt_target.target_type == MountTargetType::RATE) { + // we ignore it. We may think to just send angle_rad, but if yaw is eath frame + // this oculd result in pretty strange behavior. So better ignore. return; } -#endif -/* - https://github.com/ArduPilot/ardupilot/issues/6571 - v_ground = v_air + v_wind // note direction of wind - there are soo many different accessors, estimates, etc - vfr_hud_airspeed(), - ahrs.groundspeed() - ahrs.groundspeed_vector() - ahrs.yaw, ahrs.yaw_sensor - ahrs.airspeed_estimate(airspeed) - ahrs.airspeed_estimate_true(airspeed) - ahrs.airspeed_vector_true(airspeed_vec_bf) // gives it in BF, not NED! - AP::gps().ground_speed() - AP_Airspeed::get_singleton()->get_airspeed() - ???? - this gives some good insight!? - ahrs.groundspeed_vector() -> search for AP_AHRS_DCM::groundspeed_vector(void) - if airspeed_estimate_true(airspeed) then calculates it as gndVelADS = airspeed_vector + wind2d - else if gotGPS then gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * AP::gps().ground_speed() - => AP does indeed do vg = va + vw - => shows how ground speed is estimated - question: how does this relate to ahrs.get_velocity_NED(vground)? - ahrs.airspeed_estimate(airspeed) - does true_airspeed_vec = nav_vel - wind_vel - this suggests that nav_vel is a good ground speed - also suggests that airspeed, wind are in NED too -*/ - Vector3f wind; - wind = ahrs.wind_estimate(); - - float vehicle_heading = ahrs.yaw; - float wind_heading = atan2f(-wind.y, -wind.x); - float ground_heading = NAN; - float air_heading = NAN; - - float correction_angle = NAN; - Vector3f vground; - if (ahrs.get_velocity_NED(vground)) { // ??? is this correct, really ground speed ??? - Vector3f vair = vground - wind; - float vg2 = vground.x * vground.x + vground.y * vground.y; - float va2 = vair.x * vair.x + vair.y * vair.y; - float vgva = vground.x * vair.x + vground.y * vair.y; - correction_angle = acosf(vgva / sqrtf(vg2 * va2)); - if ((vground.x * vair.y - vground.y * vair.x) < 0.0f) correction_angle = -correction_angle; - - ground_heading = atan2f(vground.y, vground.x); - air_heading = atan2f(vair.y, vair.x); + // convert Euler angles to quaternion + float target_yaw_bf; + if (mnt_target.angle_rad.yaw_is_ef) { + if (isnan(_current_angles.delta_yaw)) { // we don't have a valid yaw_ef + target_yaw_bf = mnt_target.angle_rad.get_bf_yaw(); + } else { + target_yaw_bf = wrap_PI(mnt_target.angle_rad.yaw - _current_angles.delta_yaw); + } + } else { + target_yaw_bf = mnt_target.angle_rad.yaw; } -/* we currently don't send, just log, we need to work it out what we want to do - mavlink_msg_autopilot_state_for_gimbal_device_ext_send( + + GimbalQuaternion q; + q.from_euler(mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, target_yaw_bf); + + float qa[4] = {q.q1, q.q2, q.q3, q.q4}; + + mavlink_msg_gimbal_device_set_attitude_send( _chan, _sysid, _compid, - AP_HAL::micros64(), - wind.x, wind.y, correction_angle); */ + flags_for_gimbal, // gimbal device flags + qa, // attitude as a quaternion + NAN, NAN, NAN // angular velocities + ); - BP_LOG("MTLE", BP_LOG_MTLE_AUTOPILOTSTATEEXT_HEADER, - wind.x, wind.y, degrees(correction_angle), - degrees(vehicle_heading), degrees(wind_heading), degrees(ground_heading), degrees(air_heading)); + + BP_LOG("MTC0", BP_LOG_MTC_GIMBALCONTROL_HEADER, + (uint8_t)1, // GIMBAL_DEVICE_SET_ATTITUDE + degrees(mnt_target.angle_rad.roll), degrees(mnt_target.angle_rad.pitch), degrees(target_yaw_bf), + (uint16_t)flags_for_gimbal, (uint16_t)0, + (uint8_t)mnt_target.target_type, + (uint8_t)0); } -enum THISWOULDBEGREATTOHAVE { +enum LandedStateThisWouldBeGreatToHave { MAV_LANDED_STATE_PREPARING_FOR_TAKEOFF = 5, }; @@ -954,30 +818,35 @@ void BP_Mount_STorM32_MAVLink::send_autopilot_state_for_gimbal_device(void) const AP_AHRS &ahrs = AP::ahrs(); - Quaternion quat; - if (!ahrs.get_quaternion(quat)) { // it returns a bool, so it's a good idea to consider it - quat.q1 = quat.q2 = quat.q3 = quat.q4 = NAN; + // get vehicle attidude + Quaternion q; + if (!ahrs.get_quaternion(q)) { // it returns a bool, so it's a good idea to consider it + q.q1 = q.q2 = q.q3 = q.q4 = NAN; } - float q[4] = { quat.q1, quat.q2, quat.q3, quat.q4 }; + // get vehicle velocity // comment in AP_AHRS.cpp says "Must only be called if have_inertial_nav() is true", but probably not worth checking Vector3f vel; if (!ahrs.get_velocity_NED(vel)) { // it returns a bool, so it's a good idea to consider it - vel.x = vel.y = vel.z = 0.0f; // or NAN ??? + vel.x = vel.y = vel.z = 0.0f; // or NAN ? } - float angular_velocity_z = ahrs.get_yaw_rate_earth(); // NAN; + // get vehicle angular velocity z + float angular_velocity_z = ahrs.get_yaw_rate_earth(); - float yawrate = NAN; + // get commanded yaw rate // see https://github.com/ArduPilot/ardupilot/issues/22564 + float yawrate = NAN; const AP_Vehicle *vehicle = AP::vehicle(); Vector3f rate_ef_targets; if ((vehicle != nullptr) && vehicle->get_rate_ef_targets(rate_ef_targets)) { yawrate = rate_ef_targets.z; } -// TODO: how do notify.flags.armed and hal.util->get_soft_armed() compare against each other, also across vehicles? -/* old: + // determine estimator status +/* +TODO: how do notify.flags.armed and hal.util->get_soft_armed() compare against each other, also across vehicles? +old: bool ahrs_nav_status_horiz_vel = false; nav_filter_status nav_status; if (ahrs.get_filter_status(nav_status) && nav_status.flags.horiz_vel) { @@ -993,8 +862,7 @@ void BP_Mount_STorM32_MAVLink::send_autopilot_state_for_gimbal_device(void) //if (hal.util->get_soft_armed()) { status |= STORM32LINK_FCSTATUS_AP_ARMED; } // for copter this is !ap.land_complete, for plane this is new_is_flying if (notify.flags.flying) { status |= STORM32LINK_FCSTATUS_AP_ISFLYING; } -*/ -/* + mavlink flags nav_filter_status ESTIMATOR_ATTITUDE attitude : 1; // 0 - true if attitude estimate is valid ESTIMATOR_VELOCITY_HORIZ horiz_vel : 1; // 1 - true if horizontal velocity estimate is valid @@ -1016,7 +884,21 @@ ESTIMATOR_ACCEL_ERROR takeoff : 1; // 11 - true if filter i initalized : 1; // 16 - true if the EKF has ever been healthy rejecting_airspeed : 1; // 17 - true if we are rejecting airspeed data dead_reckoning : 1; // 18 - true if we are dead reckoning (e.g. no position or velocity source) + +estimator status: + STorM32 only listens to ESTIMATOR_ATTITUDE and ESTIMATOR_VELOCITY_VERT + ahrs.healthy() becomes true during flip of quaternion => no-go + ahrs.initialised() is simply set after AP_AHRS_NAVEKF_SETTLE_TIME_MS 20000 !! + it becomes true some secs after ESTIMATOR_ATTITUDE|ESTIMATOR_VELOCITY_VERT are set + ESTIMATOR_ATTITUDE can be set during flip of quaternion => no-go + ESTIMATOR_VELOCITY_VERT are set briefly after the quaternion flip ??Q: really, couldn't it also be raised in the flip? + ESTIMATOR_VELOCITY_VERT are set however even if there is no gps or alike! I find it hard to trust the data + => we fake it so: + for ESTIMATOR_ATTITUDE we delay the flag being raised by few secs + for ESTIMATOR_VELOCITY_VERT we check for gps like in AP_AHRS_DCM::groundspeed_vector(void) + btw STorM32 does also check for non-zero velocities for AHRSFix */ + uint16_t nav_estimator_status = 0; uint16_t nav_estimator_status2 = 0; @@ -1034,23 +916,14 @@ ESTIMATOR_ACCEL_ERROR takeoff : 1; // 11 - true if filter i nav_estimator_status2 = (uint16_t)(nav_status.value >> 10); } -/* estimator status - btw STorM32 only listens to ESTIMATOR_ATTITUDE and ESTIMATOR_VELOCITY_VERT - ahrs.healthy() becomes true during flip of quaternion => no-go - ahrs.initialised() is simply set after AP_AHRS_NAVEKF_SETTLE_TIME_MS 20000 !! - it becomes true some secs after ESTIMATOR_ATTITUDE|ESTIMATOR_VELOCITY_VERT are set - ESTIMATOR_ATTITUDE can be set during flip of quaternion => no-go - ESTIMATOR_VELOCITY_VERT are set briefly after the quaternion flip ??Q: really, couldn't it also be raised in the flip? - ESTIMATOR_VELOCITY_VERT are set however even if there is no gps or alike! I find it hard to trust the data - => we fake it so: - for ESTIMATOR_ATTITUDE we delay the flag being raised by few secs - for ESTIMATOR_VELOCITY_VERT we check for gps like in AP_AHRS_DCM::groundspeed_vector(void) - btw STorM32 does also check for non-zero velocities for AHRSFix -*/ uint16_t estimator_status = 0; + static uint32_t tahrs_healthy_ms = 0; const bool ahrs_healthy = ahrs.healthy(); // it's a bit costly - if (!tahrs_healthy_ms && ahrs_healthy) tahrs_healthy_ms = AP_HAL::millis(); + if (!tahrs_healthy_ms && ahrs_healthy) { + tahrs_healthy_ms = AP_HAL::millis(); + } + if (ahrs_healthy && (nav_estimator_status & ESTIMATOR_ATTITUDE) && ((AP_HAL::millis() - tahrs_healthy_ms) > 3000)) { estimator_status |= ESTIMATOR_ATTITUDE; // -> QFix if (ahrs.initialised() && (nav_estimator_status & ESTIMATOR_VELOCITY_VERT) && (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D)) { @@ -1058,43 +931,54 @@ ESTIMATOR_ACCEL_ERROR takeoff : 1; // 11 - true if filter i } } -/* landed state - GCS_Common.cpp: virtual MAV_LANDED_STATE landed_state() const { return MAV_LANDED_STATE_UNDEFINED; } - Copter has it: GCS_MAVLINK_Copter::landed_state(), yields ON_GROUND, TAKEOFF, IN_AIR, LANDING - Plane has it: GCS_MAVLINK_Plane::landed_state(), only yields ON_GROUND or IN_AIR - Blimp also has it, blimp not relevant for us - but is protected, so we needed to mock it up - we probably want to also take into account the arming state to mock something up - ugly as we will have vehicle dependency here + // determine landed state +/* +landed state: + GCS_Common.cpp: virtual MAV_LANDED_STATE landed_state() const { return MAV_LANDED_STATE_UNDEFINED; } + But protected so we can access it. Hence either (1) move to public, (2) add public getter to gcs class, + or (3) add it to vehicle. Latter is most work but nicest, IMHO. + Copter has it: GCS_MAVLINK_Copter::landed_state(), yields ON_GROUND, TAKEOFF, IN_AIR, LANDING + Plane has it: GCS_MAVLINK_Plane::landed_state(), only yields ON_GROUND or IN_AIR + Blimp also has it, blimp not relevant for us + but is protected, so we needed to mock it up + we probably want to also take into account the arming state to mock something up + ugly as we will have vehicle dependency here */ - uint8_t landed_state = gcs().get_landed_state(); + uint8_t landed_state = AP::vehicle()->get_landed_state(); #if APM_BUILD_TYPE(APM_BUILD_ArduCopter) -// for copter we modify the landed states so as to reflect the 2 sec pre-take-off period -// the way it is done leads to a PREPARING_FOR_TAKEOFF before takeoff, but also after landing! -// we somehow would have to catch that it was flying before to suppress it -// but we don't care, the gimbal will do inits when ON_GROUND, and apply them when transitioned to PREPARING_FOR_TAKEOFF -// it won't do it for other transitions, so e.g. also not for plane + // for copter we modify the landed states so as to reflect the 2 sec pre-take-off period + // leads to a PREPARING_FOR_TAKEOFF before takeoff, but also after landing! + // for the latter we would have to catch that it was flying, but we don't need to care + // the gimbal will do inits when ON_GROUND, and apply them when transitioning to PREPARING_FOR_TAKEOFF + // it won't do it for other transitions, so e.g. also not for plane const AP_Notify ¬ify = AP::notify(); - if ((landed_state == MAV_LANDED_STATE_ON_GROUND) && notify.flags.armed) landed_state = MAV_LANDED_STATE_PREPARING_FOR_TAKEOFF; + if ((landed_state == MAV_LANDED_STATE_ON_GROUND) && notify.flags.armed) { + landed_state = MAV_LANDED_STATE_PREPARING_FOR_TAKEOFF; + } #endif + // ready to send static uint32_t tlast_us = 0; uint32_t t_us = AP_HAL::micros(); uint32_t dt_us = t_us - tlast_us; tlast_us = t_us; + float qa[4] = {q.q1, q.q2, q.q3, q.q4}; + mavlink_msg_autopilot_state_for_gimbal_device_send( _chan, _sysid, _compid, - AP_HAL::micros64(), - q, - 0, // uint32_t q_estimated_delay_us, - vel.x, vel.y, vel.z, - 0, // uint32_t v_estimated_delay_us, - yawrate, + AP_HAL::micros64(), // uint64_t time_boot_us + qa, // attitude quaternion + 0, // uint32_t q_estimated_delay_us, + vel.x, vel.y, vel.z, // angular velocity vx, vy, vz + 0, // uint32_t v_estimated_delay_us, + yawrate, // float feed_forward_angular_velocity_z estimator_status, landed_state, - angular_velocity_z); + angular_velocity_z // float angular_velocity_z + ); + BP_LOG("MTL0", BP_LOG_MTL_AUTOPILOTSTATE_HEADER, q[0],q[1],q[2],q[3], @@ -1122,8 +1006,9 @@ void BP_Mount_STorM32_MAVLink::send_system_time(void) mavlink_msg_system_time_send( _chan, - time_unix, - AP_HAL::millis()); + time_unix, // uint64_t time_unix_usec + AP_HAL::millis() // uint32_t time_boot_ms + ); } @@ -1141,12 +1026,13 @@ void BP_Mount_STorM32_MAVLink::send_rc_channels(void) mavlink_msg_rc_channels_send( _chan, - AP_HAL::millis(), - 16, - RCHALIN(0), RCHALIN(1), RCHALIN(2), RCHALIN(3), RCHALIN(4), RCHALIN(5), RCHALIN(6), RCHALIN(7), - RCHALIN(8), RCHALIN(9), RCHALIN(10), RCHALIN(11), RCHALIN(12), RCHALIN(13), RCHALIN(14), RCHALIN(15), - 0, 0, - 0); + AP_HAL::millis(), // uint32_t time_boot_ms + 16, // uint8_t chancount, STorM32 won't handle more than 16 anyhow + RCHALIN(0), RCHALIN(1), RCHALIN(2), RCHALIN(3), RCHALIN(4), RCHALIN(5), RCHALIN(6), RCHALIN(7), // uint16_t chan1_raw .. + RCHALIN(8), RCHALIN(9), RCHALIN(10), RCHALIN(11), RCHALIN(12), RCHALIN(13), RCHALIN(14), RCHALIN(15), // .. chan16_raw + 0, 0, // uint16_t chan17_raw .. chan18_raw + 255 // uint8_t rssi, 255: invalid/unknown + ); } @@ -1154,13 +1040,13 @@ void BP_Mount_STorM32_MAVLink::send_banner(void) { if (_got_device_info) { // we have lots of info - gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: gimbal at %u%s", _instance+1, _compid, (is_primary()) ? ", is primary" : ""); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: gimbal at %u", _instance+1, _compid); // we can convert the firmware version to STorM32 convention char c = (_device_info.firmware_version & 0x00FF0000) >> 16; if (c == '\0') c = ' '; else c += 'a' - 1; - gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: %s %s v%u.%u%c", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: %s %s v%u.%u%c", _instance + 1, _device_info.vendor_name, _device_info.model_name, @@ -1169,116 +1055,111 @@ void BP_Mount_STorM32_MAVLink::send_banner(void) c ); - gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: prearm checks %s", _instance+1, (_prearmchecks_all_ok) ? "passed" : "fail"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks %s", _instance+1, (prearmchecks_all_ok()) ? "passed" : "fail"); } else if (_compid) { // we have some info - gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: gimbal at %u%s", _instance+1, _compid, (is_primary()) ? ", is primary" : ""); - gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: prearm checks %s", _instance+1, (_prearmchecks_all_ok) ? "passed" : "fail"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: gimbal at %u", _instance+1, _compid); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks %s", _instance+1, (prearmchecks_all_ok()) ? "passed" : "fail"); } else { // we don't know yet anything - gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: no gimbal yet", _instance+1); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: no gimbal yet", _instance+1); } } //------------------------------------------------------ -// helper +// MAVLink gimbal manager send functions //------------------------------------------------------ -const uint32_t FAILURE_FLAGS = - GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR | - GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR | - GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR | - GIMBAL_DEVICE_ERROR_FLAGS_SOFTWARE_ERROR | - GIMBAL_DEVICE_ERROR_FLAGS_COMMS_ERROR; - // GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER; - -bool BP_Mount_STorM32_MAVLink::prearmchecks_all_ok(void) +// send a GIMBAL_MANAGER_INFORMATION message to GCS +void BP_Mount_STorM32_MAVLink::send_gimbal_manager_information(mavlink_channel_t chan) { - return (_prearmchecks_ok && _armed && ((_device.received_failure_flags & FAILURE_FLAGS) == 0)); + // space already checked by streamer + + // There are few specific gimbal manager capability flags, which we don't use. + // So we simply can carry forward the cap_flags received from the gimbal. + uint32_t cap_flags = _device_info.cap_flags; + + // Not all capabilities are supported by this driver, so we erase them. + // ATTENTION: This can mean that the gimbal device and gimbal manager capability flags + // may be different, and any third party which mistakenly thinks it can use those from + // the gimbal device messages may get confused ! + cap_flags &=~ (GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_FOLLOW | + GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_FOLLOW | + GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK | + GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_YAW_IN_EARTH_FRAME); + + uint8_t gimbal_device_id = _compid; + + mavlink_msg_gimbal_manager_information_send( + chan, + AP_HAL::millis(), // autopilot system time + cap_flags, // bitmap of gimbal manager capability flags + gimbal_device_id, // gimbal device id + radians(_params.roll_angle_min), // roll_min in radians + radians(_params.roll_angle_max), // roll_max in radians + radians(_params.pitch_angle_min), // pitch_min in radians + radians(_params.pitch_angle_max), // pitch_max in radians + radians(_params.yaw_angle_min), // yaw_min in radians + radians(_params.yaw_angle_max) // yaw_max in radians + ); } -void BP_Mount_STorM32_MAVLink::send_prearmchecks_txt(void) +// return gimbal manager flags used by GIMBAL_MANAGER_STATUS message +uint32_t BP_Mount_STorM32_MAVLink::get_gimbal_manager_flags() const { - if (_prearmcheck.available() && !_prearmchecks_ok) { // we got a new message, and it is still not ok - _prearmcheck.updated = false; + // There are currently no specific gimbal manager flags. So we simply + // can carry forward the _flags received from the gimbal. - char txt[255]; - strcpy(txt, ""); - if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_IS_NORMAL) strcat(txt, "arm,"); - if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_IMUS_WORKING) strcat(txt, "imu,"); - if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_MOTORS_WORKING) strcat(txt, "mot,"); - if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_ENCODERS_WORKING) strcat(txt, "enc,"); - if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_VOLTAGE_OK) strcat(txt, "volt,"); - if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_VIRTUALCHANNELS_RECEIVING) strcat(txt, "chan,"); - if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_MAVLINK_RECEIVING) strcat(txt, "mav,"); - if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_STORM32LINK_QFIX) strcat(txt, "qfix,"); - if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_STORM32LINK_WORKING) strcat(txt, "stl,"); - if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_CAMERA_CONNECTED) strcat(txt, "cam,"); - if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_AUX0_LOW) strcat(txt, "aux0,"); - if (_prearmcheck.fail_flags & MAV_STORM32_GIMBAL_PREARM_FLAGS_AUX1_LOW) strcat(txt, "aux1,"); - if (txt[0] != '\0') { - txt[strlen(txt)-1] = '\0'; - } else { - strcpy(txt, "unknown"); - } - gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: %s", _instance+1, txt); - } else - if (!_initialised || !_prearmchecks_ok || !_armed) { - gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: arm", _instance+1); - } else - if (_prearmchecks_ok && (_device.received_failure_flags & FAILURE_FLAGS)) { - char txt[255]; - strcpy(txt, ""); - if (_device.received_failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR) strcat(txt, "mot,"); - if (_device.received_failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR) strcat(txt, "enc,"); - if (_device.received_failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR) strcat(txt, "volt,"); - if (txt[0] != '\0') { - txt[strlen(txt)-1] = '\0'; - gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: %s", _instance+1, txt); - } else { - gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAILURE FLAGS", _instance+1); - } - } + // ATTENTION: Not all capabilities are supported by this driver, but this + // should never be a problem since any third party should strictly adhere + // to the capability flags! + + return _device_status.received_flags; } -// this is called when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA is set -bool BP_Mount_STorM32_MAVLink::prearmchecks_do(void) -{ - _armingchecks_enabled = true; +//------------------------------------------------------ +// Scripting accescors, and get attitude fakery +//------------------------------------------------------ - if ((AP_HAL::millis() - _prearmcheck.sendtext_tlast_ms) > 30000) { // we haven't send it for a long time - _prearmcheck.sendtext_tlast_ms = AP_HAL::millis(); - send_prearmchecks_txt(); - } +// return target location if available +// returns true if a target location is available and fills in target_loc argument +bool BP_Mount_STorM32_MAVLink::get_location_target(Location &_target_loc) +{/* + if (target_loc_valid) { + _target_loc = target_loc; + return true; + } */ + return false; +} - if (!_prearmchecks_all_ok && prearmchecks_all_ok()) { // has just changed - _prearmchecks_all_ok = true; - gcs().send_text(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); - } - // unhealthy until gimbal has fully passed the startup sequence - if (!_initialised || !_prearmchecks_ok || !_armed) { - return false; - } +// update mount's actual angles (to be called by script communicating with mount) +void BP_Mount_STorM32_MAVLink::set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) +{ + _script_control_angles.roll = radians(roll_deg); + _script_control_angles.pitch = radians(pitch_deg); + _script_control_angles.yaw_bf = radians(yaw_bf_deg); +} - // unhealthy if attitude status is NOT received within the last second - if ((AP_HAL::millis() - _device.received_tlast_ms) > 1000) { + +// get attitude as a quaternion. Returns true on success +bool BP_Mount_STorM32_MAVLink::get_attitude_quaternion(Quaternion& att_quat) +{ + if (!_initialised) { return false; } - // check failure flags - // we also check for GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER, it essentially only means that STorM32 got GIMBAL_DEVICE_SET_ATTITUDE messages - if ((_device.received_failure_flags & FAILURE_FLAGS) > 0) { + if (_protocol != PROTOCOL_GIMBAL_DEVICE) { // not supported if not gimbal device return false; } - // if we get this far the mount is healthy + att_quat.from_euler(0.0f, _current_angles.pitch, _current_angles.yaw_bf); return true; } @@ -1300,7 +1181,7 @@ bool BP_Mount_STorM32_MAVLink::take_picture() send_cmd_do_digicam_control(true); -// gcs().send_text(MAV_SEVERITY_INFO, "cam take pic"); +// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "cam take pic"); return true; } @@ -1319,7 +1200,7 @@ bool BP_Mount_STorM32_MAVLink::record_video(bool start_recording) send_cmd_do_digicam_control(start_recording); -// gcs().send_text(MAV_SEVERITY_INFO, "cam rec video %u", start_recording); +// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "cam rec video %u", start_recording); return true; } @@ -1332,13 +1213,13 @@ bool BP_Mount_STorM32_MAVLink::set_cam_mode(bool video_mode) _camera_mode = (video_mode) ? CAMERA_MODE_VIDEO : CAMERA_MODE_PHOTO; send_cmd_do_digicam_configure(video_mode); -// gcs().send_text(MAV_SEVERITY_INFO, "cam set mode %u", video_mode); +// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "cam set mode %u", video_mode); return true; } -bool BP_Mount_STorM32_MAVLink::set_cam_photo_video(int8_t sw_flag) +bool BP_Mount_STorM32_MAVLink::set_cam_photo_video_mode(int8_t sw_flag) { if (!_use_3way_photo_video) return false; @@ -1376,10 +1257,12 @@ void BP_Mount_STorM32_MAVLink::send_cmd_do_digicam_configure(bool video_mode) mavlink_msg_command_long_send( _chan, _sysid, _compid, - MAV_CMD_DO_DIGICAM_CONFIGURE, 0, - param1, 0, 0, 0, 0, 0, 0); + MAV_CMD_DO_DIGICAM_CONFIGURE, // command + 0, // confirmation + param1, 0, 0, 0, 0, 0, 0 // param1 .. param7 + ); -// gcs().send_text(MAV_SEVERITY_INFO, "cam digi config %u", video_mode); +// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "cam digi config %u", video_mode); } @@ -1394,38 +1277,41 @@ void BP_Mount_STorM32_MAVLink::send_cmd_do_digicam_control(bool shoot) mavlink_msg_command_long_send( _chan, _sysid, _compid, - MAV_CMD_DO_DIGICAM_CONTROL, 0, - 0, 0, 0, 0, param5, 0, 0); + MAV_CMD_DO_DIGICAM_CONTROL, // command + 0, // confirmation + 0, 0, 0, 0, param5, 0, 0 // param1 .. param7 + ); -// gcs().send_text(MAV_SEVERITY_INFO, "cam digi cntrl %u", shoot); +// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "cam digi cntrl %u", shoot); } //------------------------------------------------------ -// MAVLink MOUNT_STATUS forwarding +// MAVLink mount status forwarding //------------------------------------------------------ // forward a MOUNT_STATUS message to ground, this is only to make MissionPlanner and alike happy -void BP_Mount_STorM32_MAVLink::send_mount_status_to_ground(MountStatus &status) +void BP_Mount_STorM32_MAVLink::send_mount_status_to_ground(void) { // space is checked by send_to_ground() - mavlink_mount_status_t msg = { - pointing_a : (int32_t)(status.pitch_deg * 100.0f), - pointing_b : (int32_t)(status.roll_deg * 100.0f), - pointing_c : (int32_t)(status.yaw_deg * 100.0f), - target_system : 0, - target_component : 0 }; + const mavlink_mount_status_t pkt = { + (int32_t)(degrees(_current_angles.pitch) * 100.0f), // int32_t pointing_a + (int32_t)(degrees(_current_angles.roll) * 100.0f), // int32_t pointing_b + (int32_t)(degrees(_current_angles.yaw_bf) * 100.0f), // int32_t pointing_c + 0, // uint8_t target_system + 0, // uint8_t target_component + get_mode() // uint8_t mount_mode + }; - send_to_ground(MAVLINK_MSG_ID_MOUNT_STATUS, (const char*)&msg); + send_to_ground(MAVLINK_MSG_ID_MOUNT_STATUS, (const char*)&pkt); } // this is essentially GCS::send_to_active_channels(uint32_t msgid, const char *pkt) // but exempts the gimbal channel -//TODO: Is dodgy as it assumes that ONLY the gimbal is on the link !! -// We actually only need to send to the ground, i.e., to the gcs-es (there could be more than one) -// This is what this achieves for gcs-ap-g or gcs-ap-cc-g topologies, but not for others +// It assumes that the gimbal and any GCS are not on the same link, which may not be the +// case in all and every situation but should a pretty fair assumption. void BP_Mount_STorM32_MAVLink::send_to_ground(uint32_t msgid, const char *pkt) { const mavlink_msg_entry_t* entry = mavlink_get_msg_entry(msgid); @@ -1440,10 +1326,8 @@ void BP_Mount_STorM32_MAVLink::send_to_ground(uint32_t msgid, const char *pkt) if (!c.is_active()) { continue; } - if (!c.is_active()) { - continue; - } - // size checks done by this method: + + // space check is done by this method c.send_message(pkt, entry); } } diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index f66d675c656b8..fd529134a587c 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -9,9 +9,6 @@ #include "AP_Mount_Backend.h" -#define PROTOCOL_AUTO_TIMEOUT_CNT 10 - - class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend { @@ -20,239 +17,269 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend BP_Mount_STorM32_MAVLink(AP_Mount &frontend, AP_Mount_Params ¶ms, uint8_t instance); // init - performs any required initialisation for this instance - void init() override {} + void init() override { AP_Mount_Backend::init(); } - // update mount position - should be called periodically + // update mount position - should be called periodically at 50 Hz void update() override; // used for gimbals that need to read INS data at full rate void update_fast() override {} // return true if healthy - // is called by mount pre_arm_checks() (and nowhere else) - // which in turn is called in AP_Arming::pre_arm_checks() (and nowhere else) - // mount prearm checks are tied to camera, and enabled by setting - // thus Gremsy's method for e.g. checking attitude_status doesn't make much sense + // Is called by mount pre_arm_checks() (and nowhere else), which in turn is called + // in AP_Arming::pre_arm_checks() (and nowhere else). + // The mount prearm checks are tied to camera, and enabled by setting ARMING_CHECK_CAMERA. + // Thus Gremsy's method for e.g. checking attitude_status doesn't really make much sense. bool healthy() const override; + + // return true if this mount accepts roll or pitch targets + // work it out later + // Only used in get_gimbal_manager_capability_flags(), which we overwrite anyhow + // => irrelevant for now + bool has_roll_control() const override { return true; } + bool has_pitch_control() const override { return true; } + // returns true if this mount can control its pan (required for multicopters) - // STorM32: work it out later // false: copter is rotated in yaw, not the gimbal - // this is evaluated in + // work it out later + // This is evaluated in // - GCS_MAVLINK_Copter::handle_command_mount(), cmd = MAV_CMD_DO_MOUNT_CONTROL // - GCS_MAVLINK_Copter::handle_mount_message(), msg = MAVLINK_MSG_ID_MOUNT_CONTROL // - Copter Mode::AutoYaw::set_roi() // - Copter ModeAuto::do_mount_control() bool has_pan_control() const override { return false; } - // set yaw_lock - // STorM32: this needs to not set _yaw_lock, as - // (1) STorM32 doesn't (yet) support earth frame, it does support yaw lock in vehicle frame - // (2) _yaw_lock and vehicle or earth frame are not fundamentally related to each other - // in the backend, _yaw_lock is used as yaw_is_ef - void set_yaw_lock(bool yaw_lock) override { _is_yaw_lock = yaw_lock; } + //>> Deal with some AP nonsense: + + // get attitude as a quaternion. Returns true on success + // att_quat will be an earth-frame quaternion rotated such that yaw is in body-frame. + // This is used in + // - AP_Mount_Backend::send_gimbal_device_attitude_status() + // - AP_Mount::get_attitude_euler() + // - AP_Mount_Backend::write_log() via calling AP_Mount::get_attitude_euler() + // - scripts via calling AP_Mount::get_attitude_euler() + // AP nonsense: uses inappropriate Euler angles. + // => we set roll to zero, to minimize harm. + bool get_attitude_quaternion(Quaternion& att_quat) override; + + // set yaw_lock. If true, the gimbal's yaw target is maintained in earth-frame meaning + // it will lock onto an earth-frame heading (e.g. North). If false (aka "follow") the gimbal's yaw + // is maintained in body-frame meaning the gimbal will rotate with the vehicle. + // This must not set _yaw_lock, since + // - AP nonsense: mixes up yaw lock and vehicle or earth frame, which are not fundamentally related to each other + // - STorM32 supports yaw lock in vehicle frame, but doesn't (yet) support earth frame + // => we can't use yaw lock without massive changes, and need to overwrite it + // In the backend, _yaw_lock is used as yaw_is_ef. However, gladly, backend never sets + // it by itself, so setting it to false in this driver will ensure it's false. + // This is called for AUX_FUNC::MOUNT_LOCK switch setting. + void set_yaw_lock(bool yaw_lock) override {} + + // send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS + // Is part of stream Extra3. This is nonsense. Streaming is appropriate only if the gimbal is + // not a gimbal device, but then it should be streamed to all parties except the gimbal, which + // seems not to be what is done. + // => we thus make it to do nothing. + // We don't do any of ArduPilot's private mavlink channel nonsense. + void send_gimbal_device_attitude_status(mavlink_channel_t chan) override {} + + // send a GIMBAL_MANAGER_INFORMATION message to GCS + // AP nonsense: does it wrong, just good for its own limits. + // => we need to overwrite it + void send_gimbal_manager_information(mavlink_channel_t chan) override; + + // added: return gimbal manager flags used by GIMBAL_MANAGER_STATUS message + // AP nonsense: does it wrong, just good for its own limits. + // => we need to overwrite it + uint32_t get_gimbal_manager_flags() const override; + + // added: handle gimbal manager flags received from gimbal manager messages + // AP nonsense: does it wrong, just good for its limits. + // => we need to overwrite it + bool handle_gimbal_manager_flags(uint32_t flags) override; + + // end of AP nonsense<< // handle GIMBAL_DEVICE_INFORMATION message + // empty => we need to overwrite it void handle_gimbal_device_information(const mavlink_message_t &msg) override; // handle GIMBAL_DEVICE_ATTITUDE_STATUS message + // empty => we need to overwrite it void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) override; - // send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS - // STorM32: this needs to be empty, because the gimbal device is sending it itself - // we don't do any of ArduPilot's private mavlink channel nonsense - void send_gimbal_device_attitude_status(mavlink_channel_t chan) override {} + // added: handle msg - allows to process a msg from a gimbal + void handle_msg(const mavlink_message_t &msg) override; + + // added: send banner + void send_banner(void) override; + + + // accessors for scripting backends + // AP provides them only for script backend, but can be useful generally. + bool get_location_target(Location &target_loc) override; + void set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) override; + + // + // camera controls for gimbals that include a camera + // // take a picture. returns true on success - // we do not need to do anything since AP_Camera::take_picture() will send a a CMD_LONG:DO_DIGICAM_CONTROL to all components - // we have modified this such that it is not send if it is CamTrigType::mount (CAM_TRIGG_TYPE = 3) + // We do not need to do anything since AP_Camera::take_picture() will send a a CMD_LONG:DO_DIGICAM_CONTROL to all components + // we have modified this such that it is not send if it is CamTrigType::mount (CAM_TRIGG_TYPE = 3). bool take_picture() override; // start or stop video recording. returns true on success // set start_recording = true to start record, false to stop recording bool record_video(bool start_recording) override; - // set camera zoom step. returns true on success - // zoom out = -1, hold = 0, zoom in = 1 -//XX ?? bool set_zoom_step(int8_t zoom_step) override { return false; } - - // set photo or video mode - bool set_cam_mode(bool video_mode) override; - - // 3-way switch mode - bool set_cam_photo_video(int8_t sw_flag) override; - -protected: - - // get attitude as a quaternion. returns true on success - // this is used in send_gimbal_device_attitude_status() - // STorM32: empty it, send_gimbal_device_attitude_status() has been emptied also, so no use - bool get_attitude_quaternion(Quaternion& att_quat) override { return false; } + // set zoom specified as a rate or percentage + bool set_zoom(ZoomType zoom_type, float zoom_value) override { return false; } -private: + // send camera information message to GCS + // If this is wanted, the camera component should be enabled in STorM32. + void send_camera_information(mavlink_channel_t chan) const override {} - // search for gimbal in GCS_MAVLink routing table - void find_gimbal(void); + // send camera settings message to GCS + // If this is wanted, the camera component should be enabled in STorM32. + void send_camera_settings(mavlink_channel_t chan) const override {} - // request GIMBAL_DEVICE_INFORMATION from gimbal (holds vendor and model name, min/max angles) - void send_request_gimbal_device_information(void); + // added: set photo or video mode + bool set_cam_mode(bool video_mode) override; -// -- this so far is the interface provided by AP_Mount_Backend and as seen in the Gremsy driver -// -- here now come our methods -public: + // added: 3-way switch mode + bool set_cam_photo_video_mode(int8_t sw_flag) override; - // handle msg - allows to process a msg from a gimbal - void handle_msg(const mavlink_message_t &msg) override; - - // send banner - void send_banner(void) override; private: // internal variables - bool _got_device_info; // true once gimbal has provided device info - bool _initialised; // true once startup procedure has been fully completed + uint8_t _sysid; // sysid of gimbal + uint8_t _compid; // component id of gimbal, zero if gimbal not yet discovered + mavlink_channel_t _chan; // mavlink channel used to communicate with gimbal + // Comment: in some places the newer _link construct is used, which is however not smart, + // since for each message it does a binary search to find and check the size. This is not + // needed with the older _chan construct, which is thus much more efficient cpu wise. The + // older _chan also allows for an early test for space avoiding potentially lengthy calcs. + // We hence continue to use the _chan construct. + + bool _got_device_info; // gimbal discovered, waiting for gimbal provide device info bool _armed; // true once the gimbal has reached normal state bool _prearmchecks_ok; // true when the gimbal stops reporting prearm fail in the HEARTBEAT message + bool _initialised; // true once all init steps have been passed bool _got_radio_rc_channels; // true when a RADIO_RC_CHANNELS message has been received - // internal MAVLink variables + // gimbal and protocol discovery - uint8_t _sysid; // system id of gimbal, supposedly equal to our flight controller sysid - uint8_t _compid; // component id of gimbal, 0 indicates gimbal not yet found - mavlink_channel_t _chan; // mavlink channel used to communicate with gimbal - - // initialization etc + // search for gimbal in GCS_MAVLink routing table + void find_gimbal(void); uint32_t _request_device_info_tlast_ms; mavlink_gimbal_device_information_t _device_info; - // flags - - bool _sendonly; - bool _send_autopilotstateext; - bool _should_log; - bool _use_3way_photo_video; - - enum PROTOCOLENUM { - PROTOCOL_UNDEFINED = 0, // we do not yet know - PROTOCOL_ARDUPILOT_LIKE, // gimbal uses V2 gimbal device messages - PROTOCOL_STORM32_GIMBAL_MANAGER, // gimbal is a STorM32 gimbal manager - }; - uint8_t _protocol; - - // internal task variables + // request GIMBAL_DEVICE_INFORMATION, we can get this also if 'old' MOUNT messages are used + void send_cmd_request_gimbal_device_information(void); - enum TASKENUM { - TASK_SLOT0 = 0, - TASK_SLOT1, - TASK_SLOT2, - TASK_SLOT3, + enum Protocol { + PROTOCOL_UNDEFINED = 0, // we do not yet know + PROTOCOL_MOUNT, // gimbal uses 'old' MOUNT messages + PROTOCOL_GIMBAL_DEVICE, // gimbal is a v2 gimbal device }; + Protocol _protocol; - uint16_t _task_counter; + // determine protocol based on receiving MOUNT_STATUS or GIMBAL_DEVICE_ATTITUDE_STATUS from gimbal + void determine_protocol(const mavlink_message_t &msg); - // blabla + // pre-arm checks - uint8_t _protocol_auto_cntdown; - void determine_protocol(const mavlink_message_t &msg); + bool _armingchecks_enabled; // true when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA set, we know from healty() + bool _prearmchecks_done; // becomes true when info has send out + uint32_t _prearmcheck_sendtext_tlast_ms; - bool _armingchecks_enabled; // true when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA set - bool _prearmchecks_all_ok; // becomes true when a number of conditions are full filled, track changes bool prearmchecks_all_ok(void); - bool prearmchecks_do(void); // workaround needed since healthy() is const + bool prearmchecks_do(void); void send_prearmchecks_txt(void); - struct { - bool updated; // set to true when a new EVENT message is received - uint32_t enabled_flags; - uint32_t fail_flags; - uint32_t fail_flags_last; - uint32_t sendtext_tlast_ms; - bool available(void) { return updated && (enabled_flags > 0) && (fail_flags > 0); } - } _prearmcheck; // for component prearm status handling + // gimbal target & control - bool _is_yaw_lock; - - void set_and_send_target_angles(void); - void send_autopilot_state_for_gimbal_device(void); - void send_autopilot_state_for_gimbal_device_ext(void); + struct { + uint16_t received_flags; // obtained from GIMBAL_DEVICE_ATTITUDE_STATUS + uint32_t received_failure_flags; // obtained from GIMBAL_DEVICE_ATTITUDE_STATUS + uint32_t received_tlast_ms; // time last GIMBAL_DEVICE_ATTITUDE_STATUS was received (used for health reporting) + } _device_status; - enum GIMBALTARGETMODEENUM { - TARGET_MODE_RETRACT = 0, - TARGET_MODE_NEUTRAL, - TARGET_MODE_ANGLE, - }; + uint16_t flags_for_gimbal; // flags to be send to gimbal device - struct GimbalTarget { + struct { float roll; float pitch; - float yaw; - bool yaw_is_ef; - uint8_t mode; - - void set(MountTarget &t, uint8_t m) { roll = t.roll; pitch = t.pitch; yaw = t.yaw; yaw_is_ef = t.yaw_is_ef; mode = m; } - void set(uint8_t m) { roll = pitch = yaw = 0.0f; yaw_is_ef = false; mode = m; } - void set_angle(MountTarget &t) { set(t, TARGET_MODE_ANGLE); } - void set_from_vec_deg(const Vector3f &v_deg, uint8_t m) { roll = ToRad(v_deg.x); pitch = ToRad(v_deg.y); yaw = ToRad(v_deg.z); yaw_is_ef = false; mode = m; } - void get_q_array(float* q_array); - }; + float yaw_bf; + float delta_yaw; + } _current_angles; // current angles, obtained from either MOUNT_STATUS or GIMBAL_DEVICE_ATTITUDE_STATUS struct { - uint16_t received_flags; // obtained from GIMBAL_DEVICE_ATTITUDE_STATUS - uint32_t received_failure_flags; // obtained from GIMBAL_DEVICE_ATTITUDE_STATUS - uint32_t received_tlast_ms; // time last GIMBAL_DEVICE_ATTITUDE_STATUS was received (used for health reporting) - uint16_t flags_for_gimbal; - } _device; + float roll; + float pitch; + float yaw_bf; + } _script_control_angles; // angles set by script - struct { - bool ap_client_is_active; // true when autopilot client is active - uint16_t flags_for_gimbal; - } _manager; + // set the flags for gimbal according to current condition + void update_gimbal_device_flags(enum MAV_MOUNT_MODE mntmode); - struct { - uint8_t mode; - uint8_t mode_last; - } _qshot; + // determines the target angles based on mount mode, does the crucial job of controlling + void update_target_angles(void); + + // sends the target angles to gimbal, called in update loop with 12.5 Hz + void send_target_angles(void); - bool get_target_angles_mount(GimbalTarget >arget, enum MAV_MOUNT_MODE mmode); - bool get_target_angles_qshot(GimbalTarget >arget); - void update_gimbal_device_flags_mount(enum MAV_MOUNT_MODE mmode); - void update_gimbal_device_flags_qshot(void); - void update_gimbal_manager_flags(void); + // send do_mount_control command with latest angle targets to the gimbal + void send_cmd_do_mount_control(void); - void send_gimbal_device_set_attitude(GimbalTarget >arget); - void send_storm32_gimbal_manager_control(GimbalTarget >arget); + // send GIMBAL_DEVICE_SET_ATTITUDE with latest angle targets to the gimbal to control attitude + // When the gimbal receives this message, it can assume it is coming from its gimbal manager + // (as nobody else is allowed to send this to it). STorM32 thus identifies the message's + // source sysid & compid as its associated gimbal manager's sysid & compid. + void send_gimbal_device_set_attitude(void); + + void send_autopilot_state_for_gimbal_device(void); + void send_rc_channels(void); uint32_t _send_system_time_tlast_ms; void send_system_time(void); - void send_rc_channels(void); + // task - // camera + enum Task { + TASK_SLOT0 = 0, + TASK_SLOT1, + TASK_SLOT2, + TASK_SLOT3, + }; - uint8_t _camera_compid; // component id of a mavlink camera, 0 indicates camera not yet found + uint8_t _task_counter; - enum CAMERAMODEENUM { + // camera + + enum CameraMode { CAMERA_MODE_UNDEFINED = 0, // we do not yet know CAMERA_MODE_PHOTO, CAMERA_MODE_VIDEO, }; - uint8_t _camera_mode; // current camera mode + CameraMode _camera_mode; // current camera mode + + bool _use_3way_photo_video; void send_cmd_do_digicam_configure(bool video_mode); void send_cmd_do_digicam_control(bool shoot); - // mount_status forwarding + // logging - struct MountStatus { - float roll_deg; - float pitch_deg; - float yaw_deg; - }; + bool _should_log; - void send_mount_status_to_ground(MountStatus &status); + // mount_status forwarding + // MissionPlaner "understands" gimbal device attitude status, but doesn't use it for campoint, so we still need to send + void send_mount_status_to_ground(void); void send_to_ground(uint32_t msgid, const char *pkt); }; diff --git a/libraries/bp_version.h b/libraries/bp_version.h index d4fff4babd1e1..ffa4c256858b9 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,11 +1,11 @@ #pragma once -#define BETAPILOTVERSION "v058" +#define BETAPILOTVERSION "v059" /* search for //OW to find all changes -2023.08.10: v058 +2023.11.01: v059 upgraded to ArduPilot master 4.5.0-dev 2023.03.10: v058 @@ -40,7 +40,7 @@ search for //OW to find all changes ArduCopter specific -- GCS_Mavlink.cpp: (2 comments, no change) 1x RADIOLINK +- GCS_Mavlink.cpp: 1x RADIOLINK - version.h: 1x ArduPlane specific @@ -50,20 +50,19 @@ ArduPlane specific Libraries: - AP_Camera.cpp: 1x - AP_Camera.h: 1x -- AP_Mount_Backend.cpp: ? -- AP_Mount_Backend.h: 1x +- AP_Mount_Backend.cpp: 3x +- AP_Mount_Backend.h: 4x - AP_Mount_Params.cpp: 1x - AP_Mount_Params.h: 1x -- AP_Mount.cpp: 3x -- AP_Mount.h: 8x +- AP_Mount.cpp: 16x +- AP_Mount.h: 7x - AP_RCProtocol.cpp: 4x RADIOLINK - AP_RCProtocol.h: 4x RADIOLINK - AP_RSSI.h: 1x RADIOLINK - GCS_Common.cpp: 2x 3x RADIOLINK - GCS_MAVLink.h: 2x -- GCS.h: 2x 1x RADIOLINK -- MAVLink_routing.cpp: 1x 1x RADIOLINK -- MAVLink_routing.h: 1x +- GCS.h: 1x 1x RADIOLINK +- MAVLink_routing.cpp: 1x RADIOLINK - RC_Channel.cpp: 4x Additional Files in library: From a32b053af61e300a235368c601c973eb3ccb1a4b Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 4 Nov 2023 12:13:49 +0100 Subject: [PATCH 011/125] cc --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 44 +++++-------------- 1 file changed, 12 insertions(+), 32 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 49c47d91a0dcd..4dcb42db72c2d 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -181,6 +181,8 @@ BP_Mount_STorM32_MAVLink::BP_Mount_STorM32_MAVLink(AP_Mount &frontend, AP_Mount_ // STorM32-Link wants 25 Hz, so we update at 25 Hz and 12.5 Hz respectively void BP_Mount_STorM32_MAVLink::update() { + update_target_angles(); // update at 50 Hz + switch (_task_counter) { case TASK_SLOT0: case TASK_SLOT2: @@ -207,8 +209,6 @@ void BP_Mount_STorM32_MAVLink::update() _task_counter++; if (_task_counter > TASK_SLOT3) _task_counter = TASK_SLOT0; - update_target_angles(); // update at 50 Hz - if (!_initialised) { find_gimbal(); return; @@ -272,6 +272,14 @@ void BP_Mount_STorM32_MAVLink::send_target_angles(void) update_gimbal_device_flags(get_mode()); + if (mnt_target.target_type == MountTargetType::RATE) { + // we ignore it. We may think to just send angle_rad, but if yaw is earth frame + // this could result in pretty strange behavior. So better ignore. + // Should happen only in MAV_MOUNT_MODE_RC_TARGETING, so no need to test for this + // explicitly. + return; + } + if (_protocol == PROTOCOL_GIMBAL_DEVICE) { send_gimbal_device_set_attitude(); } else { @@ -575,15 +583,7 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_information(const mavlink_me mavlink_msg_gimbal_device_information_decode(&msg, &_device_info); - if (_device_info.gimbal_device_id != 0) { // something is wrong here, so reject - // some few STorM32 firmware versions have a bug in that they set this field to the gimbal's id - // so reject only if not a gimbal id - // shall be removed after some time - if (!(_device_info.gimbal_device_id == MAV_COMP_ID_GIMBAL) && - !(_device_info.gimbal_device_id >= MAV_COMP_ID_GIMBAL2 && _device_info.gimbal_device_id <= MAV_COMP_ID_GIMBAL6)) { - return; - } - } + // we could check here for sanity of _device_info.gimbal_device_id, but let's just be happy // set parameter defaults from gimbal information // Q: why default ?? why not actual value ?? I don't understand this @@ -620,15 +620,7 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_attitude_status(const mavlin mavlink_gimbal_device_attitude_status_t payload; mavlink_msg_gimbal_device_attitude_status_decode(&msg, &payload); - if (payload.gimbal_device_id != 0) { // something is wrong here, so reject - // some few STorM32 firmware versions have a bug in that they set this field to the gimbal's id - // so reject only if not a gimbal id - // shall be removed after some time - if (!(_device_info.gimbal_device_id == MAV_COMP_ID_GIMBAL) && - !(_device_info.gimbal_device_id >= MAV_COMP_ID_GIMBAL2 && _device_info.gimbal_device_id <= MAV_COMP_ID_GIMBAL6)) { - return; - } - } + // we could check here for sanity of _device_info.gimbal_device_id, but let's just be happy // get relevant data @@ -733,12 +725,6 @@ void BP_Mount_STorM32_MAVLink::send_cmd_do_mount_control(void) return; } - if (mnt_target.target_type == MountTargetType::RATE) { - // we ignore it. We may think to just send angle_rad, but if yaw is eath frame - // this oculd result in pretty strange behavior. So better ignore. - return; - } - // send command_long command containing a do_mount_control command // Note: pitch and yaw are reversed // ATTENTION: uses get_bf_yaw() to ensure body frame, which uses ahrs.yaw, not delta_yaw!!! @@ -764,12 +750,6 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude(void) return; } - if (mnt_target.target_type == MountTargetType::RATE) { - // we ignore it. We may think to just send angle_rad, but if yaw is eath frame - // this oculd result in pretty strange behavior. So better ignore. - return; - } - // convert Euler angles to quaternion float target_yaw_bf; if (mnt_target.angle_rad.yaw_is_ef) { From 8e2666eea252bb4758983206c3473ef6b4c1b08e Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 4 Nov 2023 12:14:04 +0100 Subject: [PATCH 012/125] rc channels --- libraries/RC_Channel/RC_Channel.cpp | 38 ++++++++++++++++++++++------- libraries/RC_Channel/RC_Channel.h | 20 +++++++++++---- 2 files changed, 44 insertions(+), 14 deletions(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index e0a82b7f12a7f..729645de97422 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -692,6 +692,9 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const AuxSwitchPo #if HAL_MOUNT_ENABLED case AUX_FUNC::RETRACT_MOUNT1: case AUX_FUNC::MOUNT_LOCK: +//OW + case AUX_FUNC::RETRACT_MOUNT1_3POS: +//OWEND #endif case AUX_FUNC::LOG_PAUSE: case AUX_FUNC::ARM_EMERGENCY_STOP: @@ -726,6 +729,9 @@ const RC_Channel::LookupTable RC_Channel::lookuptable[] = { { AUX_FUNC::PARACHUTE_3POS,"Parachute3Position"}, { AUX_FUNC::MISSION_RESET,"MissionReset"}, { AUX_FUNC::RETRACT_MOUNT1,"RetractMount1"}, +//OW +// { AUX_FUNC::RETRACT_MOUNT1_3POS,"RetractMount1 3pos"}, +//OWEND { AUX_FUNC::RELAY,"Relay1"}, { AUX_FUNC::MOTOR_ESTOP,"MotorEStop"}, { AUX_FUNC::MOTOR_INTERLOCK,"MotorInterlock"}, @@ -1526,21 +1532,12 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos if (mount == nullptr) { break; } -//OW -static enum MAV_MOUNT_MODE mode_last = MAV_MOUNT_MODE_RETRACT; -//OWEND switch (ch_flag) { case AuxSwitchPos::HIGH: -//OW - if (mount->get_mode(0) > MAV_MOUNT_MODE_NEUTRAL) mode_last = mount->get_mode(0); -//OWEND mount->set_mode(0,MAV_MOUNT_MODE_RETRACT); break; case AuxSwitchPos::MIDDLE: // nothing -//OW - if (mode_last > MAV_MOUNT_MODE_NEUTRAL) mount->set_mode(0, mode_last); -//OWEND break; case AuxSwitchPos::LOW: mount->set_mode_to_default(0); @@ -1557,6 +1554,29 @@ static enum MAV_MOUNT_MODE mode_last = MAV_MOUNT_MODE_RETRACT; mount->set_yaw_lock(ch_flag == AuxSwitchPos::HIGH); break; } + +//OW +// case AUX_FUNC::RETRACT_MOUNT1: + case AUX_FUNC::RETRACT_MOUNT1_3POS: { + AP_Mount *mount = AP::mount(); + if (mount == nullptr) { + break; + } + switch (ch_flag) { + case AuxSwitchPos::HIGH: + if (mount->get_mode(0) > MAV_MOUNT_MODE_NEUTRAL) _mount_mode_last = mount->get_mode(0); + mount->set_mode(0,MAV_MOUNT_MODE_RETRACT); + break; + case AuxSwitchPos::MIDDLE: + if (_mount_mode_last > MAV_MOUNT_MODE_NEUTRAL) mount->set_mode(0, _mount_mode_last); + break; + case AuxSwitchPos::LOW: + mount->set_mode_to_default(0); + break; + } + break; + } +//OWEND #endif case AUX_FUNC::LOG_PAUSE: { diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 46520602a1d03..28955cb10bd6a 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -6,6 +6,9 @@ #if AP_RC_CHANNEL_ENABLED +//OW +#include +//OWEND #include #include #include @@ -77,7 +80,7 @@ class RC_Channel { void set_override(const uint16_t v, const uint32_t timestamp_ms); bool has_override() const; - float stick_mixing(const float servo_in); + float stick_mixing(const float servo_in); // get control input with zero deadzone int16_t get_control_in_zero_dz(void) const; @@ -212,7 +215,7 @@ class RC_Channel { EKF_YAW_RESET = 104, // trigger yaw reset attempt GPS_DISABLE_YAW = 105, // disable GPS yaw for testing DISABLE_AIRSPEED_USE = 106, // equivalent to AIRSPEED_USE 0 - FW_AUTOTUNE = 107, // fixed wing auto tune + FW_AUTOTUNE = 107, // fixed wing auto tune QRTL = 108, // QRTL mode CUSTOM_CONTROLLER = 109, // use Custom Controller KILL_IMU3 = 110, // disable third IMU (for IMU failure testing) @@ -249,7 +252,9 @@ class RC_Channel { CAMERA_IMAGE_TRACKING = 174, // camera image tracking CAMERA_LENS = 175, // camera lens selection VFWD_THR_OVERRIDE = 176, // force enabled VTOL forward throttle method - +//OW + RETRACT_MOUNT1_3POS = 177, // Retract Mount1 3pos +//OWEND // inputs from 200 will eventually used to replace RCMAP ROLL = 201, // roll input @@ -422,6 +427,11 @@ class RC_Channel { static const LookupTable lookuptable[]; #endif +//OW +//#if HAL_MOUNT_ENABLED + enum MAV_MOUNT_MODE _mount_mode_last = MAV_MOUNT_MODE_RETRACT; +//#endif +//OWEND }; @@ -460,7 +470,7 @@ class RC_Channels { // this function is implemented in the child class in the vehicle // code virtual RC_Channel *channel(uint8_t chan) = 0; - // helper used by scripting to convert the above function from 0 to 1 indexeing + // helper used by scripting to convert the above function from 0 to 1 indexing // range is checked correctly by the underlying channel function RC_Channel *lua_rc_channel(const uint8_t chan) { return channel(chan -1); @@ -471,7 +481,7 @@ class RC_Channels { static uint8_t get_valid_channel_count(void); // returns the number of valid channels in the last read static int16_t get_receiver_rssi(void); // returns [0, 255] for receiver RSSI (0 is no link) if present, otherwise -1 - static int16_t get_receiver_link_quality(void); // returns 0-100 % of last 100 packets received at receiver are valid + static int16_t get_receiver_link_quality(void); // returns 0-100 % of last 100 packets received at receiver are valid bool read_input(void); // returns true if new input has been read in static void clear_overrides(void); // clears any active overrides static bool receiver_bind(const int dsmMode); // puts the receiver in bind mode if present, returns true if success From 8a8b37dec3e7cfe5cb92b3705192787873412492 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 4 Nov 2023 15:02:49 +0100 Subject: [PATCH 013/125] camera --- libraries/AP_Camera/AP_Camera.cpp | 58 ++++++++++++------- libraries/AP_Camera/AP_Camera.h | 5 +- libraries/AP_Camera/AP_Camera_Backend.h | 8 +++ libraries/AP_Camera/AP_Camera_Mount.cpp | 22 +++++++ libraries/AP_Camera/AP_Camera_Mount.h | 8 +++ libraries/AP_Mount/AP_Mount.cpp | 4 +- libraries/AP_Mount/AP_Mount.h | 2 +- libraries/AP_Mount/AP_Mount_Backend.h | 2 +- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 6 +- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 3 +- 10 files changed, 87 insertions(+), 31 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index d6607e4666d9a..1f1135fe7ab8a 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -758,32 +758,48 @@ AP_Camera *camera() //OW bool AP_Camera::set_cam_mode(bool video_mode) { -/* XX ?? -#if HAL_MOUNT_ENABLED - if (get_trigger_type() == CamTrigType::mount) { - AP_Mount* mount = AP::mount(); - if (mount != nullptr) { - return mount->set_cam_mode(0, video_mode); - } + WITH_SEMAPHORE(_rsem); + + if (primary == nullptr) { + return false; } -#endif -*/ - return false; + return primary->set_cam_mode(video_mode); } -bool AP_Camera::set_cam_photo_video_mode(int8_t sw_flag) +bool AP_Camera::set_cam_mode(uint8_t instance, bool video_mode) { -/* XX ?? -#if HAL_MOUNT_ENABLED - if (get_trigger_type() == CamTrigType::mount) { - AP_Mount* mount = AP::mount(); - if (mount != nullptr) { - return mount->set_cam_photo_video_mode(0, sw_flag); - } + WITH_SEMAPHORE(_rsem); + + auto *backend = get_instance(instance); + if (backend == nullptr) { + return false; } -#endif -*/ - return false; + + // call backend + return backend->set_cam_mode(video_mode); +} + +bool AP_Camera::set_cam_photo_video_mode(int8_t ch_flag) +{ + WITH_SEMAPHORE(_rsem); + + if (primary == nullptr) { + return false; + } + return primary->set_cam_photo_video_mode(ch_flag); +} + +bool AP_Camera::set_cam_photo_video_mode(uint8_t instance, int8_t ch_flag) +{ + WITH_SEMAPHORE(_rsem); + + auto *backend = get_instance(instance); + if (backend == nullptr) { + return false; + } + + // call backend + return backend->set_cam_photo_video_mode(ch_flag); } //OWEND diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 6fbb11fd2a35f..a0275dbf3a0f1 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -157,7 +157,10 @@ class AP_Camera { //OW bool set_cam_mode(bool video_mode); - bool set_cam_photo_video_mode(int8_t sw_flag); + bool set_cam_mode(uint8_t instance, bool video_mode); + + bool set_cam_photo_video_mode(int8_t ch_flag); + bool set_cam_photo_video_mode(uint8_t instance, int8_t ch_flag); //OWEND #if AP_CAMERA_SCRIPTING_ENABLED diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/libraries/AP_Camera/AP_Camera_Backend.h index 5f8f8b1d78fb9..29cedf164ff02 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/libraries/AP_Camera/AP_Camera_Backend.h @@ -53,6 +53,14 @@ class AP_Camera_Backend // momentary switch to change camera between picture and video modes virtual void cam_mode_toggle() {} +//OW + // momentary switch to directly set camera to picture or video mode + virtual bool set_cam_mode(bool video_mode) { return false; } + + // momentary switch to set camera to picture and video mode and start/stop recording using a 3 pos switch + virtual bool set_cam_photo_video_mode(int8_t ch_flag) { return false; } +//OWEND + // take a picture. returns true on success bool take_picture(); diff --git a/libraries/AP_Camera/AP_Camera_Mount.cpp b/libraries/AP_Camera/AP_Camera_Mount.cpp index e81327fb58201..b79ef37bda32f 100644 --- a/libraries/AP_Camera/AP_Camera_Mount.cpp +++ b/libraries/AP_Camera/AP_Camera_Mount.cpp @@ -5,6 +5,28 @@ extern const AP_HAL::HAL& hal; +//OW +// momentary switch to directly set camera to picture or video mode +bool AP_Camera_Mount::set_cam_mode(bool video_mode) +{ + AP_Mount* mount = AP::mount(); + if (mount != nullptr) { + return mount->set_cam_mode(get_mount_instance(), video_mode); + } + return false; +} + +// momentary switch to set camera to picture and video mode and start/stop recording using a 3 pos switch +bool AP_Camera_Mount::set_cam_photo_video_mode(int8_t ch_flag) +{ + AP_Mount* mount = AP::mount(); + if (mount != nullptr) { + return mount->set_cam_photo_video_mode(get_mount_instance(), ch_flag); + } + return false; +} +//OWEND + // entry point to actually take a picture. returns true on success bool AP_Camera_Mount::trigger_pic() { diff --git a/libraries/AP_Camera/AP_Camera_Mount.h b/libraries/AP_Camera/AP_Camera_Mount.h index fa53057295b8c..b98528d0f520d 100644 --- a/libraries/AP_Camera/AP_Camera_Mount.h +++ b/libraries/AP_Camera/AP_Camera_Mount.h @@ -32,6 +32,14 @@ class AP_Camera_Mount : public AP_Camera_Backend /* Do not allow copies */ CLASS_NO_COPY(AP_Camera_Mount); +//OW + // momentary switch to directly set camera to picture or video mode + bool set_cam_mode(bool video_mode) override; + + // momentary switch to set camera to picture and video mode and start/stop recording using a 3 pos switch + bool set_cam_photo_video_mode(int8_t ch_flag) override; +//OWEND + // entry point to actually take a picture. returns true on success bool trigger_pic() override; diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index f15c6ff7e3c18..ca37ea70defc1 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -1092,13 +1092,13 @@ bool AP_Mount::set_cam_mode(uint8_t instance, bool video_mode) return backend->set_cam_mode(video_mode); } -bool AP_Mount::set_cam_photo_video_mode(uint8_t instance, int8_t sw_flag) +bool AP_Mount::set_cam_photo_video_mode(uint8_t instance, int8_t ch_flag) { auto *backend = get_instance(instance); if (backend == nullptr) { return false; } - return backend->set_cam_photo_video_mode(sw_flag); + return backend->set_cam_photo_video_mode(ch_flag); } void AP_Mount::handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 970e5ada505ca..9e29a3a9963a0 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -275,7 +275,7 @@ class AP_Mount bool set_cam_mode(uint8_t instance, bool video_mode); // 3-way switch mode - bool set_cam_photo_video_mode(uint8_t instance, int8_t sw_flag); + bool set_cam_photo_video_mode(uint8_t instance, int8_t ch_flag); // this is somewhat different to handle_message() in that it catches all messages // with significant work it potentially could be combined, but let's not introduce side effects diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 408c24f0d48bb..38f4ce3e3d9ca 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -196,7 +196,7 @@ class AP_Mount_Backend virtual bool set_cam_mode(bool video_mode) { return false; } // 3-way switch mode - virtual bool set_cam_photo_video_mode(int8_t sw_flag) { return false; } + virtual bool set_cam_photo_video_mode(int8_t ch_flag) { return false; } // handle msg - allows to process a msg from a gimbal virtual void handle_msg(const mavlink_message_t &msg) {} diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 4dcb42db72c2d..481ad74be4f2f 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -1199,18 +1199,18 @@ bool BP_Mount_STorM32_MAVLink::set_cam_mode(bool video_mode) } -bool BP_Mount_STorM32_MAVLink::set_cam_photo_video_mode(int8_t sw_flag) +bool BP_Mount_STorM32_MAVLink::set_cam_photo_video_mode(int8_t ch_flag) { if (!_use_3way_photo_video) return false; - if (sw_flag > 0) { + if (ch_flag > 0) { if (_camera_mode != CAMERA_MODE_VIDEO) { _camera_mode = CAMERA_MODE_VIDEO; send_cmd_do_digicam_configure(true); } send_cmd_do_digicam_control(true); } else - if (sw_flag < 0) { + if (ch_flag < 0) { if (_camera_mode != CAMERA_MODE_PHOTO) { _camera_mode = CAMERA_MODE_PHOTO; send_cmd_do_digicam_configure(false); diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index fd529134a587c..dd620d0cc256b 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -148,8 +148,7 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend bool set_cam_mode(bool video_mode) override; // added: 3-way switch mode - bool set_cam_photo_video_mode(int8_t sw_flag) override; - + bool set_cam_photo_video_mode(int8_t ch_flag) override; private: From 654342ae10a193553514646254cb287a06f8528a Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 4 Nov 2023 16:17:39 +0100 Subject: [PATCH 014/125] camera rc switch --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 11 +--- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 2 - libraries/RC_Channel/RC_Channel.cpp | 52 +++++++++++++++---- libraries/RC_Channel/RC_Channel.h | 2 + 4 files changed, 44 insertions(+), 23 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 481ad74be4f2f..e3b9ca3cf6548 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -170,7 +170,6 @@ BP_Mount_STorM32_MAVLink::BP_Mount_STorM32_MAVLink(AP_Mount &frontend, AP_Mount_ _yaw_lock = false; // can't be currently supported, so we need to ensure this is false. This is important! _got_radio_rc_channels = false; // disable sending rc channels when RADIO_RC_CHANNELS messages are detected - _use_3way_photo_video = true; _camera_mode = CAMERA_MODE_UNDEFINED; } @@ -1104,7 +1103,7 @@ uint32_t BP_Mount_STorM32_MAVLink::get_gimbal_manager_flags() const //------------------------------------------------------ -// Scripting accescors, and get attitude fakery +// Scripting accessors, and get attitude fakery //------------------------------------------------------ // return target location if available @@ -1150,8 +1149,6 @@ bool BP_Mount_STorM32_MAVLink::get_attitude_quaternion(Quaternion& att_quat) bool BP_Mount_STorM32_MAVLink::take_picture() { - if (_use_3way_photo_video) return false; - if (_camera_mode == CAMERA_MODE_UNDEFINED) { _camera_mode = CAMERA_MODE_PHOTO; send_cmd_do_digicam_configure(false); @@ -1169,8 +1166,6 @@ bool BP_Mount_STorM32_MAVLink::take_picture() bool BP_Mount_STorM32_MAVLink::record_video(bool start_recording) { - if (_use_3way_photo_video) return false; - if (_camera_mode == CAMERA_MODE_UNDEFINED) { _camera_mode = CAMERA_MODE_VIDEO; send_cmd_do_digicam_configure(true); @@ -1188,8 +1183,6 @@ bool BP_Mount_STorM32_MAVLink::record_video(bool start_recording) bool BP_Mount_STorM32_MAVLink::set_cam_mode(bool video_mode) { - if (_use_3way_photo_video) return false; - _camera_mode = (video_mode) ? CAMERA_MODE_VIDEO : CAMERA_MODE_PHOTO; send_cmd_do_digicam_configure(video_mode); @@ -1201,8 +1194,6 @@ bool BP_Mount_STorM32_MAVLink::set_cam_mode(bool video_mode) bool BP_Mount_STorM32_MAVLink::set_cam_photo_video_mode(int8_t ch_flag) { - if (!_use_3way_photo_video) return false; - if (ch_flag > 0) { if (_camera_mode != CAMERA_MODE_VIDEO) { _camera_mode = CAMERA_MODE_VIDEO; diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index dd620d0cc256b..ad65ae03ea8e1 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -267,8 +267,6 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend }; CameraMode _camera_mode; // current camera mode - bool _use_3way_photo_video; - void send_cmd_do_digicam_configure(bool video_mode); void send_cmd_do_digicam_control(bool shoot); diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 729645de97422..08f3b3cfe7837 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -703,6 +703,10 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const AuxSwitchPo case AUX_FUNC::CAMERA_MANUAL_FOCUS: case AUX_FUNC::CAMERA_AUTO_FOCUS: case AUX_FUNC::CAMERA_LENS: +//OW + case AUX_FUNC::CAMERA_SET_MODE: + case AUX_FUNC::CAMERA_TRIG_MODE: +//OWEND run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT); break; default: @@ -1479,7 +1483,7 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos case AUX_FUNC::CAMERA_TRIGGER: do_aux_function_camera_trigger(ch_flag); break; - +/* case AUX_FUNC::CAM_MODE_TOGGLE: { // Momentary switch to for cycling camera modes AP_Camera *camera = AP_Camera::get_singleton(); @@ -1497,16 +1501,9 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos camera->cam_mode_toggle(); break; } -//OW - switch (ch_flag) { - case AuxSwitchPos::HIGH: camera->set_cam_photo_video_mode(1); break; - case AuxSwitchPos::MIDDLE: camera->set_cam_photo_video_mode(0); break; - case AuxSwitchPos::LOW: camera->set_cam_photo_video_mode(-1); break; - } - camera->set_cam_mode(ch_flag == AuxSwitchPos::HIGH); -//OWEND break; } +*/ case AUX_FUNC::CAMERA_REC_VIDEO: return do_aux_function_record_video(ch_flag); @@ -1524,9 +1521,42 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos case AUX_FUNC::CAMERA_LENS: return do_aux_function_camera_lens(ch_flag); + +//OW +// case AUX_FUNC::CAM_MODE_TOGGLE: { + case AUX_FUNC::CAMERA_SET_MODE: { + AP_Camera *camera = AP_Camera::get_singleton(); + if (camera == nullptr) { + break; + } + camera->set_cam_mode(ch_flag == AuxSwitchPos::HIGH); + break; + } + + case AUX_FUNC::CAM_MODE_TOGGLE: { +// case AUX_FUNC::CAMERA_TRIG_MODE: { + AP_Camera *camera = AP_Camera::get_singleton(); + if (camera == nullptr) { + break; + } + switch (ch_flag) { + case AuxSwitchPos::HIGH: + camera->set_cam_photo_video_mode(1); + break; + case AuxSwitchPos::MIDDLE: + camera->set_cam_photo_video_mode(0); + break; + case AuxSwitchPos::LOW: + camera->set_cam_photo_video_mode(-1); + break; + } + break; + } + //OWEND #endif #if HAL_MOUNT_ENABLED +/* case AUX_FUNC::RETRACT_MOUNT1: { AP_Mount *mount = AP::mount(); if (mount == nullptr) { @@ -1544,7 +1574,7 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos break; } break; - } + } */ case AUX_FUNC::MOUNT_LOCK: { AP_Mount *mount = AP::mount(); @@ -1556,7 +1586,7 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos } //OW -// case AUX_FUNC::RETRACT_MOUNT1: + case AUX_FUNC::RETRACT_MOUNT1: case AUX_FUNC::RETRACT_MOUNT1_3POS: { AP_Mount *mount = AP::mount(); if (mount == nullptr) { diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 28955cb10bd6a..2ff196b8be56d 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -254,6 +254,8 @@ class RC_Channel { VFWD_THR_OVERRIDE = 176, // force enabled VTOL forward throttle method //OW RETRACT_MOUNT1_3POS = 177, // Retract Mount1 3pos + CAMERA_SET_MODE = 178, // set camera mode, high = video mode, low = picture mode + CAMERA_TRIG_MODE = 179, // set camera mode and trigger/start/stop, high = start video, mid = stop video, low = take picture //OWEND // inputs from 200 will eventually used to replace RCMAP From ab0e7b6550221f6d41992940297f18d9d496975f Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 4 Nov 2023 17:38:22 +0100 Subject: [PATCH 015/125] constructor & init --- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 5 +++-- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index e3b9ca3cf6548..d4c30e5b112e4 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -143,9 +143,10 @@ enum STORM32STATEENUM { // BP_Mount_STorM32_MAVLink, main class //****************************************************** -BP_Mount_STorM32_MAVLink::BP_Mount_STorM32_MAVLink(AP_Mount &frontend, AP_Mount_Params ¶ms, uint8_t instance) : - AP_Mount_Backend(frontend, params, instance) +void BP_Mount_STorM32_MAVLink::init() { + AP_Mount_Backend::init(); + _sysid = 0; _compid = 0; // gimbal not yet discovered _chan = MAVLINK_COMM_0; // this is a dummy, will be set correctly by find_gimbal() diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index ad65ae03ea8e1..83b974a9f9444 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -14,10 +14,10 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend public: // Constructor - BP_Mount_STorM32_MAVLink(AP_Mount &frontend, AP_Mount_Params ¶ms, uint8_t instance); + using AP_Mount_Backend::AP_Mount_Backend; // init - performs any required initialisation for this instance - void init() override { AP_Mount_Backend::init(); } + void init() override; // update mount position - should be called periodically at 50 Hz void update() override; From 31269cac4a69c2d52ce4e42ae97749435ec5e950 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sun, 5 Nov 2023 00:17:08 +0100 Subject: [PATCH 016/125] straighten pream & healthy --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 125 +++++++++++------- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 12 +- 2 files changed, 83 insertions(+), 54 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index d4c30e5b112e4..ed6d4a1f88618 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -153,16 +153,17 @@ void BP_Mount_STorM32_MAVLink::init() _got_device_info = false; _armed = false; - _prearmchecks_ok = false; _initialised = false; - _mode = MAV_MOUNT_MODE_RC_TARGETING; - _protocol = PROTOCOL_UNDEFINED; + _gimbal_prearmchecks_ok = false; _armingchecks_enabled = false; _prearmchecks_done = false; + _mode = MAV_MOUNT_MODE_RC_TARGETING; + + _mount_status = {}; _device_status = {}; flags_for_gimbal = 0; _current_angles = { 0.0f, 0.0f, 0.0f, NAN}; // the NAN is important! @@ -490,21 +491,15 @@ const uint32_t FAILURE_FLAGS = // GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER; -bool BP_Mount_STorM32_MAVLink::prearmchecks_all_ok(void) +void BP_Mount_STorM32_MAVLink::send_prearmchecks_txt(void) { - return (_armingchecks_enabled && // healthy() called at least once - _prearmchecks_ok && // HEARTBEAT reported ok - _armed && // gimbal is in NORMAL state - ((_device_status.received_failure_flags & FAILURE_FLAGS) == 0));// gimbal has no errors -} +GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: i%u p%u a%u pr%u", _instance+1, _initialised, _gimbal_prearmchecks_ok, _armed, _protocol); -void BP_Mount_STorM32_MAVLink::send_prearmchecks_txt(void) -{ - if (!_initialised || !_prearmchecks_ok || !_armed) { + if (!_initialised || !_gimbal_prearmchecks_ok || !_armed) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: arm", _instance+1); } else - if (_prearmchecks_ok && (_device_status.received_failure_flags & FAILURE_FLAGS)) { + if (_device_status.received_failure_flags & FAILURE_FLAGS) { char txt[255]; strcpy(txt, ""); if (_device_status.received_failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR) strcat(txt, "mot,"); @@ -514,7 +509,7 @@ void BP_Mount_STorM32_MAVLink::send_prearmchecks_txt(void) txt[strlen(txt)-1] = '\0'; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: %s", _instance+1, txt); } else { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAILURE FLAGS", _instance+1); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: err flags", _instance+1); } } } @@ -522,50 +517,77 @@ void BP_Mount_STorM32_MAVLink::send_prearmchecks_txt(void) // return true if healthy // this is called when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA is set, else not -// workaround to that healthy() is const +// is called with 1 Hz +// workaround to get around that healthy() is const bool BP_Mount_STorM32_MAVLink::healthy() const { - return const_cast(this)->prearmchecks_do(); // yes, ugly, but I have not overdesigned the backend + return const_cast(this)->healthy_nonconst(); // yes, ugly, but I haven't overdesigned the backend +} + + +bool BP_Mount_STorM32_MAVLink::healthy_nonconst(void) +{ + _armingchecks_enabled = true; // to signal that ArduPilot arming check mechanism is active + + return prearmchecks_do(); } bool BP_Mount_STorM32_MAVLink::prearmchecks_do(void) { - _armingchecks_enabled = true; + // these we do only at startup + if (!_prearmchecks_done) { - if (prearmchecks_all_ok() && !_prearmchecks_done) { // has just changed - _prearmchecks_done = true; - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); - } else - if ((AP_HAL::millis() - _prearmcheck_sendtext_tlast_ms) > 30000) { // we haven't send it for a long time - _prearmcheck_sendtext_tlast_ms = AP_HAL::millis(); - send_prearmchecks_txt(); - } + if (_armingchecks_enabled) { // do only if ArduPilot check is enabled + if((AP_HAL::millis() - _prearmcheck_sendtext_tlast_ms) > 30000) { // we haven't send it for a long time + _prearmcheck_sendtext_tlast_ms = AP_HAL::millis(); + send_prearmchecks_txt(); + } + } - // unhealthy until gimbal has fully passed the startup sequence - // implies gimbal has been found and replied with device info, i.e. _compid != 0 and _got_device_info = true - if (!_initialised || !_prearmchecks_ok || !_armed) { - return false; + // unhealthy until gimbal has fully passed the startup sequence + // implies gimbal has been found, has replied with device info, and status message was received + // _initialised: -> gimbal found (HB received,_compid != 0) + // -> device info obtained (_got_device_info = true) + // -> status message received, protocol set (_protocol != PROTOCOL_UNDEFINED) + // _gimbal_prearmchecks_ok: -> gimbal HB reported gimbal's prearmchecks ok + // _armed: -> gimbal HB reported gimbal is in normal state + if (!_initialised || !_gimbal_prearmchecks_ok || !_armed) { + return false; + } } - // if we get this far and mount is not gimbal device then mount is healthy - if (_protocol == PROTOCOL_MOUNT) { - return true; - } + // these we do continuously - // unhealthy if attitude status is NOT received within the last second - if ((AP_HAL::millis() - _device_status.received_tlast_ms) > 1000) { - return false; - } + if (_protocol == PROTOCOL_GIMBAL_DEVICE) { - // check failure flags - // We also check for GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER - // which essentially only means that gimbal got GIMBAL_DEVICE_SET_ATTITUDE messages. - if ((_device_status.received_failure_flags & FAILURE_FLAGS) > 0) { - return false; + // unhealthy if attitude status is not received within the last second + if ((AP_HAL::millis() - _device_status.received_tlast_ms) > 1000) { + return false; + } + + // check failure flags + // We also check for GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER + // which essentially only means that gimbal got GIMBAL_DEVICE_SET_ATTITUDE messages. + if ((_device_status.received_failure_flags & FAILURE_FLAGS) > 0) { + return false; + } + + } else { + // unhealthy if mount status is not received within the last second + if ((AP_HAL::millis() - _mount_status.received_tlast_ms) > 1000) { + return false; + } } // if we get this far the mount is healthy + + // if we got this far the first time we inform the gcs + if (!_prearmchecks_done) { + _prearmchecks_done = true; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); + } + return true; } @@ -687,13 +709,16 @@ void BP_Mount_STorM32_MAVLink::handle_msg(const mavlink_message_t &msg) uint8_t storm32_state = (payload.custom_mode & 0xFF); _armed = ((storm32_state == STORM32STATE_NORMAL) || (storm32_state == STORM32STATE_STARTUP_FASTLEVEL)); if ((payload.custom_mode & 0x80000000) == 0) { // we don't follow all changes, but just toggle it to true once - _prearmchecks_ok = true; + _gimbal_prearmchecks_ok = true; } - if (prearmchecks_all_ok()) { - _prearmchecks_done = true; - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); + if (!_armingchecks_enabled) { // ArduPilot arming checks disabled, so let's do ourself + if (!_prearmchecks_done) prearmchecks_do(); } - }break; + break; } + + case MAVLINK_MSG_ID_MOUNT_STATUS: + _mount_status.received_tlast_ms = AP_HAL::millis(); + break; } } @@ -1028,20 +1053,20 @@ void BP_Mount_STorM32_MAVLink::send_banner(void) GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: %s %s v%u.%u%c", _instance + 1, - _device_info.vendor_name, + "", //_device_info.vendor_name, _device_info.model_name, (unsigned)(_device_info.firmware_version & 0x000000FF), (unsigned)((_device_info.firmware_version & 0x0000FF00) >> 8), c ); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks %s", _instance+1, (prearmchecks_all_ok()) ? "passed" : "fail"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks %s", _instance+1, (_prearmchecks_done) ? "passed" : "fail"); } else if (_compid) { // we have some info GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: gimbal at %u", _instance+1, _compid); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks %s", _instance+1, (prearmchecks_all_ok()) ? "passed" : "fail"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks %s", _instance+1, (_prearmchecks_done) ? "passed" : "fail"); } else { // we don't know yet anything diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 83b974a9f9444..1c94c0d558263 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -165,7 +165,6 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend bool _got_device_info; // gimbal discovered, waiting for gimbal provide device info bool _armed; // true once the gimbal has reached normal state - bool _prearmchecks_ok; // true when the gimbal stops reporting prearm fail in the HEARTBEAT message bool _initialised; // true once all init steps have been passed bool _got_radio_rc_channels; // true when a RADIO_RC_CHANNELS message has been received @@ -190,18 +189,23 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // determine protocol based on receiving MOUNT_STATUS or GIMBAL_DEVICE_ATTITUDE_STATUS from gimbal void determine_protocol(const mavlink_message_t &msg); - // pre-arm checks + // pre-arm and healthy checks + bool _gimbal_prearmchecks_ok; // true when the gimbal stops reporting prearm fail in the HEARTBEAT message bool _armingchecks_enabled; // true when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA set, we know from healty() - bool _prearmchecks_done; // becomes true when info has send out + bool _prearmchecks_done; // becomes true when all checks were passed once at startup uint32_t _prearmcheck_sendtext_tlast_ms; - bool prearmchecks_all_ok(void); + bool healthy_nonconst(void); // workaround to healthy() being const bool prearmchecks_do(void); void send_prearmchecks_txt(void); // gimbal target & control + struct { + uint32_t received_tlast_ms; // time last MOUNT_STATUS was received (used for health reporting) + } _mount_status; + struct { uint16_t received_flags; // obtained from GIMBAL_DEVICE_ATTITUDE_STATUS uint32_t received_failure_flags; // obtained from GIMBAL_DEVICE_ATTITUDE_STATUS From 6b5fa988e4375a6fc34f747845e08badf41972db Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sun, 5 Nov 2023 01:11:51 +0100 Subject: [PATCH 017/125] arming, speed up response when it goes from ok to bad --- libraries/AP_Arming/AP_Arming.cpp | 22 ++++++++++++++++--- libraries/AP_Arming/AP_Arming.h | 5 +++++ .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 3 --- 3 files changed, 24 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index fef0bff6abad9..99948ca27db39 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -183,7 +183,12 @@ void AP_Arming::update(void) const uint32_t now_ms = AP_HAL::millis(); // perform pre-arm checks & display failures every 30 seconds bool display_fail = false; - if (now_ms - last_prearm_display_ms > PREARM_DISPLAY_PERIOD*1000) { +//OW +// if (now_ms - last_prearm_display_ms > PREARM_DISPLAY_PERIOD*1000) { + if ((report_immediately && (now_ms - last_prearm_display_ms > 750)) || + (now_ms - last_prearm_display_ms > PREARM_DISPLAY_PERIOD*1000)) { + report_immediately = false; +//OWEND display_fail = true; last_prearm_display_ms = now_ms; } @@ -1539,8 +1544,10 @@ bool AP_Arming::pre_arm_checks(bool report) return true; } #endif - - return hardware_safety_check(report) +//OW +// return hardware_safety_check(report) + bool res = hardware_safety_check(report) +//OWEND #if HAL_HAVE_IMU_HEATER & heater_min_temperature_checks(report) #endif @@ -1575,6 +1582,15 @@ bool AP_Arming::pre_arm_checks(bool report) & opendroneid_checks(report) & serial_protocol_checks(report) & estop_checks(report); + +//OW + if (!res && pre_arm_checks_last_result) { // check went from true to false + report_immediately = true; + } + pre_arm_checks_last_result = res; + + return res; +//OWEND } bool AP_Arming::arm_checks(AP_Arming::Method method) diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index a755bc0bd04d4..b16b9fbdcffd0 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -303,6 +303,11 @@ class AP_Arming { uint32_t last_prearm_display_ms; // last time we send statustexts for prearm failures bool running_arming_checks; // true if the arming checks currently being performed are being done because the vehicle is trying to arm the vehicle + +//OW + bool pre_arm_checks_last_result; + bool report_immediately; +//OWEND }; namespace AP { diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index ed6d4a1f88618..f652a2a9928c7 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -493,9 +493,6 @@ const uint32_t FAILURE_FLAGS = void BP_Mount_STorM32_MAVLink::send_prearmchecks_txt(void) { -GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: i%u p%u a%u pr%u", _instance+1, _initialised, _gimbal_prearmchecks_ok, _armed, _protocol); - - if (!_initialised || !_gimbal_prearmchecks_ok || !_armed) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: arm", _instance+1); } else From 491d2a3f95c95f3545339edcf015f5faffc36530 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sun, 5 Nov 2023 09:43:12 +0100 Subject: [PATCH 018/125] send mount status to gcs also in mount mode, MP wants this --- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index f652a2a9928c7..2b4454c9bf03b 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -161,7 +161,7 @@ void BP_Mount_STorM32_MAVLink::init() _armingchecks_enabled = false; _prearmchecks_done = false; - _mode = MAV_MOUNT_MODE_RC_TARGETING; + _mode = MAV_MOUNT_MODE_RC_TARGETING; // irrelevant, will be later set to default in frontend init() _mount_status = {}; _device_status = {}; @@ -713,9 +713,17 @@ void BP_Mount_STorM32_MAVLink::handle_msg(const mavlink_message_t &msg) } break; } - case MAVLINK_MSG_ID_MOUNT_STATUS: - _mount_status.received_tlast_ms = AP_HAL::millis(); - break; + case MAVLINK_MSG_ID_MOUNT_STATUS: { + _mount_status.received_tlast_ms = AP_HAL::millis(); // for health reporting + + mavlink_mount_status_t payload; + mavlink_msg_mount_status_decode(&msg, &payload); + _current_angles.pitch = radians((float)payload.pointing_a * 0.01f); + _current_angles.roll = radians((float)payload.pointing_b * 0.01f); + _current_angles.yaw_bf = radians((float)payload.pointing_c * 0.01f); + _current_angles.delta_yaw = NAN; + send_mount_status_to_ground(); // this is what MissionPlanner wants ... + break; } } } From 56dd4c7412518be5f911db94f2faa60bc31b8c97 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sun, 5 Nov 2023 21:08:32 +0100 Subject: [PATCH 019/125] revise camera --- libraries/AP_Camera/AP_Camera.cpp | 16 ++++++++-------- libraries/AP_Camera/AP_Camera.h | 18 ++++++++++-------- libraries/AP_Camera/AP_Camera_Backend.h | 8 ++++---- libraries/AP_Camera/AP_Camera_Mount.cpp | 12 ++++++------ libraries/AP_Camera/AP_Camera_Mount.h | 8 ++++---- libraries/AP_Camera/AP_Camera_shareddefs.h | 8 ++++++++ libraries/AP_Mount/AP_Mount.cpp | 8 ++++---- libraries/AP_Mount/AP_Mount.h | 8 ++++---- libraries/AP_Mount/AP_Mount_Backend.h | 8 ++++---- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 8 ++++---- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 11 +++++++---- libraries/RC_Channel/RC_Channel.cpp | 10 +++++----- 12 files changed, 68 insertions(+), 55 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 1f1135fe7ab8a..2a6f1702b737e 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -756,17 +756,17 @@ AP_Camera *camera() } //OW -bool AP_Camera::set_cam_mode(bool video_mode) +bool AP_Camera::cam_set_mode(bool video_mode) { WITH_SEMAPHORE(_rsem); if (primary == nullptr) { return false; } - return primary->set_cam_mode(video_mode); + return primary->cam_set_mode(video_mode); } -bool AP_Camera::set_cam_mode(uint8_t instance, bool video_mode) +bool AP_Camera::cam_set_mode(uint8_t instance, bool video_mode) { WITH_SEMAPHORE(_rsem); @@ -776,20 +776,20 @@ bool AP_Camera::set_cam_mode(uint8_t instance, bool video_mode) } // call backend - return backend->set_cam_mode(video_mode); + return backend->cam_set_mode(video_mode); } -bool AP_Camera::set_cam_photo_video_mode(int8_t ch_flag) +bool AP_Camera::cam_do_photo_video_mode(PhotoVideoMode photo_video_mode) { WITH_SEMAPHORE(_rsem); if (primary == nullptr) { return false; } - return primary->set_cam_photo_video_mode(ch_flag); + return primary->cam_do_photo_video_mode(photo_video_mode); } -bool AP_Camera::set_cam_photo_video_mode(uint8_t instance, int8_t ch_flag) +bool AP_Camera::cam_do_photo_video_mode(uint8_t instance, PhotoVideoMode photo_video_mode) { WITH_SEMAPHORE(_rsem); @@ -799,7 +799,7 @@ bool AP_Camera::set_cam_photo_video_mode(uint8_t instance, int8_t ch_flag) } // call backend - return backend->set_cam_photo_video_mode(ch_flag); + return backend->cam_do_photo_video_mode(photo_video_mode); } //OWEND diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index a0275dbf3a0f1..e7c6e13091ccb 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -111,6 +111,16 @@ class AP_Camera { void cam_mode_toggle(); void cam_mode_toggle(uint8_t instance); +//OW + // momentary switch to set to photo or video mode (video_mode false: photo mode, true: video mode) + bool cam_set_mode(bool video_mode); + bool cam_set_mode(uint8_t instance, bool video_mode); + + // momentary 3 pos switch to set to photo mode and take picture, set to video mode and start recording, or stop video recording + bool cam_do_photo_video_mode(PhotoVideoMode photo_video_mode); + bool cam_do_photo_video_mode(uint8_t instance, PhotoVideoMode photo_video_mode); +//OWEND + // take a picture. If instance is not provided, all available cameras affected // returns true if at least one camera took a picture bool take_picture(); @@ -155,14 +165,6 @@ class AP_Camera { // set if vehicle is in AUTO mode void set_is_auto_mode(bool enable) { _is_in_auto_mode = enable; } -//OW - bool set_cam_mode(bool video_mode); - bool set_cam_mode(uint8_t instance, bool video_mode); - - bool set_cam_photo_video_mode(int8_t ch_flag); - bool set_cam_photo_video_mode(uint8_t instance, int8_t ch_flag); -//OWEND - #if AP_CAMERA_SCRIPTING_ENABLED // structure and accessors for use by scripting backends typedef struct { diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/libraries/AP_Camera/AP_Camera_Backend.h index 29cedf164ff02..272f30c69a1e3 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/libraries/AP_Camera/AP_Camera_Backend.h @@ -54,11 +54,11 @@ class AP_Camera_Backend virtual void cam_mode_toggle() {} //OW - // momentary switch to directly set camera to picture or video mode - virtual bool set_cam_mode(bool video_mode) { return false; } + // momentary switch to set to photo or video mode (video_mode false: photo mode, true: video mode) + virtual bool cam_set_mode(bool video_mode) { return false; } - // momentary switch to set camera to picture and video mode and start/stop recording using a 3 pos switch - virtual bool set_cam_photo_video_mode(int8_t ch_flag) { return false; } + // momentary 3 pos switch to set to photo mode and take picture, set to video mode and start recording, or stop video recording + virtual bool cam_do_photo_video_mode(PhotoVideoMode photo_video_mode) { return false; } //OWEND // take a picture. returns true on success diff --git a/libraries/AP_Camera/AP_Camera_Mount.cpp b/libraries/AP_Camera/AP_Camera_Mount.cpp index b79ef37bda32f..d52dc19fb89d5 100644 --- a/libraries/AP_Camera/AP_Camera_Mount.cpp +++ b/libraries/AP_Camera/AP_Camera_Mount.cpp @@ -6,22 +6,22 @@ extern const AP_HAL::HAL& hal; //OW -// momentary switch to directly set camera to picture or video mode -bool AP_Camera_Mount::set_cam_mode(bool video_mode) +// momentary switch to set to photo or video mode (video_mode false: photo mode, true: video mode) +bool AP_Camera_Mount::cam_set_mode(bool video_mode) { AP_Mount* mount = AP::mount(); if (mount != nullptr) { - return mount->set_cam_mode(get_mount_instance(), video_mode); + return mount->cam_set_mode(get_mount_instance(), video_mode); } return false; } -// momentary switch to set camera to picture and video mode and start/stop recording using a 3 pos switch -bool AP_Camera_Mount::set_cam_photo_video_mode(int8_t ch_flag) +// momentary 3 pos switch to set to photo mode and take picture, set to video mode and start recording, or stop video recording +bool AP_Camera_Mount::cam_do_photo_video_mode(PhotoVideoMode photo_video_mode) { AP_Mount* mount = AP::mount(); if (mount != nullptr) { - return mount->set_cam_photo_video_mode(get_mount_instance(), ch_flag); + return mount->cam_do_photo_video_mode(get_mount_instance(), photo_video_mode); } return false; } diff --git a/libraries/AP_Camera/AP_Camera_Mount.h b/libraries/AP_Camera/AP_Camera_Mount.h index b98528d0f520d..7de19dc9992a8 100644 --- a/libraries/AP_Camera/AP_Camera_Mount.h +++ b/libraries/AP_Camera/AP_Camera_Mount.h @@ -33,11 +33,11 @@ class AP_Camera_Mount : public AP_Camera_Backend CLASS_NO_COPY(AP_Camera_Mount); //OW - // momentary switch to directly set camera to picture or video mode - bool set_cam_mode(bool video_mode) override; + // momentary switch to set to photo or video mode (video_mode false: photo mode, true: video mode) + bool cam_set_mode(bool video_mode) override; - // momentary switch to set camera to picture and video mode and start/stop recording using a 3 pos switch - bool set_cam_photo_video_mode(int8_t ch_flag) override; + // momentary 3 pos switch to set to photo mode and take picture, set to video mode and start recording, or stop video recording + bool cam_do_photo_video_mode(PhotoVideoMode photo_video_mode) override; //OWEND // entry point to actually take a picture. returns true on success diff --git a/libraries/AP_Camera/AP_Camera_shareddefs.h b/libraries/AP_Camera/AP_Camera_shareddefs.h index 6543eff0bc860..0f1a7430e6371 100644 --- a/libraries/AP_Camera/AP_Camera_shareddefs.h +++ b/libraries/AP_Camera/AP_Camera_shareddefs.h @@ -36,3 +36,11 @@ enum class TrackingType : uint8_t { TRK_RECTANGLE = 2 // tracking a rectangle }; +//OW +// options for set_cam_photo_video_mode() +enum class PhotoVideoMode : uint8_t { + PHOTO_TAKE_PIC = 0, // set to photo mode and take picture + VIDEO_START = 1, // set to video mode and start recording + VIDEO_STOP = 2 // stop video recording +}; +//OWEND diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index ca37ea70defc1..ebcd1f65d667e 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -1083,22 +1083,22 @@ AP_Mount *mount() }; //OW -bool AP_Mount::set_cam_mode(uint8_t instance, bool video_mode) +bool AP_Mount::cam_set_mode(uint8_t instance, bool video_mode) { auto *backend = get_instance(instance); if (backend == nullptr) { return false; } - return backend->set_cam_mode(video_mode); + return backend->cam_set_mode(video_mode); } -bool AP_Mount::set_cam_photo_video_mode(uint8_t instance, int8_t ch_flag) +bool AP_Mount::cam_do_photo_video_mode(uint8_t instance, PhotoVideoMode photo_video_mode) { auto *backend = get_instance(instance); if (backend == nullptr) { return false; } - return backend->set_cam_photo_video_mode(ch_flag); + return backend->cam_do_photo_video_mode(photo_video_mode); } void AP_Mount::handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 9e29a3a9963a0..bf9f273e7dc7a 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -271,11 +271,11 @@ class AP_Mount static const struct AP_Param::GroupInfo var_info[]; //OW - // set photo or video mode - bool set_cam_mode(uint8_t instance, bool video_mode); + // momentary switch to set to photo or video mode (video_mode false: photo mode, true: video mode) + bool cam_set_mode(uint8_t instance, bool video_mode); - // 3-way switch mode - bool set_cam_photo_video_mode(uint8_t instance, int8_t ch_flag); + // momentary 3 pos switch to set to photo mode and take picture, set to video mode and start recording, or stop video recording + bool cam_do_photo_video_mode(uint8_t instance, PhotoVideoMode photo_video_mode); // this is somewhat different to handle_message() in that it catches all messages // with significant work it potentially could be combined, but let's not introduce side effects diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 38f4ce3e3d9ca..4abb90259ceb9 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -192,11 +192,11 @@ class AP_Mount_Backend virtual void send_camera_settings(mavlink_channel_t chan) const {} //OW - // set photo or video mode - virtual bool set_cam_mode(bool video_mode) { return false; } + // momentary switch to set to photo or video mode (video_mode false: photo mode, true: video mode) + virtual bool cam_set_mode(bool video_mode) { return false; } - // 3-way switch mode - virtual bool set_cam_photo_video_mode(int8_t ch_flag) { return false; } + // momentary 3 pos switch to set to photo mode and take picture, set to video mode and start recording, or stop video recording + virtual bool cam_do_photo_video_mode(PhotoVideoMode photo_video_mode) { return false; } // handle msg - allows to process a msg from a gimbal virtual void handle_msg(const mavlink_message_t &msg) {} diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 2b4454c9bf03b..fa88f96333a39 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -1212,7 +1212,7 @@ bool BP_Mount_STorM32_MAVLink::record_video(bool start_recording) } -bool BP_Mount_STorM32_MAVLink::set_cam_mode(bool video_mode) +bool BP_Mount_STorM32_MAVLink::cam_set_mode(bool video_mode) { _camera_mode = (video_mode) ? CAMERA_MODE_VIDEO : CAMERA_MODE_PHOTO; send_cmd_do_digicam_configure(video_mode); @@ -1223,16 +1223,16 @@ bool BP_Mount_STorM32_MAVLink::set_cam_mode(bool video_mode) } -bool BP_Mount_STorM32_MAVLink::set_cam_photo_video_mode(int8_t ch_flag) +bool BP_Mount_STorM32_MAVLink::cam_do_photo_video_mode(PhotoVideoMode photo_video_mode) { - if (ch_flag > 0) { + if (photo_video_mode == PhotoVideoMode::VIDEO_START) { if (_camera_mode != CAMERA_MODE_VIDEO) { _camera_mode = CAMERA_MODE_VIDEO; send_cmd_do_digicam_configure(true); } send_cmd_do_digicam_control(true); } else - if (ch_flag < 0) { + if (photo_video_mode == PhotoVideoMode::PHOTO_TAKE_PIC) { if (_camera_mode != CAMERA_MODE_PHOTO) { _camera_mode = CAMERA_MODE_PHOTO; send_cmd_do_digicam_configure(false); diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 1c94c0d558263..83360f3594f19 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -7,6 +7,7 @@ #include "AP_Mount.h" #include "AP_Mount_Backend.h" +#include "AP_Camera/AP_Camera_shareddefs.h" class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend @@ -144,11 +145,13 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // If this is wanted, the camera component should be enabled in STorM32. void send_camera_settings(mavlink_channel_t chan) const override {} - // added: set photo or video mode - bool set_cam_mode(bool video_mode) override; + // added: - // added: 3-way switch mode - bool set_cam_photo_video_mode(int8_t ch_flag) override; + // momentary switch to set to photo or video mode (video_mode false: photo mode, true: video mode) + bool cam_set_mode(bool video_mode) override; + + // momentary 3 pos switch to set to photo mode and take picture, set to video mode and start recording, or stop video recording + bool cam_do_photo_video_mode(PhotoVideoMode photo_video_mode) override; private: diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 08f3b3cfe7837..1d8561be42f99 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -1529,7 +1529,7 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos if (camera == nullptr) { break; } - camera->set_cam_mode(ch_flag == AuxSwitchPos::HIGH); + camera->cam_set_mode(ch_flag == AuxSwitchPos::HIGH); break; } @@ -1541,18 +1541,18 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos } switch (ch_flag) { case AuxSwitchPos::HIGH: - camera->set_cam_photo_video_mode(1); + camera->cam_do_photo_video_mode(PhotoVideoMode::VIDEO_START); break; case AuxSwitchPos::MIDDLE: - camera->set_cam_photo_video_mode(0); + camera->cam_do_photo_video_mode(PhotoVideoMode::VIDEO_STOP); break; case AuxSwitchPos::LOW: - camera->set_cam_photo_video_mode(-1); + camera->cam_do_photo_video_mode(PhotoVideoMode::PHOTO_TAKE_PIC); break; } break; } - //OWEND +//OWEND #endif #if HAL_MOUNT_ENABLED From c84a568bcf03f851d9fc21e015ea2a37549ca1f5 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Mon, 6 Nov 2023 04:15:38 +0100 Subject: [PATCH 020/125] tidies --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 2 +- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 18 +++++++++++++----- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index fa88f96333a39..589a062fb334a 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -182,7 +182,7 @@ void BP_Mount_STorM32_MAVLink::init() // STorM32-Link wants 25 Hz, so we update at 25 Hz and 12.5 Hz respectively void BP_Mount_STorM32_MAVLink::update() { - update_target_angles(); // update at 50 Hz + update_target_angles(); // update at 50 Hz (RC_TARGETING handling assumes this) switch (_task_counter) { case TASK_SLOT0: diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 83360f3594f19..bdc4f762a28a8 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -30,7 +30,6 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // Is called by mount pre_arm_checks() (and nowhere else), which in turn is called // in AP_Arming::pre_arm_checks() (and nowhere else). // The mount prearm checks are tied to camera, and enabled by setting ARMING_CHECK_CAMERA. - // Thus Gremsy's method for e.g. checking attitude_status doesn't really make much sense. bool healthy() const override; @@ -51,7 +50,7 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // - Copter ModeAuto::do_mount_control() bool has_pan_control() const override { return false; } - //>> Deal with some AP nonsense: + //>> Deal with some AP strangeness: // get attitude as a quaternion. Returns true on success // att_quat will be an earth-frame quaternion rotated such that yaw is in body-frame. @@ -99,7 +98,8 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // => we need to overwrite it bool handle_gimbal_manager_flags(uint32_t flags) override; - // end of AP nonsense<< + //<< end of Deal with some AP strangeness + // handle GIMBAL_DEVICE_INFORMATION message // empty => we need to overwrite it @@ -125,9 +125,17 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // camera controls for gimbals that include a camera // + // These are called from the Camera_Mount backend (CameraType::Mount = 4). + // One could instead use Camera_MAVLink backend (CameraType::MAVLINK = 5), which uses + // MAV_CMD_DO_DIGICAM_CONTROL and MAV_CMD_DO_DIGICAM_CONFIGURE commands. + // One also could use Camera_MAVLinkCamV2 backend (CameraType::MAVLINK_CAMV2 = 6), which uses + // MAV_CMD_IMAGE_START_CAPTURE, MAV_CMD_VIDEO_START_CAPTURE, MAV_CMD_VIDEO_STOP_CAPTURE, and so on. This + // backend also digests a MAVLINK_MSG_ID_CAMERA_INFORMATION message. + // + // We implement them by using MAV_CMD_DO_DIGICAM_CONFIGURE, MAV_CMD_DO_DIGICAM_CONTROL. + // So may not provide advantages over CameraType::MAVLINK, maybe is even less powerful. + // take a picture. returns true on success - // We do not need to do anything since AP_Camera::take_picture() will send a a CMD_LONG:DO_DIGICAM_CONTROL to all components - // we have modified this such that it is not send if it is CamTrigType::mount (CAM_TRIGG_TYPE = 3). bool take_picture() override; // start or stop video recording. returns true on success From a3199c2b50e86454377689bd683029dc7e207f89 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Mon, 6 Nov 2023 05:29:12 +0100 Subject: [PATCH 021/125] has_xxx_control() --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 43 ++++++++++++++++++- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 13 +++--- 2 files changed, 48 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 589a062fb334a..59078cbfc93c1 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -166,7 +166,7 @@ void BP_Mount_STorM32_MAVLink::init() _mount_status = {}; _device_status = {}; flags_for_gimbal = 0; - _current_angles = { 0.0f, 0.0f, 0.0f, NAN}; // the NAN is important! + _current_angles = {0.0f, 0.0f, 0.0f, NAN}; // the NAN is important! _script_control_angles = {}; _yaw_lock = false; // can't be currently supported, so we need to ensure this is false. This is important! @@ -415,7 +415,6 @@ void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags(enum MAV_MOUNT_MODE mn } - //------------------------------------------------------ // Gimbal and protocol discovery //------------------------------------------------------ @@ -478,6 +477,46 @@ void BP_Mount_STorM32_MAVLink::determine_protocol(const mavlink_message_t &msg) } +//------------------------------------------------------ +// Gimbal control flags +//------------------------------------------------------ + +bool BP_Mount_STorM32_MAVLink::has_roll_control() const +{ + if (_protocol == PROTOCOL_GIMBAL_DEVICE) { + return (_device_info.cap_flags & GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS); + } + if (_protocol == PROTOCOL_MOUNT) { + return (_params.roll_angle_min < _params.roll_angle_max); + } + return false; +} + + +bool BP_Mount_STorM32_MAVLink::has_pitch_control() const +{ + if (_protocol == PROTOCOL_GIMBAL_DEVICE) { + return (_device_info.cap_flags & GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS); + } + if (_protocol == PROTOCOL_MOUNT) { + return (_params.pitch_angle_min < _params.pitch_angle_max); + } + return false; +} + + +bool BP_Mount_STorM32_MAVLink::has_pan_control() const +{ + if (_protocol == PROTOCOL_GIMBAL_DEVICE) { + return (_device_info.cap_flags & GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS); + } + if (_protocol == PROTOCOL_MOUNT) { + return yaw_range_valid(); + } + return false; +} + + //------------------------------------------------------ // Prearm & healthy functions //------------------------------------------------------ diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index bdc4f762a28a8..57faace0be0df 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -34,11 +34,11 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // return true if this mount accepts roll or pitch targets - // work it out later - // Only used in get_gimbal_manager_capability_flags(), which we overwrite anyhow - // => irrelevant for now - bool has_roll_control() const override { return true; } - bool has_pitch_control() const override { return true; } + // affects the gimbal manager capability flags send out by AP + // only used in get_gimbal_manager_capability_flags(), which we overwrite anyhow + // => irrelevant for now, but we set it to something useful nevertheless + bool has_roll_control() const override; + bool has_pitch_control() const override; // returns true if this mount can control its pan (required for multicopters) // false: copter is rotated in yaw, not the gimbal @@ -48,7 +48,8 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // - GCS_MAVLINK_Copter::handle_mount_message(), msg = MAVLINK_MSG_ID_MOUNT_CONTROL // - Copter Mode::AutoYaw::set_roi() // - Copter ModeAuto::do_mount_control() - bool has_pan_control() const override { return false; } + // for now do something simple + bool has_pan_control() const override; //>> Deal with some AP strangeness: From d9a3fa7c11cd29dbdb57e0177c99c562c91c3638 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Mon, 6 Nov 2023 06:04:14 +0100 Subject: [PATCH 022/125] more landed state --- ArduCopter/Copter.cpp | 10 +++++----- ArduCopter/Copter.h | 2 +- ArduPlane/ArduPlane.cpp | 6 +++--- ArduPlane/Plane.h | 2 +- Blimp/Blimp.cpp | 10 +++++----- Blimp/Blimp.h | 2 +- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 2 +- libraries/AP_Vehicle/AP_Vehicle.h | 13 ++++++++++++- libraries/GCS_MAVLink/GCS.h | 15 ++++++++++++++- 9 files changed, 43 insertions(+), 19 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 651473abb9072..1de5edc925c16 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -460,18 +460,18 @@ bool Copter::is_taking_off() const } //OW -MAV_LANDED_STATE Copter::get_landed_state() const +AP_Vehicle::LandedState Copter::get_landed_state() const { if (ap.land_complete) { - return MAV_LANDED_STATE_ON_GROUND; + return AP_Vehicle::LandedState::OnGround; } if (flightmode->is_landing()) { - return MAV_LANDED_STATE_LANDING; + return AP_Vehicle::LandedState::Landing; } if (flightmode->is_taking_off()) { - return MAV_LANDED_STATE_TAKEOFF; + return AP_Vehicle::LandedState::TakeOff; } - return MAV_LANDED_STATE_IN_AIR; + return AP_Vehicle::LandedState::InAir; } //OWEND diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 2c1d60ed78742..ad92553eced4e 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -701,7 +701,7 @@ class Copter : public AP_Vehicle { bool is_landing() const override; bool is_taking_off() const override; //OW - MAV_LANDED_STATE get_landed_state() const override; + AP_Vehicle::LandedState get_landed_state() const override; //OWEND void rc_loop(); void throttle_loop(); diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 4d41877747089..5dc31055700c5 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -895,14 +895,14 @@ bool Plane::is_taking_off() const } //OW -MAV_LANDED_STATE Plane::get_landed_state() const +AP_Vehicle::LandedState Plane::get_landed_state() const { if (plane.is_flying()) { // note that Q-modes almost always consider themselves as flying - return MAV_LANDED_STATE_IN_AIR; + return AP_Vehicle::LandedState::InAir; } - return MAV_LANDED_STATE_ON_GROUND; + return AP_Vehicle::LandedState::OnGround; } //OWEND diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index cfaf44f99cd09..a03e0b5a198b2 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1240,7 +1240,7 @@ class Plane : public AP_Vehicle { bool is_landing() const override; bool is_taking_off() const override; //OW - MAV_LANDED_STATE get_landed_state() const override; + AP_Vehicle::LandedState get_landed_state() const override; //OWEND #if AP_SCRIPTING_ENABLED bool set_target_location(const Location& target_loc) override; diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index cabacf8764746..5cd9d07709575 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -273,18 +273,18 @@ void Blimp::rotate_NE_to_BF(Vector2f &vec) } //OW -MAV_LANDED_STATE Blimp::get_landed_state() const +AP_Vehicle::LandedState Blimp::get_landed_state() const { if (ap.land_complete) { - return MAV_LANDED_STATE_ON_GROUND; + return AP_Vehicle::LandedState::OnGround; } if (flightmode->is_landing()) { - return MAV_LANDED_STATE_LANDING; + return AP_Vehicle::LandedState::Landing; } // if (flightmode->is_taking_off()) { - // return MAV_LANDED_STATE_TAKEOFF; + // return AP_Vehicle::LandedState::TakeOff; // } - return MAV_LANDED_STATE_IN_AIR; + return AP_Vehicle::LandedState::InAir; } //OWEND diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index c1536c0c2c46c..a8b0c87608756 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -310,7 +310,7 @@ class Blimp : public AP_Vehicle void rotate_NE_to_BF(Vector2f &vec); void rotate_BF_to_NE(Vector2f &vec); //OW - MAV_LANDED_STATE get_landed_state() const override; + AP_Vehicle::LandedState get_landed_state() const override; //OWEND // commands.cpp diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 59078cbfc93c1..c8b9cdb38cb06 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -993,7 +993,7 @@ landed state: we probably want to also take into account the arming state to mock something up ugly as we will have vehicle dependency here */ - uint8_t landed_state = AP::vehicle()->get_landed_state(); + uint8_t landed_state = (uint8_t)AP::vehicle()->get_landed_state(); #if APM_BUILD_TYPE(APM_BUILD_ArduCopter) // for copter we modify the landed states so as to reflect the 2 sec pre-take-off period diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 9fa057d34606d..b83238df1fc83 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -226,8 +226,19 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { virtual bool is_taking_off() const { return false; } //OW + // landed state enumeration + // not strictly required, but should mirror MAV_LANDED_STATE enum + enum class LandedState { + Undefined = 0, // landed state unknown/not available + OnGround = 1, // vehicle is landed (on ground) + InAir = 2, // vehicle is in air + TakeOff = 3, // vehicle currently taking off + Landing = 4, // vehicle currently landing + Last_ControlOutput // place new values before this + }; + // returns landed state - virtual MAV_LANDED_STATE get_landed_state() const { return MAV_LANDED_STATE_UNDEFINED; } + virtual AP_Vehicle::LandedState get_landed_state() const { return LandedState::Undefined; } //OWEND // zeroing the RC outputs can prevent unwanted motor movement: diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 62e1853000ae9..8539e777a63a9 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -508,7 +508,20 @@ class GCS_MAVLINK virtual MAV_VTOL_STATE vtol_state() const { return MAV_VTOL_STATE_UNDEFINED; } //OW // virtual MAV_LANDED_STATE landed_state() const { return MAV_LANDED_STATE_UNDEFINED; } - MAV_LANDED_STATE landed_state() const { return AP::vehicle()->get_landed_state(); } + MAV_LANDED_STATE landed_state() const { + switch (AP::vehicle()->get_landed_state()) { + case AP_Vehicle::LandedState::OnGround: + return MAV_LANDED_STATE_ON_GROUND; + case AP_Vehicle::LandedState::InAir: + return MAV_LANDED_STATE_IN_AIR; + case AP_Vehicle::LandedState::TakeOff: + return MAV_LANDED_STATE_TAKEOFF; + case AP_Vehicle::LandedState::Landing: + return MAV_LANDED_STATE_LANDING; + default: + return MAV_LANDED_STATE_UNDEFINED; + } + } //OWEND // return a MAVLink parameter type given a AP_Param type From ca5e68bbc2255f93ada117fd01e213906bd23d6e Mon Sep 17 00:00:00 2001 From: olliw42 Date: Mon, 6 Nov 2023 18:07:12 +0100 Subject: [PATCH 023/125] snyc landed state with pr --- libraries/GCS_MAVLink/GCS.h | 15 +-------------- libraries/GCS_MAVLink/GCS_Common.cpp | 18 ++++++++++++++++++ 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 8539e777a63a9..9f79276805779 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -508,20 +508,7 @@ class GCS_MAVLINK virtual MAV_VTOL_STATE vtol_state() const { return MAV_VTOL_STATE_UNDEFINED; } //OW // virtual MAV_LANDED_STATE landed_state() const { return MAV_LANDED_STATE_UNDEFINED; } - MAV_LANDED_STATE landed_state() const { - switch (AP::vehicle()->get_landed_state()) { - case AP_Vehicle::LandedState::OnGround: - return MAV_LANDED_STATE_ON_GROUND; - case AP_Vehicle::LandedState::InAir: - return MAV_LANDED_STATE_IN_AIR; - case AP_Vehicle::LandedState::TakeOff: - return MAV_LANDED_STATE_TAKEOFF; - case AP_Vehicle::LandedState::Landing: - return MAV_LANDED_STATE_LANDING; - default: - return MAV_LANDED_STATE_UNDEFINED; - } - } + MAV_LANDED_STATE landed_state() const; //OWEND // return a MAVLink parameter type given a AP_Param type diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index d8b90f08c53ab..0adc1ee5e2487 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -5369,6 +5369,24 @@ void GCS_MAVLINK::send_sys_status() errors4); // errors4 } +//OW +MAV_LANDED_STATE GCS_MAVLINK::landed_state() const +{ + switch (AP::vehicle()->get_landed_state()) { + case AP_Vehicle::LandedState::OnGround: + return MAV_LANDED_STATE_ON_GROUND; + case AP_Vehicle::LandedState::InAir: + return MAV_LANDED_STATE_IN_AIR; + case AP_Vehicle::LandedState::TakeOff: + return MAV_LANDED_STATE_TAKEOFF; + case AP_Vehicle::LandedState::Landing: + return MAV_LANDED_STATE_LANDING; + default: + return MAV_LANDED_STATE_UNDEFINED; + } +} +//OWEND + void GCS_MAVLINK::send_extended_sys_state() const { mavlink_msg_extended_sys_state_send(chan, vtol_state(), landed_state()); From 5c6302362cd2764eb2a29d9c32346fc03d35a518 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 7 Nov 2023 08:09:14 +0100 Subject: [PATCH 024/125] slight redo of version --- ArduCopter/version.h | 2 +- ArduPlane/version.h | 6 ++++++ libraries/bp_version.h | 3 ++- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 2811befe67b88..8ac546d2665ee 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -11,7 +11,7 @@ //OW #undef THISFIRMWARE #include "../libraries/bp_version.h" -#define THISFIRMWARE "BetaCopter V4.5.0-dev" BETAPILOTVERSION " 20231101" +#define THISFIRMWARE "BetaCopter V4.5.0-dev" BETAPILOTVERSION " " DATEOFBASEBRANCH //OWEND // the following line is parsed by the autotest scripts diff --git a/ArduPlane/version.h b/ArduPlane/version.h index cdf466f7155fb..b963797507461 100644 --- a/ArduPlane/version.h +++ b/ArduPlane/version.h @@ -8,6 +8,12 @@ #define THISFIRMWARE "ArduPlane V4.5.0-dev" +//OW +#undef THISFIRMWARE +#include "../libraries/bp_version.h" +#define THISFIRMWARE "BetaPlane V4.5.0-dev" BETAPILOTVERSION " " DATEOFBASEBRANCH +//OWEND + // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV diff --git a/libraries/bp_version.h b/libraries/bp_version.h index ffa4c256858b9..5e54fb0910b06 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,11 +1,12 @@ #pragma once #define BETAPILOTVERSION "v059" +#define DATEOFBASEBRANCH "20231106" /* search for //OW to find all changes -2023.11.01: v059 +2023.11.06: v059 upgraded to ArduPilot master 4.5.0-dev 2023.03.10: v058 From af42feb2ddd9126c6061e1cdef09d341bcb5e4fc Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 7 Nov 2023 09:30:30 +0100 Subject: [PATCH 025/125] align with landed satte PR --- libraries/GCS_MAVLink/GCS.h | 2 +- libraries/GCS_MAVLink/GCS_Common.cpp | 18 ------------------ libraries/bp_version.h | 2 +- 3 files changed, 2 insertions(+), 20 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index fabcef3c61500..5bf889ebe3aeb 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -508,7 +508,7 @@ class GCS_MAVLINK virtual MAV_VTOL_STATE vtol_state() const { return MAV_VTOL_STATE_UNDEFINED; } //OW // virtual MAV_LANDED_STATE landed_state() const { return MAV_LANDED_STATE_UNDEFINED; } - MAV_LANDED_STATE landed_state() const; + MAV_LANDED_STATE landed_state() const { return (MAV_LANDED_STATE)AP::vehicle()->get_landed_state(); } //OWEND // return a MAVLink parameter type given a AP_Param type diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 654b091488787..2efa4c7be6902 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -5366,24 +5366,6 @@ void GCS_MAVLINK::send_sys_status() errors4); // errors4 } -//OW -MAV_LANDED_STATE GCS_MAVLINK::landed_state() const -{ - switch (AP::vehicle()->get_landed_state()) { - case AP_Vehicle::LandedState::OnGround: - return MAV_LANDED_STATE_ON_GROUND; - case AP_Vehicle::LandedState::InAir: - return MAV_LANDED_STATE_IN_AIR; - case AP_Vehicle::LandedState::TakeOff: - return MAV_LANDED_STATE_TAKEOFF; - case AP_Vehicle::LandedState::Landing: - return MAV_LANDED_STATE_LANDING; - default: - return MAV_LANDED_STATE_UNDEFINED; - } -} -//OWEND - void GCS_MAVLINK::send_extended_sys_state() const { mavlink_msg_extended_sys_state_send(chan, vtol_state(), landed_state()); diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 5e54fb0910b06..ed321512ab8f0 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,7 +1,7 @@ #pragma once #define BETAPILOTVERSION "v059" -#define DATEOFBASEBRANCH "20231106" +#define DATEOFBASEBRANCH "20231107" /* search for //OW to find all changes From c93928ef9f7754a1899a3bb9a00137b616f673f3 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 7 Nov 2023 09:41:10 +0100 Subject: [PATCH 026/125] more --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 20 +++++++++---------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index c8b9cdb38cb06..8a83e53f26ff7 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -989,17 +989,16 @@ landed state: Copter has it: GCS_MAVLINK_Copter::landed_state(), yields ON_GROUND, TAKEOFF, IN_AIR, LANDING Plane has it: GCS_MAVLINK_Plane::landed_state(), only yields ON_GROUND or IN_AIR Blimp also has it, blimp not relevant for us - but is protected, so we needed to mock it up - we probably want to also take into account the arming state to mock something up - ugly as we will have vehicle dependency here + we want to also take into account the arming state to mock something up + ugly as we have vehicle dependency, but that's how it is */ uint8_t landed_state = (uint8_t)AP::vehicle()->get_landed_state(); #if APM_BUILD_TYPE(APM_BUILD_ArduCopter) - // for copter we modify the landed states so as to reflect the 2 sec pre-take-off period - // leads to a PREPARING_FOR_TAKEOFF before takeoff, but also after landing! + // for copter we modify the landed state so as to reflect the 2 sec pre-take-off period + // the code below leads to a PREPARING_FOR_TAKEOFF before takeoff, but also after landing! // for the latter we would have to catch that it was flying, but we don't need to care - // the gimbal will do inits when ON_GROUND, and apply them when transitioning to PREPARING_FOR_TAKEOFF + // the gimbal will do inits when ON_GROUND, and refresh them when transitioning to PREPARING_FOR_TAKEOFF // it won't do it for other transitions, so e.g. also not for plane const AP_Notify ¬ify = AP::notify(); if ((landed_state == MAV_LANDED_STATE_ON_GROUND) && notify.flags.armed) { @@ -1008,11 +1007,6 @@ landed state: #endif // ready to send - static uint32_t tlast_us = 0; - uint32_t t_us = AP_HAL::micros(); - uint32_t dt_us = t_us - tlast_us; - tlast_us = t_us; - float qa[4] = {q.q1, q.q2, q.q3, q.q4}; mavlink_msg_autopilot_state_for_gimbal_device_send( @@ -1028,6 +1022,10 @@ landed state: angular_velocity_z // float angular_velocity_z ); + static uint32_t tlast_us = 0; + uint32_t t_us = AP_HAL::micros(); + uint32_t dt_us = t_us - tlast_us; + tlast_us = t_us; BP_LOG("MTL0", BP_LOG_MTL_AUTOPILOTSTATE_HEADER, q[0],q[1],q[2],q[3], From bdde5e702ae081b55a491e2f9ac06c0407a70118 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 7 Nov 2023 18:20:08 +0100 Subject: [PATCH 027/125] delay sending banner --- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 15 +++++++++++++++ libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 3 +++ 2 files changed, 18 insertions(+) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 8a83e53f26ff7..3e1570432a523 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -210,6 +210,8 @@ void BP_Mount_STorM32_MAVLink::update() _task_counter++; if (_task_counter > TASK_SLOT3) _task_counter = TASK_SLOT0; + update_send_banner(); + if (!_initialised) { find_gimbal(); return; @@ -1085,6 +1087,19 @@ void BP_Mount_STorM32_MAVLink::send_rc_channels(void) void BP_Mount_STorM32_MAVLink::send_banner(void) { + // we postpone sending it by few seconds, to avoid multiple sends + _request_send_banner_ms = AP_HAL::millis(); +} + + +void BP_Mount_STorM32_MAVLink::update_send_banner(void) +{ + if (!_request_send_banner_ms) return; // no request + + uint32_t tnow_ms = AP_HAL::millis(); + if ((tnow_ms - _request_send_banner_ms) < 3500) return; // not yet time to send + _request_send_banner_ms = 0; + if (_got_device_info) { // we have lots of info GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: gimbal at %u", _instance+1, _compid); diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 57faace0be0df..1142b047df260 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -212,6 +212,9 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend bool prearmchecks_do(void); void send_prearmchecks_txt(void); + uint32_t _request_send_banner_ms; + void update_send_banner(void); + // gimbal target & control struct { From db23c352d649780f5a3d7c5007b7025ed022722a Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 7 Nov 2023 18:20:50 +0100 Subject: [PATCH 028/125] handle heartbeat error flags for mount mode --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 22 +++++++++++++------ libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 1 + 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 3e1570432a523..7949354f3bfa7 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -529,20 +529,21 @@ const uint32_t FAILURE_FLAGS = GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR | GIMBAL_DEVICE_ERROR_FLAGS_SOFTWARE_ERROR | GIMBAL_DEVICE_ERROR_FLAGS_COMMS_ERROR; - // GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER; void BP_Mount_STorM32_MAVLink::send_prearmchecks_txt(void) { + uint32_t failure_flags = (_protocol == PROTOCOL_GIMBAL_DEVICE) ? _device_status.received_failure_flags : _gimbal_error_flags; + if (!_initialised || !_gimbal_prearmchecks_ok || !_armed) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: arm", _instance+1); } else - if (_device_status.received_failure_flags & FAILURE_FLAGS) { + if (failure_flags & FAILURE_FLAGS) { char txt[255]; strcpy(txt, ""); - if (_device_status.received_failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR) strcat(txt, "mot,"); - if (_device_status.received_failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR) strcat(txt, "enc,"); - if (_device_status.received_failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR) strcat(txt, "volt,"); + if (failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR) strcat(txt, "mot,"); + if (failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR) strcat(txt, "enc,"); + if (failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR) strcat(txt, "volt,"); if (txt[0] != '\0') { txt[strlen(txt)-1] = '\0'; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: %s", _instance+1, txt); @@ -605,8 +606,9 @@ bool BP_Mount_STorM32_MAVLink::prearmchecks_do(void) } // check failure flags - // We also check for GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER - // which essentially only means that gimbal got GIMBAL_DEVICE_SET_ATTITUDE messages. + // We should also check for GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER + // which means that gimbal did not got GIMBAL_DEVICE_SET_ATTITUDE messages. +// if ((_device_status.received_failure_flags & (FAILURE_FLAGS | GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER)) > 0) { if ((_device_status.received_failure_flags & FAILURE_FLAGS) > 0) { return false; } @@ -616,6 +618,11 @@ bool BP_Mount_STorM32_MAVLink::prearmchecks_do(void) if ((AP_HAL::millis() - _mount_status.received_tlast_ms) > 1000) { return false; } + + // check failure flags // are set since v2.68b + if ((_gimbal_error_flags & FAILURE_FLAGS) > 0) { + return false; + } } // if we get this far the mount is healthy @@ -749,6 +756,7 @@ void BP_Mount_STorM32_MAVLink::handle_msg(const mavlink_message_t &msg) if ((payload.custom_mode & 0x80000000) == 0) { // we don't follow all changes, but just toggle it to true once _gimbal_prearmchecks_ok = true; } + _gimbal_error_flags = (payload.custom_mode & 0x00FFFF00) >> 8; if (!_armingchecks_enabled) { // ArduPilot arming checks disabled, so let's do ourself if (!_prearmchecks_done) prearmchecks_do(); } diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 1142b047df260..32f7bca7a5858 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -203,6 +203,7 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // pre-arm and healthy checks + uint32_t _gimbal_error_flags; // error flags in custom_mode field of the HEARTBEAT message (restricted to 16 bits) bool _gimbal_prearmchecks_ok; // true when the gimbal stops reporting prearm fail in the HEARTBEAT message bool _armingchecks_enabled; // true when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA set, we know from healty() bool _prearmchecks_done; // becomes true when all checks were passed once at startup From 688a38997b380502c833f66cddcb631cb196d9b9 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 7 Nov 2023 18:21:11 +0100 Subject: [PATCH 029/125] camera better --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 22 +++++++++++++++---- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 1 + 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 7949354f3bfa7..9c458421ed66a 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -1286,22 +1286,36 @@ bool BP_Mount_STorM32_MAVLink::cam_set_mode(bool video_mode) bool BP_Mount_STorM32_MAVLink::cam_do_photo_video_mode(PhotoVideoMode photo_video_mode) { if (photo_video_mode == PhotoVideoMode::VIDEO_START) { + if (_camera_mode != CAMERA_MODE_VIDEO) { _camera_mode = CAMERA_MODE_VIDEO; - send_cmd_do_digicam_configure(true); + send_cmd_do_digicam_configure(true); // set video mode } - send_cmd_do_digicam_control(true); + + send_cmd_do_digicam_control(true); // start video + _camera_is_recording = true; + } else if (photo_video_mode == PhotoVideoMode::PHOTO_TAKE_PIC) { + + if (_camera_is_recording) { + send_cmd_do_digicam_control(false); + _camera_is_recording = false; + } + if (_camera_mode != CAMERA_MODE_PHOTO) { _camera_mode = CAMERA_MODE_PHOTO; - send_cmd_do_digicam_configure(false); + send_cmd_do_digicam_configure(false); // set photo mode } - send_cmd_do_digicam_control(true); + send_cmd_do_digicam_control(true); // take picture + } else { + if (_camera_mode == CAMERA_MODE_VIDEO) { send_cmd_do_digicam_control(false); + _camera_is_recording = false; } + } return true; diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 32f7bca7a5858..0427d90439a15 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -286,6 +286,7 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend CAMERA_MODE_VIDEO, }; CameraMode _camera_mode; // current camera mode + bool _camera_is_recording; void send_cmd_do_digicam_configure(bool video_mode); void send_cmd_do_digicam_control(bool shoot); From 7c8307c3d611e97b9f573d0677e5a6c7631797e4 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 8 Nov 2023 18:35:18 +0100 Subject: [PATCH 030/125] v --- ArduCopter/version.h | 2 +- ArduPlane/version.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 8ac546d2665ee..c0f3d45f032ad 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -11,7 +11,7 @@ //OW #undef THISFIRMWARE #include "../libraries/bp_version.h" -#define THISFIRMWARE "BetaCopter V4.5.0-dev" BETAPILOTVERSION " " DATEOFBASEBRANCH +#define THISFIRMWARE "BetaCopter V4.5.0-dev " BETAPILOTVERSION " " DATEOFBASEBRANCH //OWEND // the following line is parsed by the autotest scripts diff --git a/ArduPlane/version.h b/ArduPlane/version.h index b963797507461..1d58323c15b3b 100644 --- a/ArduPlane/version.h +++ b/ArduPlane/version.h @@ -11,7 +11,7 @@ //OW #undef THISFIRMWARE #include "../libraries/bp_version.h" -#define THISFIRMWARE "BetaPlane V4.5.0-dev" BETAPILOTVERSION " " DATEOFBASEBRANCH +#define THISFIRMWARE "BetaPlane V4.5.0-dev " BETAPILOTVERSION " " DATEOFBASEBRANCH //OWEND // the following line is parsed by the autotest scripts From dd18c0e15d755c10759d12b8ee33a65f4cccd5ab Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 8 Nov 2023 18:35:54 +0100 Subject: [PATCH 031/125] tidy --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 56 +++++++++---------- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 2 +- 2 files changed, 29 insertions(+), 29 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 9c458421ed66a..757b7a29b7f93 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -269,9 +269,35 @@ bool BP_Mount_STorM32_MAVLink::handle_gimbal_manager_flags(uint32_t flags) } +void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags(enum MAV_MOUNT_MODE mntmode) +{ + flags_for_gimbal = 0; + + switch (mntmode) { + case MAV_MOUNT_MODE_RETRACT: + flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RETRACT; + break; + case MAV_MOUNT_MODE_NEUTRAL: + flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_NEUTRAL; + break; + default: + break; + } + + // we currently only support pitch,roll lock, not pitch,roll follow + flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK; + + // we currently do not support yaw lock +// if (_is_yaw_lock) flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_LOCK; + + // set either YAW_IN_VEHICLE_FRAME or YAW_IN_EARTH_FRAME, to indicate new message format, STorM32 will reject otherwise + flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME; +} + + void BP_Mount_STorM32_MAVLink::send_target_angles(void) { - // just send stupidly at 12.5 Hz if (!get_target_angles()) return; // if false don't send + // just send stupidly at 12.5 Hz, no check if get_target_angles() made a change update_gimbal_device_flags(get_mode()); @@ -391,32 +417,6 @@ void BP_Mount_STorM32_MAVLink::update_target_angles(void) } -void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags(enum MAV_MOUNT_MODE mntmode) -{ - flags_for_gimbal = 0; - - switch (mntmode) { - case MAV_MOUNT_MODE_RETRACT: - flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RETRACT; - break; - case MAV_MOUNT_MODE_NEUTRAL: - flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_NEUTRAL; - break; - default: - break; - } - - // we currently only support pitch,roll lock, not pitch,roll follow - flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK; - - // we currently do not support yaw lock -// if (_is_yaw_lock) flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_LOCK; - - // set either YAW_IN_VEHICLE_FRAME or YAW_IN_EARTH_FRAME, to indicate new message format, STorM32 will reject otherwise - flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME; -} - - //------------------------------------------------------ // Gimbal and protocol discovery //------------------------------------------------------ @@ -1219,7 +1219,7 @@ void BP_Mount_STorM32_MAVLink::set_attitude_euler(float roll_deg, float pitch_de // get attitude as a quaternion. Returns true on success -bool BP_Mount_STorM32_MAVLink::get_attitude_quaternion(Quaternion& att_quat) +bool BP_Mount_STorM32_MAVLink::get_attitude_quaternion(Quaternion &att_quat) { if (!_initialised) { return false; diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 0427d90439a15..8aafc18106f6d 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -62,7 +62,7 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // - scripts via calling AP_Mount::get_attitude_euler() // AP nonsense: uses inappropriate Euler angles. // => we set roll to zero, to minimize harm. - bool get_attitude_quaternion(Quaternion& att_quat) override; + bool get_attitude_quaternion(Quaternion &att_quat) override; // set yaw_lock. If true, the gimbal's yaw target is maintained in earth-frame meaning // it will lock onto an earth-frame heading (e.g. North). If false (aka "follow") the gimbal's yaw From 563b3fdd922c4f145515c5d3d8ad050719754588 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Thu, 9 Nov 2023 08:53:04 +0100 Subject: [PATCH 032/125] tidy --- libraries/AP_RCProtocol/AP_RCProtocol.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index 42f62a7fcf6d0..54c0de69cbe46 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -74,7 +74,7 @@ class AP_RCProtocol { DRONECAN = 13, #endif //OW RADIOLINK - MAVLINK_RADIO = 14, // RC_PROTOCOLS +2^14 = 16384 + MAVLINK_RADIO = 14, // RC_PROTOCOLS +2^15 = 32768 //OWEND NONE //last enum always is None }; From 5a6bd619092e363e703ace0260cd5786c6c20212 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Thu, 9 Nov 2023 08:54:09 +0100 Subject: [PATCH 033/125] bp_version --- libraries/bp_version.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/bp_version.h b/libraries/bp_version.h index ed321512ab8f0..168b3b04eb932 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,7 +1,7 @@ #pragma once -#define BETAPILOTVERSION "v059" -#define DATEOFBASEBRANCH "20231107" +#define BETAPILOTVERSION "v059a" +#define DATEOFBASEBRANCH "20231109" /* search for //OW to find all changes From da83e13d3e06df2820a4cd104cf44558f37548f3 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Thu, 9 Nov 2023 09:23:45 +0100 Subject: [PATCH 034/125] vbump --- libraries/bp_version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 168b3b04eb932..903a7176fa6d7 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,6 +1,6 @@ #pragma once -#define BETAPILOTVERSION "v059a" +#define BETAPILOTVERSION "v059b" #define DATEOFBASEBRANCH "20231109" /* From bbbb5a8ea7251662403311119e01227b7b2010fa Mon Sep 17 00:00:00 2001 From: olliw42 Date: Fri, 10 Nov 2023 02:49:50 +0100 Subject: [PATCH 035/125] undo vehicle --- ArduCopter/Copter.cpp | 16 ---------------- ArduCopter/Copter.h | 3 --- ArduCopter/GCS_Mavlink.cpp | 4 ---- ArduCopter/GCS_Mavlink.h | 4 +--- ArduPlane/ArduPlane.cpp | 12 ------------ ArduPlane/GCS_Mavlink.h | 4 +--- ArduPlane/Plane.h | 3 --- Blimp/Blimp.cpp | 16 ---------------- Blimp/Blimp.h | 3 --- Blimp/GCS_Mavlink.cpp | 5 +---- Blimp/GCS_Mavlink.h | 5 ++--- libraries/AP_Vehicle/AP_Vehicle.h | 16 ---------------- libraries/GCS_MAVLink/GCS.h | 12 +++++------- 13 files changed, 10 insertions(+), 93 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 0ab84e7119e36..7ddd0be1a8760 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -461,22 +461,6 @@ bool Copter::is_taking_off() const return flightmode->is_taking_off(); } -//OW -AP_Vehicle::LandedState Copter::get_landed_state() const -{ - if (ap.land_complete) { - return AP_Vehicle::LandedState::OnGround; - } - if (flightmode->is_landing()) { - return AP_Vehicle::LandedState::Landing; - } - if (flightmode->is_taking_off()) { - return AP_Vehicle::LandedState::TakeOff; - } - return AP_Vehicle::LandedState::InAir; -} -//OWEND - bool Copter::current_mode_requires_mission() const { #if MODE_AUTO_ENABLED == ENABLED diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index ad92553eced4e..2d620deb8305c 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -700,9 +700,6 @@ class Copter : public AP_Vehicle { #endif // AP_SCRIPTING_ENABLED bool is_landing() const override; bool is_taking_off() const override; -//OW - AP_Vehicle::LandedState get_landed_state() const override; -//OWEND void rc_loop(); void throttle_loop(); void update_batt_compass(void); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index dd3c97109e344..d86efc6c0591c 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1544,8 +1544,6 @@ uint64_t GCS_MAVLINK_Copter::capabilities() const GCS_MAVLINK::capabilities()); } -//OW -/* MAV_LANDED_STATE GCS_MAVLINK_Copter::landed_state() const { if (copter.ap.land_complete) { @@ -1559,8 +1557,6 @@ MAV_LANDED_STATE GCS_MAVLINK_Copter::landed_state() const } return MAV_LANDED_STATE_IN_AIR; } -*/ -//OWEND void GCS_MAVLINK_Copter::send_wind() const { diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index 35a8222a0db72..98de6b502f386 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -53,9 +53,7 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK uint64_t capabilities() const override; virtual MAV_VTOL_STATE vtol_state() const override { return MAV_VTOL_STATE_MC; }; -//OW -// virtual MAV_LANDED_STATE landed_state() const override; -//OWEND + virtual MAV_LANDED_STATE landed_state() const override; void handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow) override; diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 5dc31055700c5..61d7642533327 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -894,18 +894,6 @@ bool Plane::is_taking_off() const return control_mode->is_taking_off(); } -//OW -AP_Vehicle::LandedState Plane::get_landed_state() const -{ - if (plane.is_flying()) { - // note that Q-modes almost always consider themselves as flying - return AP_Vehicle::LandedState::InAir; - } - - return AP_Vehicle::LandedState::OnGround; -} -//OWEND - // correct AHRS pitch for TRIM_PITCH_CD in non-VTOL modes, and return VTOL view in VTOL void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const { diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index aef55f2eb2c9e..bdc3422ee9b5a 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -94,7 +94,5 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK #endif MAV_VTOL_STATE vtol_state() const override; -//OW -// MAV_LANDED_STATE landed_state() const override; -//OWEND + MAV_LANDED_STATE landed_state() const override; }; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index a03e0b5a198b2..d2fd912d4d293 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1239,9 +1239,6 @@ class Plane : public AP_Vehicle { void failsafe_check(void); bool is_landing() const override; bool is_taking_off() const override; -//OW - AP_Vehicle::LandedState get_landed_state() const override; -//OWEND #if AP_SCRIPTING_ENABLED bool set_target_location(const Location& target_loc) override; bool get_target_location(Location& target_loc) override; diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index 5cd9d07709575..ff5327006738d 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -272,22 +272,6 @@ void Blimp::rotate_NE_to_BF(Vector2f &vec) } -//OW -AP_Vehicle::LandedState Blimp::get_landed_state() const -{ - if (ap.land_complete) { - return AP_Vehicle::LandedState::OnGround; - } - if (flightmode->is_landing()) { - return AP_Vehicle::LandedState::Landing; - } - // if (flightmode->is_taking_off()) { - // return AP_Vehicle::LandedState::TakeOff; - // } - return AP_Vehicle::LandedState::InAir; -} -//OWEND - /* constructor for main Blimp class */ diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index a8b0c87608756..32ac39b5bddf4 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -309,9 +309,6 @@ class Blimp : public AP_Vehicle void update_altitude(); void rotate_NE_to_BF(Vector2f &vec); void rotate_BF_to_NE(Vector2f &vec); -//OW - AP_Vehicle::LandedState get_landed_state() const override; -//OWEND // commands.cpp void update_home_from_EKF(); diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index 082c2157b1194..c96624649184a 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -549,8 +549,7 @@ uint64_t GCS_MAVLINK_Blimp::capabilities() const MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET | GCS_MAVLINK::capabilities()); } -//OW -/* + MAV_LANDED_STATE GCS_MAVLINK_Blimp::landed_state() const { if (blimp.ap.land_complete) { @@ -564,8 +563,6 @@ MAV_LANDED_STATE GCS_MAVLINK_Blimp::landed_state() const // } return MAV_LANDED_STATE_IN_AIR; } -*/ -//OWEND void GCS_MAVLINK_Blimp::send_wind() const { diff --git a/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h index 14899074e01fb..c248ae0886e2b 100644 --- a/Blimp/GCS_Mavlink.h +++ b/Blimp/GCS_Mavlink.h @@ -40,9 +40,8 @@ class GCS_MAVLINK_Blimp : public GCS_MAVLINK { return MAV_VTOL_STATE_MC; }; -//OW -// virtual MAV_LANDED_STATE landed_state() const override; -//OWEND + virtual MAV_LANDED_STATE landed_state() const override; + private: void handleMessage(const mavlink_message_t &msg) override; diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index b83238df1fc83..18b43fc0209ca 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -225,22 +225,6 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { // returns true if vehicle is in the process of taking off virtual bool is_taking_off() const { return false; } -//OW - // landed state enumeration - // not strictly required, but should mirror MAV_LANDED_STATE enum - enum class LandedState { - Undefined = 0, // landed state unknown/not available - OnGround = 1, // vehicle is landed (on ground) - InAir = 2, // vehicle is in air - TakeOff = 3, // vehicle currently taking off - Landing = 4, // vehicle currently landing - Last_ControlOutput // place new values before this - }; - - // returns landed state - virtual AP_Vehicle::LandedState get_landed_state() const { return LandedState::Undefined; } -//OWEND - // zeroing the RC outputs can prevent unwanted motor movement: virtual bool should_zero_rc_outputs_on_reboot() const { return false; } diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index fe389b458b7ac..448dfa9752e4c 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -24,9 +24,6 @@ #include #include #include -//OW -#include -//OWEND #include "ap_message.h" #define GCS_DEBUG_SEND_MESSAGE_TIMINGS 0 @@ -506,10 +503,7 @@ class GCS_MAVLINK virtual MAV_STATE vehicle_system_status() const = 0; virtual MAV_VTOL_STATE vtol_state() const { return MAV_VTOL_STATE_UNDEFINED; } -//OW -// virtual MAV_LANDED_STATE landed_state() const { return MAV_LANDED_STATE_UNDEFINED; } - MAV_LANDED_STATE landed_state() const { return (MAV_LANDED_STATE)AP::vehicle()->get_landed_state(); } -//OWEND + virtual MAV_LANDED_STATE landed_state() const { return MAV_LANDED_STATE_UNDEFINED; } // return a MAVLink parameter type given a AP_Param type static MAV_PARAM_TYPE mav_param_type(enum ap_var_type t); @@ -1262,6 +1256,10 @@ class GCS virtual uint8_t sysid_this_mav() const = 0; +//OW + MAV_LANDED_STATE get_landed_state(void) const { return num_gcs() > 0 ? chan(0)->landed_state() : MAV_LANDED_STATE_UNDEFINED; } +//OWEND + protected: virtual GCS_MAVLINK *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters ¶ms, From 7a1fad7022960ee8d21d4e2809ff56e901227fd0 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Fri, 10 Nov 2023 03:03:39 +0100 Subject: [PATCH 036/125] fast arming report synced with PR, crsf throttle --- libraries/AP_Arming/AP_Arming.cpp | 8 ++++---- libraries/AP_Arming/AP_Arming.h | 2 +- libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp | 9 ++++++++- libraries/AP_RCTelemetry/AP_CRSF_Telem.h | 3 +++ 4 files changed, 16 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 99948ca27db39..dd88305730bdd 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -1546,7 +1546,7 @@ bool AP_Arming::pre_arm_checks(bool report) #endif //OW // return hardware_safety_check(report) - bool res = hardware_safety_check(report) + bool checks_result = hardware_safety_check(report) //OWEND #if HAL_HAVE_IMU_HEATER & heater_min_temperature_checks(report) @@ -1584,12 +1584,12 @@ bool AP_Arming::pre_arm_checks(bool report) & estop_checks(report); //OW - if (!res && pre_arm_checks_last_result) { // check went from true to false + if (!checks_result && last_prearm_checks_result) { // check went from true to false report_immediately = true; } - pre_arm_checks_last_result = res; + last_prearm_checks_result = checks_result; - return res; + return checks_result; //OWEND } diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index b16b9fbdcffd0..dd08041a232ab 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -305,7 +305,7 @@ class AP_Arming { bool running_arming_checks; // true if the arming checks currently being performed are being done because the vehicle is trying to arm the vehicle //OW - bool pre_arm_checks_last_result; + bool last_prearm_checks_result; bool report_immediately; //OWEND }; diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp index cd21896afe7d7..a440e65f4dd87 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp @@ -468,7 +468,14 @@ void AP_CRSF_Telem::process_packet(uint8_t idx) GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"%s: RX device ping failed", get_protocol_string()); } else { calc_device_ping(AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER); - GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"%s: requesting RX device info", get_protocol_string()); +//OWCRSF +// GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"%s: requesting RX device info", get_protocol_string()); + uint32_t tnow_ms = AP_HAL::millis(); + if ((tnow_ms - _crsf_version.last_request_info_ms) > 5000) { + _crsf_version.last_request_info_ms = tnow_ms; + GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"%s: requesting RX device info", get_protocol_string()); + } +//OWEND } break; case DEVICE_PING: diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.h b/libraries/AP_RCTelemetry/AP_CRSF_Telem.h index 9bbeeaf03a29b..d4154563238ea 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.h +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.h @@ -335,6 +335,9 @@ class AP_CRSF_Telem : public AP_RCTelemetry { bool use_rf_mode; AP_RCProtocol_CRSF::ProtocolType protocol; bool pending = true; +//OWCRSF + uint32_t last_request_info_ms; +//OWEND } _crsf_version; struct { From dab78c806884c93eae430fa9356401eaba4f5bb5 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Fri, 10 Nov 2023 03:04:35 +0100 Subject: [PATCH 037/125] ,ore undo vehicle, sync with master --- ArduPlane/GCS_Mavlink.cpp | 5 +---- ArduPlane/GCS_Mavlink.h | 1 + libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 2 +- libraries/GCS_MAVLink/GCS.h | 1 + libraries/GCS_MAVLink/GCS_Common.cpp | 1 - libraries/GCS_MAVLink/MAVLink_routing.cpp | 1 - 6 files changed, 4 insertions(+), 7 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index f46a7cba5333c..71df773b681fd 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1508,8 +1508,6 @@ MAV_VTOL_STATE GCS_MAVLINK_Plane::vtol_state() const #endif }; -//OW -/* MAV_LANDED_STATE GCS_MAVLINK_Plane::landed_state() const { if (plane.is_flying()) { @@ -1519,5 +1517,4 @@ MAV_LANDED_STATE GCS_MAVLINK_Plane::landed_state() const return MAV_LANDED_STATE_ON_GROUND; } -*/ -//OWEND + diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index bdc3422ee9b5a..b30ac87287669 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -95,4 +95,5 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK MAV_VTOL_STATE vtol_state() const override; MAV_LANDED_STATE landed_state() const override; + }; diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 757b7a29b7f93..d15f160a1c992 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -1002,7 +1002,7 @@ landed state: we want to also take into account the arming state to mock something up ugly as we have vehicle dependency, but that's how it is */ - uint8_t landed_state = (uint8_t)AP::vehicle()->get_landed_state(); + uint8_t landed_state = (uint8_t)gcs().get_landed_state(); // AP::vehicle()->get_landed_state(); #if APM_BUILD_TYPE(APM_BUILD_ArduCopter) // for copter we modify the landed state so as to reflect the 2 sec pre-take-off period diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 448dfa9752e4c..ce6e81f62d95e 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -24,6 +24,7 @@ #include #include #include + #include "ap_message.h" #define GCS_DEBUG_SEND_MESSAGE_TIMINGS 0 diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 213f546956b9f..0f94d7de11a3b 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -6902,4 +6902,3 @@ void GCS_MAVLINK::handle_radio_rc_channels(const mavlink_message_t &msg) //OWEND #endif // HAL_GCS_ENABLED - diff --git a/libraries/GCS_MAVLink/MAVLink_routing.cpp b/libraries/GCS_MAVLink/MAVLink_routing.cpp index d9e7db9fa1313..3933360c33519 100644 --- a/libraries/GCS_MAVLink/MAVLink_routing.cpp +++ b/libraries/GCS_MAVLink/MAVLink_routing.cpp @@ -436,5 +436,4 @@ void MAVLink_routing::get_targets(const mavlink_message_t &msg, int16_t &sysid, } } - #endif // HAL_GCS_ENABLED From 280a23a0bc4542eee4715fb63d673eb14ce9c021 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Fri, 10 Nov 2023 03:09:04 +0100 Subject: [PATCH 038/125] serial control --- libraries/GCS_MAVLink/GCS_serial_control.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS_serial_control.cpp b/libraries/GCS_MAVLink/GCS_serial_control.cpp index 50322e2e5dac2..149dffae9cb0b 100644 --- a/libraries/GCS_MAVLink/GCS_serial_control.cpp +++ b/libraries/GCS_MAVLink/GCS_serial_control.cpp @@ -118,6 +118,22 @@ void GCS_MAVLINK::handle_serial_control(const mavlink_message_t &msg) port->begin(packet.baudrate); } +//OWSERIALCONTROL + #define SERIAL_CONTROL_FLAG_8N1 0x40 + #define SERIAL_CONTROL_FLAG_8E1 0x80 + + if (exclusive && port != nullptr) { + if (packet.flags & SERIAL_CONTROL_FLAG_8N1) { + port->configure_parity(0); + port->set_stop_bits(1); + } + if (packet.flags & SERIAL_CONTROL_FLAG_8E1) { + port->configure_parity(2); // enable even parity + port->set_stop_bits(1); + } + } +//OWEND + // write the data if (packet.count != 0) { if ((packet.flags & SERIAL_CONTROL_FLAG_BLOCKING) == 0) { From 8df64d18993ccedfb95e8109c1dda6201baa81ba Mon Sep 17 00:00:00 2001 From: olliw42 Date: Fri, 10 Nov 2023 03:10:21 +0100 Subject: [PATCH 039/125] commentary --- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 2 +- libraries/bp_version.h | 9 ++++++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index d15f160a1c992..1a280f3e985c5 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -994,7 +994,7 @@ estimator status: /* landed state: GCS_Common.cpp: virtual MAV_LANDED_STATE landed_state() const { return MAV_LANDED_STATE_UNDEFINED; } - But protected so we can access it. Hence either (1) move to public, (2) add public getter to gcs class, + But protected so we can't access it. Hence either (1) move to public, (2) add public getter to gcs class, or (3) add it to vehicle. Latter is most work but nicest, IMHO. Copter has it: GCS_MAVLINK_Copter::landed_state(), yields ON_GROUND, TAKEOFF, IN_AIR, LANDING Plane has it: GCS_MAVLINK_Plane::landed_state(), only yields ON_GROUND or IN_AIR diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 903a7176fa6d7..2fae0c84e28cc 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -6,8 +6,15 @@ /* search for //OW to find all changes +on-top features: +- RADIO_LINK +- AP_Arming: immediate report +- AP_RCTelemetry: CRSF statustext throttle +- GCS_serial_control: SERIAL_CONTROL 8E1 +- RC_Channel, AUX_FUNC: RETRACT_MOUNT1_3POS (eq RETRACT_MOUNT1), CAMERA_SET_MODE, CAMERA_TRIG_MODE (eq CAM_MODE_TOGGLE) + 2023.11.06: v059 - upgraded to ArduPilot master 4.5.0-dev + upgraded to ArduPilot master 4.5.0-dev, mount revised significantly 2023.03.10: v058 upgraded to Copter4.3.6 stable From c2cd0e9fad2d45e281a4d48eb2fdb8bee54f8793 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Mon, 13 Nov 2023 21:31:19 +0100 Subject: [PATCH 040/125] renames --- libraries/AP_Mount/AP_Mount.cpp | 4 +- libraries/AP_Mount/AP_Mount.h | 2 +- libraries/AP_Mount/AP_Mount_Backend.h | 2 +- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 44 +++++++++---------- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 6 ++- libraries/GCS_MAVLink/GCS_Common.cpp | 2 +- libraries/bp_version.h | 8 ++-- 7 files changed, 33 insertions(+), 35 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 5fb455ce2f07a..fe20abf0e5356 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -1123,11 +1123,11 @@ bool AP_Mount::cam_do_photo_video_mode(uint8_t instance, PhotoVideoMode photo_vi return backend->cam_do_photo_video_mode(photo_video_mode); } -void AP_Mount::handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg) +void AP_Mount::handle_msg_extra(mavlink_channel_t chan, const mavlink_message_t &msg) { for (uint8_t instance=0; instancehandle_msg(msg); + _backends[instance]->handle_msg_extra(msg); } } } diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index cd1c7a386ac5c..1a466cbde4e92 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -287,7 +287,7 @@ class AP_Mount // this is somewhat different to handle_message() in that it catches all messages // with significant work it potentially could be combined, but let's not introduce side effects - void handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg); + void handle_msg_extra(mavlink_channel_t chan, const mavlink_message_t &msg); // send banner void send_banner(void); diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 220e21afd0eb4..23fd28662596f 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -207,7 +207,7 @@ class AP_Mount_Backend virtual bool cam_do_photo_video_mode(PhotoVideoMode photo_video_mode) { return false; } // handle msg - allows to process a msg from a gimbal - virtual void handle_msg(const mavlink_message_t &msg) {} + virtual void handle_msg_extra(const mavlink_message_t &msg) {} // send banner virtual void send_banner(void) {} diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 1a280f3e985c5..947cafa8cd255 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -23,12 +23,12 @@ extern const AP_HAL::HAL& hal; //****************************************************** // Quaternion & Euler for Gimbal //****************************************************** -// It is inappropriate to use NED (roll-pitch-yaw) to convert received quaternion to Euler angles and vice versa. -// For most gimbals pitch-roll-yaw is appropriate. -// When the roll angle is zero, both are equivalent, which should be the majority of cases anyhow. -// The issue with NED is the gimbal lock at pitch +-90 deg, but pitch +-90 deg is a common operation point for -// gimbals. -// Tthe angles in this driver are thus pitch-roll-yaw Euler. +// It is inappropriate to use NED (roll-pitch-yaw) to convert received quaternion to Euler angles +// and vice versa. For most gimbals pitch-roll-yaw is appropriate. When the roll angle is zero, +// both are equivalent, which should be the majority of cases. +// The issue with NED is the gimbal lock at pitch +-90 deg, but pitch +-90 deg is a common +// operation point for gimbals. +// The angles in this driver are thus pitch-roll-yaw Euler. class GimbalQuaternion : public Quaternion { @@ -165,7 +165,7 @@ void BP_Mount_STorM32_MAVLink::init() _mount_status = {}; _device_status = {}; - flags_for_gimbal = 0; + _flags_for_gimbal = 0; _current_angles = {0.0f, 0.0f, 0.0f, NAN}; // the NAN is important! _script_control_angles = {}; @@ -271,27 +271,27 @@ bool BP_Mount_STorM32_MAVLink::handle_gimbal_manager_flags(uint32_t flags) void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags(enum MAV_MOUNT_MODE mntmode) { - flags_for_gimbal = 0; + _flags_for_gimbal = 0; switch (mntmode) { case MAV_MOUNT_MODE_RETRACT: - flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RETRACT; + _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RETRACT; break; case MAV_MOUNT_MODE_NEUTRAL: - flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_NEUTRAL; + _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_NEUTRAL; break; default: break; } // we currently only support pitch,roll lock, not pitch,roll follow - flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK; + _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK; // we currently do not support yaw lock -// if (_is_yaw_lock) flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_LOCK; +// if (_is_yaw_lock) _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_LOCK; // set either YAW_IN_VEHICLE_FRAME or YAW_IN_EARTH_FRAME, to indicate new message format, STorM32 will reject otherwise - flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME; + _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME; } @@ -730,7 +730,7 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_attitude_status(const mavlin } -void BP_Mount_STorM32_MAVLink::handle_msg(const mavlink_message_t &msg) +void BP_Mount_STorM32_MAVLink::handle_msg_extra(const mavlink_message_t &msg) { if (_protocol == PROTOCOL_UNDEFINED) { // implies !_initialised && _compid determine_protocol(msg); @@ -849,7 +849,7 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude(void) mavlink_msg_gimbal_device_set_attitude_send( _chan, _sysid, _compid, - flags_for_gimbal, // gimbal device flags + _flags_for_gimbal, // gimbal device flags qa, // attitude as a quaternion NAN, NAN, NAN // angular velocities ); @@ -858,7 +858,7 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude(void) BP_LOG("MTC0", BP_LOG_MTC_GIMBALCONTROL_HEADER, (uint8_t)1, // GIMBAL_DEVICE_SET_ATTITUDE degrees(mnt_target.angle_rad.roll), degrees(mnt_target.angle_rad.pitch), degrees(target_yaw_bf), - (uint16_t)flags_for_gimbal, (uint16_t)0, + (uint16_t)_flags_for_gimbal, (uint16_t)0, (uint8_t)mnt_target.target_type, (uint8_t)0); } @@ -977,13 +977,12 @@ estimator status: uint16_t estimator_status = 0; - static uint32_t tahrs_healthy_ms = 0; const bool ahrs_healthy = ahrs.healthy(); // it's a bit costly - if (!tahrs_healthy_ms && ahrs_healthy) { - tahrs_healthy_ms = AP_HAL::millis(); + if (!_tahrs_healthy_ms && ahrs_healthy) { + _tahrs_healthy_ms = AP_HAL::millis(); } - if (ahrs_healthy && (nav_estimator_status & ESTIMATOR_ATTITUDE) && ((AP_HAL::millis() - tahrs_healthy_ms) > 3000)) { + if (ahrs_healthy && (nav_estimator_status & ESTIMATOR_ATTITUDE) && ((AP_HAL::millis() - _tahrs_healthy_ms) > 3000)) { estimator_status |= ESTIMATOR_ATTITUDE; // -> QFix if (ahrs.initialised() && (nav_estimator_status & ESTIMATOR_VELOCITY_VERT) && (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D)) { estimator_status |= ESTIMATOR_VELOCITY_VERT; // -> AHRSFix @@ -1225,10 +1224,7 @@ bool BP_Mount_STorM32_MAVLink::get_attitude_quaternion(Quaternion &att_quat) return false; } - if (_protocol != PROTOCOL_GIMBAL_DEVICE) { // not supported if not gimbal device - return false; - } - + // we set roll to zero since wrong Euler's att_quat.from_euler(0.0f, _current_angles.pitch, _current_angles.yaw_bf); return true; } diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 8aafc18106f6d..6283a3f4356b1 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -111,7 +111,7 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) override; // added: handle msg - allows to process a msg from a gimbal - void handle_msg(const mavlink_message_t &msg) override; + void handle_msg_extra(const mavlink_message_t &msg) override; // added: send banner void send_banner(void) override; @@ -228,7 +228,7 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend uint32_t received_tlast_ms; // time last GIMBAL_DEVICE_ATTITUDE_STATUS was received (used for health reporting) } _device_status; - uint16_t flags_for_gimbal; // flags to be send to gimbal device + uint16_t _flags_for_gimbal; // flags to be send to gimbal device struct { float roll; @@ -261,7 +261,9 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // source sysid & compid as its associated gimbal manager's sysid & compid. void send_gimbal_device_set_attitude(void); + uint32_t _tahrs_healthy_ms; void send_autopilot_state_for_gimbal_device(void); + void send_rc_channels(void); uint32_t _send_system_time_tlast_ms; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 0f94d7de11a3b..ec575465969f1 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1732,7 +1732,7 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status, //OW #if HAL_MOUNT_ENABLED AP_Mount *mount = AP::mount(); - if (mount != nullptr) mount->handle_msg(chan, msg); + if (mount != nullptr) mount->handle_msg_extra(chan, msg); #endif //OWEND handleMessage(msg); diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 2fae0c84e28cc..0a5dac01977ba 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -7,12 +7,12 @@ search for //OW to find all changes on-top features: -- RADIO_LINK +- RADIO_LINK (OWRADIOLINK) - AP_Arming: immediate report -- AP_RCTelemetry: CRSF statustext throttle -- GCS_serial_control: SERIAL_CONTROL 8E1 +- AP_RCTelemetry: CRSF statustext throttle (OWCRSF) +- GCS_serial_control: SERIAL_CONTROL 8E1 (OWSERIALCONTORL) - RC_Channel, AUX_FUNC: RETRACT_MOUNT1_3POS (eq RETRACT_MOUNT1), CAMERA_SET_MODE, CAMERA_TRIG_MODE (eq CAM_MODE_TOGGLE) - +- 2023.11.06: v059 upgraded to ArduPilot master 4.5.0-dev, mount revised significantly From 8a7547e686bdcf135b20ff07968fb7b75f733d55 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 14 Nov 2023 01:37:10 +0100 Subject: [PATCH 041/125] sync --- libraries/bp_version.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 0a5dac01977ba..778a2d738eb56 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,7 +1,7 @@ #pragma once #define BETAPILOTVERSION "v059b" -#define DATEOFBASEBRANCH "20231109" +#define DATEOFBASEBRANCH "20231114" /* search for //OW to find all changes @@ -9,7 +9,6 @@ search for //OW to find all changes on-top features: - RADIO_LINK (OWRADIOLINK) - AP_Arming: immediate report -- AP_RCTelemetry: CRSF statustext throttle (OWCRSF) - GCS_serial_control: SERIAL_CONTROL 8E1 (OWSERIALCONTORL) - RC_Channel, AUX_FUNC: RETRACT_MOUNT1_3POS (eq RETRACT_MOUNT1), CAMERA_SET_MODE, CAMERA_TRIG_MODE (eq CAM_MODE_TOGGLE) - From 50eb702d48d644a407a595b1752a70d0dc673e53 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 14 Nov 2023 09:54:50 +0100 Subject: [PATCH 042/125] more tidy --- libraries/AP_Arming/AP_Arming.cpp | 6 +++--- libraries/AP_Arming/AP_Arming.h | 2 +- libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp | 2 +- libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h | 2 +- libraries/GCS_MAVLink/GCS_serial_control.cpp | 2 +- libraries/bp_version.h | 6 +++--- 6 files changed, 10 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index dd88305730bdd..dbe22e76a8098 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -183,7 +183,7 @@ void AP_Arming::update(void) const uint32_t now_ms = AP_HAL::millis(); // perform pre-arm checks & display failures every 30 seconds bool display_fail = false; -//OW +//OW ARM // if (now_ms - last_prearm_display_ms > PREARM_DISPLAY_PERIOD*1000) { if ((report_immediately && (now_ms - last_prearm_display_ms > 750)) || (now_ms - last_prearm_display_ms > PREARM_DISPLAY_PERIOD*1000)) { @@ -1544,7 +1544,7 @@ bool AP_Arming::pre_arm_checks(bool report) return true; } #endif -//OW +//OW ARM // return hardware_safety_check(report) bool checks_result = hardware_safety_check(report) //OWEND @@ -1583,7 +1583,7 @@ bool AP_Arming::pre_arm_checks(bool report) & serial_protocol_checks(report) & estop_checks(report); -//OW +//OW ARM if (!checks_result && last_prearm_checks_result) { // check went from true to false report_immediately = true; } diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index dd08041a232ab..f38427ce86ec3 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -304,7 +304,7 @@ class AP_Arming { uint32_t last_prearm_display_ms; // last time we send statustexts for prearm failures bool running_arming_checks; // true if the arming checks currently being performed are being done because the vehicle is trying to arm the vehicle -//OW +//OW ARM bool last_prearm_checks_result; bool report_immediately; //OWEND diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp index 1b42ccca98131..f113a72aeaa2d 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp @@ -1,5 +1,5 @@ //***************************************************** -//OW +//OW RADIOLINK // (c) olliw, www.olliw.eu, GPL3 // for mLRS //***************************************************** diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h index c7b0a7fed1dbb..74efd5040c4cd 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h @@ -1,5 +1,5 @@ //***************************************************** -//OW +//OW RADIOLINK // (c) olliw, www.olliw.eu, GPL3 // for mLRS //***************************************************** diff --git a/libraries/GCS_MAVLink/GCS_serial_control.cpp b/libraries/GCS_MAVLink/GCS_serial_control.cpp index 149dffae9cb0b..f538d64648302 100644 --- a/libraries/GCS_MAVLink/GCS_serial_control.cpp +++ b/libraries/GCS_MAVLink/GCS_serial_control.cpp @@ -118,7 +118,7 @@ void GCS_MAVLINK::handle_serial_control(const mavlink_message_t &msg) port->begin(packet.baudrate); } -//OWSERIALCONTROL +//OW SERIALCONTROL #define SERIAL_CONTROL_FLAG_8N1 0x40 #define SERIAL_CONTROL_FLAG_8E1 0x80 diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 778a2d738eb56..eca5cf9f14df6 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -7,9 +7,9 @@ search for //OW to find all changes on-top features: -- RADIO_LINK (OWRADIOLINK) -- AP_Arming: immediate report -- GCS_serial_control: SERIAL_CONTROL 8E1 (OWSERIALCONTORL) +- RADIO_LINK (OW RADIOLINK) +- AP_Arming: immediate report (OWARM) +- GCS_serial_control: SERIAL_CONTROL 8E1 (OW SERIALCONTROL) - RC_Channel, AUX_FUNC: RETRACT_MOUNT1_3POS (eq RETRACT_MOUNT1), CAMERA_SET_MODE, CAMERA_TRIG_MODE (eq CAM_MODE_TOGGLE) - 2023.11.06: v059 From 7a2dba8dc0521dcbbd26b3a3342633926dab5780 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 14 Nov 2023 11:39:26 +0100 Subject: [PATCH 043/125] un-void send_banner() --- libraries/AP_Mount/AP_Mount.cpp | 2 +- libraries/AP_Mount/AP_Mount.h | 2 +- libraries/AP_Mount/AP_Mount_Backend.h | 2 +- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 2 +- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 3 +-- 5 files changed, 5 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index fe20abf0e5356..2627ed792dd9a 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -1132,7 +1132,7 @@ void AP_Mount::handle_msg_extra(mavlink_channel_t chan, const mavlink_message_t } } -void AP_Mount::send_banner(void) +void AP_Mount::send_banner() { for (uint8_t instance=0; instance Date: Tue, 14 Nov 2023 12:13:17 +0100 Subject: [PATCH 044/125] simplify mount status fake --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 45 +++---------------- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 11 +++-- 2 files changed, 12 insertions(+), 44 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index d246b64dae982..d24fb8e659184 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -725,8 +725,6 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_attitude_status(const mavlin if (payload.target_system) { // send MOUNT_STATUS to ground only if target_sysid = 0 return; } - - send_mount_status_to_ground(); } @@ -771,7 +769,6 @@ void BP_Mount_STorM32_MAVLink::handle_msg_extra(const mavlink_message_t &msg) _current_angles.roll = radians((float)payload.pointing_b * 0.01f); _current_angles.yaw_bf = radians((float)payload.pointing_c * 0.01f); _current_angles.delta_yaw = NAN; - send_mount_status_to_ground(); // this is what MissionPlanner wants ... break; } } } @@ -1362,46 +1359,18 @@ void BP_Mount_STorM32_MAVLink::send_cmd_do_digicam_control(bool shoot) // MAVLink mount status forwarding //------------------------------------------------------ -// forward a MOUNT_STATUS message to ground, this is only to make MissionPlanner and alike happy -void BP_Mount_STorM32_MAVLink::send_mount_status_to_ground(void) +// send a MOUNT_STATUS message to GCS, this is only to make MissionPlanner and alike happy +void BP_Mount_STorM32_MAVLink::send_gimbal_device_attitude_status(mavlink_channel_t chan) { - // space is checked by send_to_ground() - - const mavlink_mount_status_t pkt = { + mavlink_msg_mount_status_send( + chan, + 0, // uint8_t target_system + 0, // uint8_t target_component (int32_t)(degrees(_current_angles.pitch) * 100.0f), // int32_t pointing_a (int32_t)(degrees(_current_angles.roll) * 100.0f), // int32_t pointing_b (int32_t)(degrees(_current_angles.yaw_bf) * 100.0f), // int32_t pointing_c - 0, // uint8_t target_system - 0, // uint8_t target_component get_mode() // uint8_t mount_mode - }; - - send_to_ground(MAVLINK_MSG_ID_MOUNT_STATUS, (const char*)&pkt); -} - - -// this is essentially GCS::send_to_active_channels(uint32_t msgid, const char *pkt) -// but exempts the gimbal channel -// It assumes that the gimbal and any GCS are not on the same link, which may not be the -// case in all and every situation but should a pretty fair assumption. -void BP_Mount_STorM32_MAVLink::send_to_ground(uint32_t msgid, const char *pkt) -{ - const mavlink_msg_entry_t* entry = mavlink_get_msg_entry(msgid); - if (entry == nullptr) { - return; - } - for (uint8_t i=0; i we thus make it to do nothing. + // MissionPlaner "understands" gimbal device attitude status, but doesn't use it for campoint, so we + // still want to send MOUNT_STATUS. + // => for as long as MissionPlanner doesn't do mount/gimbal status properly + // we use this to stream MOUNT_STATUS // We don't do any of ArduPilot's private mavlink channel nonsense. - void send_gimbal_device_attitude_status(mavlink_channel_t chan) override {} + void send_gimbal_device_attitude_status(mavlink_channel_t chan) override; // send a GIMBAL_MANAGER_INFORMATION message to GCS // AP nonsense: does it wrong, just good for its own limits. @@ -296,9 +299,5 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend bool _should_log; - // mount_status forwarding - // MissionPlaner "understands" gimbal device attitude status, but doesn't use it for campoint, so we still need to send - void send_mount_status_to_ground(void); - void send_to_ground(uint32_t msgid, const char *pkt); }; From eef3c0c58c24497ffdffef67d1584c92a1bb66d0 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 14 Nov 2023 14:38:11 +0100 Subject: [PATCH 045/125] log em --- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 15 ++++++++++----- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 2 +- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index d24fb8e659184..281c9d10341de 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -173,6 +173,8 @@ void BP_Mount_STorM32_MAVLink::init() _got_radio_rc_channels = false; // disable sending rc channels when RADIO_RC_CHANNELS messages are detected _camera_mode = CAMERA_MODE_UNDEFINED; + + _should_log = true; // for now do always log } @@ -853,11 +855,11 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude(void) BP_LOG("MTC0", BP_LOG_MTC_GIMBALCONTROL_HEADER, - (uint8_t)1, // GIMBAL_DEVICE_SET_ATTITUDE - degrees(mnt_target.angle_rad.roll), degrees(mnt_target.angle_rad.pitch), degrees(target_yaw_bf), - (uint16_t)_flags_for_gimbal, (uint16_t)0, - (uint8_t)mnt_target.target_type, - (uint8_t)0); + (uint8_t)1, // Type, GIMBAL_DEVICE_SET_ATTITUDE + degrees(mnt_target.angle_rad.roll), degrees(mnt_target.angle_rad.pitch), degrees(target_yaw_bf), // Roll, Pitch, Yaw + (uint16_t)_flags_for_gimbal, (uint16_t)0, // GDFlags, GMFlags + (uint8_t)mnt_target.target_type, // TMode + (uint8_t)0); // QMode } @@ -1362,6 +1364,9 @@ void BP_Mount_STorM32_MAVLink::send_cmd_do_digicam_control(bool shoot) // send a MOUNT_STATUS message to GCS, this is only to make MissionPlanner and alike happy void BP_Mount_STorM32_MAVLink::send_gimbal_device_attitude_status(mavlink_channel_t chan) { + // space already checked by streamer + // checked space of GIMBAL_DEVICE_ATTITUDE_STATUS, but MOUNT_STATUS is (much) smaller, so no issue + mavlink_msg_mount_status_send( chan, 0, // uint8_t target_system diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 81ac7506a8bd3..57c4c662163b6 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -171,7 +171,7 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend uint8_t _sysid; // sysid of gimbal uint8_t _compid; // component id of gimbal, zero if gimbal not yet discovered mavlink_channel_t _chan; // mavlink channel used to communicate with gimbal - // Comment: in some places the newer _link construct is used, which is however not smart, + // Comment: in some drivers the newer _link construct is used, which is however not smart, // since for each message it does a binary search to find and check the size. This is not // needed with the older _chan construct, which is thus much more efficient cpu wise. The // older _chan also allows for an early test for space avoiding potentially lengthy calcs. From f4df60770d477b640a50ed0b6db73f798fd95e3b Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 14 Nov 2023 16:45:27 +0100 Subject: [PATCH 046/125] undo AP_Arming --- libraries/AP_Arming/AP_Arming.cpp | 21 ++------------------- libraries/AP_Arming/AP_Arming.h | 5 ----- 2 files changed, 2 insertions(+), 24 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index dbe22e76a8098..c0178a1ccfa6d 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -183,12 +183,7 @@ void AP_Arming::update(void) const uint32_t now_ms = AP_HAL::millis(); // perform pre-arm checks & display failures every 30 seconds bool display_fail = false; -//OW ARM -// if (now_ms - last_prearm_display_ms > PREARM_DISPLAY_PERIOD*1000) { - if ((report_immediately && (now_ms - last_prearm_display_ms > 750)) || - (now_ms - last_prearm_display_ms > PREARM_DISPLAY_PERIOD*1000)) { - report_immediately = false; -//OWEND + if (now_ms - last_prearm_display_ms > PREARM_DISPLAY_PERIOD*1000) { display_fail = true; last_prearm_display_ms = now_ms; } @@ -1544,10 +1539,7 @@ bool AP_Arming::pre_arm_checks(bool report) return true; } #endif -//OW ARM -// return hardware_safety_check(report) - bool checks_result = hardware_safety_check(report) -//OWEND + return hardware_safety_check(report) #if HAL_HAVE_IMU_HEATER & heater_min_temperature_checks(report) #endif @@ -1582,15 +1574,6 @@ bool AP_Arming::pre_arm_checks(bool report) & opendroneid_checks(report) & serial_protocol_checks(report) & estop_checks(report); - -//OW ARM - if (!checks_result && last_prearm_checks_result) { // check went from true to false - report_immediately = true; - } - last_prearm_checks_result = checks_result; - - return checks_result; -//OWEND } bool AP_Arming::arm_checks(AP_Arming::Method method) diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index f38427ce86ec3..a755bc0bd04d4 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -303,11 +303,6 @@ class AP_Arming { uint32_t last_prearm_display_ms; // last time we send statustexts for prearm failures bool running_arming_checks; // true if the arming checks currently being performed are being done because the vehicle is trying to arm the vehicle - -//OW ARM - bool last_prearm_checks_result; - bool report_immediately; -//OWEND }; namespace AP { From a8f41cb74f1358ecca78ce6b1cdeb8e52c61101f Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 14 Nov 2023 17:01:55 +0100 Subject: [PATCH 047/125] rework prearm & health checking --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 127 ++++++++++++------ libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 8 +- 2 files changed, 90 insertions(+), 45 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 281c9d10341de..ea9b0836f4de9 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -159,7 +159,7 @@ void BP_Mount_STorM32_MAVLink::init() _gimbal_prearmchecks_ok = false; _armingchecks_enabled = false; - _prearmchecks_done = false; + _prearmchecks_passed = false; _mode = MAV_MOUNT_MODE_RC_TARGETING; // irrelevant, will be later set to default in frontend init() @@ -221,6 +221,11 @@ void BP_Mount_STorM32_MAVLink::update() uint32_t tnow_ms = AP_HAL::millis(); + if ((tnow_ms - _checks_tlast_ms) >= 1000) { + _checks_tlast_ms = tnow_ms; + update_checks(); + } + if ((tnow_ms - _send_system_time_tlast_ms) >= 5000) { // every 5 sec is really plenty _send_system_time_tlast_ms = tnow_ms; send_system_time(); @@ -556,8 +561,59 @@ void BP_Mount_STorM32_MAVLink::send_prearmchecks_txt(void) } +bool BP_Mount_STorM32_MAVLink::is_healthy(void) +{ + if (_protocol == PROTOCOL_GIMBAL_DEVICE) { + + // unhealthy if attitude status is not received within the last second + if ((AP_HAL::millis() - _device_status.received_tlast_ms) > 1000) { + return false; + } + + // check failure flags + // We should also check for GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER + // which means that gimbal did not got GIMBAL_DEVICE_SET_ATTITUDE messages. +// if ((_device_status.received_failure_flags & (FAILURE_FLAGS | GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER)) > 0) { + if ((_device_status.received_failure_flags & FAILURE_FLAGS) > 0) { + return false; + } + + } else { + // unhealthy if mount status is not received within the last second + if ((AP_HAL::millis() - _mount_status.received_tlast_ms) > 1000) { + return false; + } + + // check failure flags // are set since v2.68b + if ((_gimbal_error_flags & FAILURE_FLAGS) > 0) { + return false; + } + } + + return true; +} + + +void BP_Mount_STorM32_MAVLink::update_checks(void) +{ + if (!_armingchecks_enabled || (_prearmchecks_passed && AP::notify().flags.armed)) { + + bool checks = is_healthy(); + + if (_checks_last && !checks) { // checks went from true to false + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "MNT%u: not healthy", _instance+1); + } + if (!_checks_last && checks) { // checks went from false to true + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: healthy again", _instance+1); + } + _checks_last = checks; + + } +} + + // return true if healthy -// this is called when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA is set, else not +// this is called when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA is set, and if disarmed, else not // is called with 1 Hz // workaround to get around that healthy() is const bool BP_Mount_STorM32_MAVLink::healthy() const @@ -570,20 +626,20 @@ bool BP_Mount_STorM32_MAVLink::healthy_nonconst(void) { _armingchecks_enabled = true; // to signal that ArduPilot arming check mechanism is active - return prearmchecks_do(); + _checks_tlast_ms = AP_HAL::millis(); // to sync update_checks() + + return update_prearmchecks(); } -bool BP_Mount_STorM32_MAVLink::prearmchecks_do(void) +bool BP_Mount_STorM32_MAVLink::update_prearmchecks(void) { - // these we do only at startup - if (!_prearmchecks_done) { + // we do these only at startup + if (!_prearmchecks_passed) { - if (_armingchecks_enabled) { // do only if ArduPilot check is enabled - if((AP_HAL::millis() - _prearmcheck_sendtext_tlast_ms) > 30000) { // we haven't send it for a long time - _prearmcheck_sendtext_tlast_ms = AP_HAL::millis(); - send_prearmchecks_txt(); - } + if((AP_HAL::millis() - _prearmcheck_sendtext_tlast_ms) > 30000) { // we haven't send it for a long time + _prearmcheck_sendtext_tlast_ms = AP_HAL::millis(); + send_prearmchecks_txt(); } // unhealthy until gimbal has fully passed the startup sequence @@ -599,39 +655,27 @@ bool BP_Mount_STorM32_MAVLink::prearmchecks_do(void) } // these we do continuously + bool checks = is_healthy(); - if (_protocol == PROTOCOL_GIMBAL_DEVICE) { - - // unhealthy if attitude status is not received within the last second - if ((AP_HAL::millis() - _device_status.received_tlast_ms) > 1000) { - return false; + if (_prearmchecks_passed) { // we are past prearm checks + if (_checks_last && !checks) { // checks went from true to false + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PreArm: Mount: not healthy"); } - - // check failure flags - // We should also check for GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER - // which means that gimbal did not got GIMBAL_DEVICE_SET_ATTITUDE messages. -// if ((_device_status.received_failure_flags & (FAILURE_FLAGS | GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER)) > 0) { - if ((_device_status.received_failure_flags & FAILURE_FLAGS) > 0) { - return false; - } - - } else { - // unhealthy if mount status is not received within the last second - if ((AP_HAL::millis() - _mount_status.received_tlast_ms) > 1000) { - return false; - } - - // check failure flags // are set since v2.68b - if ((_gimbal_error_flags & FAILURE_FLAGS) > 0) { - return false; + if (!_checks_last && checks) { // checks went from false to true + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); } + _checks_last = checks; + return checks; } + _checks_last = checks; + if (!checks) return false; + // if we get this far the mount is healthy // if we got this far the first time we inform the gcs - if (!_prearmchecks_done) { - _prearmchecks_done = true; + if (!_prearmchecks_passed) { + _prearmchecks_passed = true; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); } @@ -757,9 +801,6 @@ void BP_Mount_STorM32_MAVLink::handle_msg_extra(const mavlink_message_t &msg) _gimbal_prearmchecks_ok = true; } _gimbal_error_flags = (payload.custom_mode & 0x00FFFF00) >> 8; - if (!_armingchecks_enabled) { // ArduPilot arming checks disabled, so let's do ourself - if (!_prearmchecks_done) prearmchecks_do(); - } break; } case MAVLINK_MSG_ID_MOUNT_STATUS: { @@ -1114,22 +1155,22 @@ void BP_Mount_STorM32_MAVLink::update_send_banner(void) char c = (_device_info.firmware_version & 0x00FF0000) >> 16; if (c == '\0') c = ' '; else c += 'a' - 1; - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: %s %s v%u.%u%c", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: %s v%u.%u%c", //%s %s v%u.%u%c", _instance + 1, - "", //_device_info.vendor_name, + //_device_info.vendor_name, _device_info.model_name, (unsigned)(_device_info.firmware_version & 0x000000FF), (unsigned)((_device_info.firmware_version & 0x0000FF00) >> 8), c ); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks %s", _instance+1, (_prearmchecks_done) ? "passed" : "fail"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks %s", _instance+1, (_prearmchecks_passed) ? "passed" : "fail"); } else if (_compid) { // we have some info GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: gimbal at %u", _instance+1, _compid); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks %s", _instance+1, (_prearmchecks_done) ? "passed" : "fail"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks %s", _instance+1, (_prearmchecks_passed) ? "passed" : "fail"); } else { // we don't know yet anything diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 57c4c662163b6..f4e65863370f2 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -208,12 +208,16 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend uint32_t _gimbal_error_flags; // error flags in custom_mode field of the HEARTBEAT message (restricted to 16 bits) bool _gimbal_prearmchecks_ok; // true when the gimbal stops reporting prearm fail in the HEARTBEAT message bool _armingchecks_enabled; // true when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA set, we know from healty() - bool _prearmchecks_done; // becomes true when all checks were passed once at startup + bool _prearmchecks_passed; // becomes true when all checks were passed once at startup uint32_t _prearmcheck_sendtext_tlast_ms; + bool _checks_last; // result of last check, to detect toggle from true -> false + uint32_t _checks_tlast_ms; bool healthy_nonconst(void); // workaround to healthy() being const - bool prearmchecks_do(void); + bool update_prearmchecks(void); void send_prearmchecks_txt(void); + void update_checks(void); + bool is_healthy(void); uint32_t _request_send_banner_ms; void update_send_banner(void); From 0a32ac9136b2c7562bc7f7f2eb6e0445f5f9aecd Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 14 Nov 2023 18:11:09 +0100 Subject: [PATCH 048/125] ok --- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 2 +- libraries/bp_version.h | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index f4e65863370f2..3d4b54cdf731d 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -210,7 +210,7 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend bool _armingchecks_enabled; // true when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA set, we know from healty() bool _prearmchecks_passed; // becomes true when all checks were passed once at startup uint32_t _prearmcheck_sendtext_tlast_ms; - bool _checks_last; // result of last check, to detect toggle from true -> false + bool _checks_last; // result of last check, to detect toggle from true -> false, false -> true uint32_t _checks_tlast_ms; bool healthy_nonconst(void); // workaround to healthy() being const diff --git a/libraries/bp_version.h b/libraries/bp_version.h index eca5cf9f14df6..047cadf8f576a 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,6 +1,6 @@ #pragma once -#define BETAPILOTVERSION "v059b" +#define BETAPILOTVERSION "v059c" #define DATEOFBASEBRANCH "20231114" /* @@ -8,7 +8,6 @@ search for //OW to find all changes on-top features: - RADIO_LINK (OW RADIOLINK) -- AP_Arming: immediate report (OWARM) - GCS_serial_control: SERIAL_CONTROL 8E1 (OW SERIALCONTROL) - RC_Channel, AUX_FUNC: RETRACT_MOUNT1_3POS (eq RETRACT_MOUNT1), CAMERA_SET_MODE, CAMERA_TRIG_MODE (eq CAM_MODE_TOGGLE) - From c6284c5bc81b205cfb508004000efc171c4cdd07 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 15 Nov 2023 03:56:30 +0100 Subject: [PATCH 049/125] better workaround to healthy() is const --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 30 ++++++------------- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 23 +++++++------- 2 files changed, 20 insertions(+), 33 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index ea9b0836f4de9..3154be24ed98c 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -152,11 +152,11 @@ void BP_Mount_STorM32_MAVLink::init() _chan = MAVLINK_COMM_0; // this is a dummy, will be set correctly by find_gimbal() _got_device_info = false; - _armed = false; _initialised = false; _protocol = PROTOCOL_UNDEFINED; + _gimbal_armed = false; _gimbal_prearmchecks_ok = false; _armingchecks_enabled = false; _prearmchecks_passed = false; @@ -538,11 +538,11 @@ const uint32_t FAILURE_FLAGS = GIMBAL_DEVICE_ERROR_FLAGS_COMMS_ERROR; -void BP_Mount_STorM32_MAVLink::send_prearmchecks_txt(void) +void BP_Mount_STorM32_MAVLink::send_prearmchecks_txt(void) const { uint32_t failure_flags = (_protocol == PROTOCOL_GIMBAL_DEVICE) ? _device_status.received_failure_flags : _gimbal_error_flags; - if (!_initialised || !_gimbal_prearmchecks_ok || !_armed) { + if (!_initialised || !_gimbal_prearmchecks_ok || !_gimbal_armed) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: arm", _instance+1); } else if (failure_flags & FAILURE_FLAGS) { @@ -561,7 +561,7 @@ void BP_Mount_STorM32_MAVLink::send_prearmchecks_txt(void) } -bool BP_Mount_STorM32_MAVLink::is_healthy(void) +bool BP_Mount_STorM32_MAVLink::is_healthy(void) const { if (_protocol == PROTOCOL_GIMBAL_DEVICE) { @@ -594,6 +594,7 @@ bool BP_Mount_STorM32_MAVLink::is_healthy(void) } +// is called with 1 Hz from main loop void BP_Mount_STorM32_MAVLink::update_checks(void) { if (!_armingchecks_enabled || (_prearmchecks_passed && AP::notify().flags.armed)) { @@ -613,27 +614,14 @@ void BP_Mount_STorM32_MAVLink::update_checks(void) // return true if healthy -// this is called when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA is set, and if disarmed, else not +// this is called when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA is set, and if not armed, else not // is called with 1 Hz -// workaround to get around that healthy() is const bool BP_Mount_STorM32_MAVLink::healthy() const -{ - return const_cast(this)->healthy_nonconst(); // yes, ugly, but I haven't overdesigned the backend -} - - -bool BP_Mount_STorM32_MAVLink::healthy_nonconst(void) { _armingchecks_enabled = true; // to signal that ArduPilot arming check mechanism is active _checks_tlast_ms = AP_HAL::millis(); // to sync update_checks() - return update_prearmchecks(); -} - - -bool BP_Mount_STorM32_MAVLink::update_prearmchecks(void) -{ // we do these only at startup if (!_prearmchecks_passed) { @@ -648,8 +636,8 @@ bool BP_Mount_STorM32_MAVLink::update_prearmchecks(void) // -> device info obtained (_got_device_info = true) // -> status message received, protocol set (_protocol != PROTOCOL_UNDEFINED) // _gimbal_prearmchecks_ok: -> gimbal HB reported gimbal's prearmchecks ok - // _armed: -> gimbal HB reported gimbal is in normal state - if (!_initialised || !_gimbal_prearmchecks_ok || !_armed) { + // _gimbal_armed: -> gimbal HB reported gimbal is in normal state + if (!_initialised || !_gimbal_prearmchecks_ok || !_gimbal_armed) { return false; } } @@ -796,7 +784,7 @@ void BP_Mount_STorM32_MAVLink::handle_msg_extra(const mavlink_message_t &msg) mavlink_heartbeat_t payload; mavlink_msg_heartbeat_decode(&msg, &payload); uint8_t storm32_state = (payload.custom_mode & 0xFF); - _armed = ((storm32_state == STORM32STATE_NORMAL) || (storm32_state == STORM32STATE_STARTUP_FASTLEVEL)); + _gimbal_armed = ((storm32_state == STORM32STATE_NORMAL) || (storm32_state == STORM32STATE_STARTUP_FASTLEVEL)); if ((payload.custom_mode & 0x80000000) == 0) { // we don't follow all changes, but just toggle it to true once _gimbal_prearmchecks_ok = true; } diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 3d4b54cdf731d..2886c02477ac1 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -178,7 +178,6 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // We hence continue to use the _chan construct. bool _got_device_info; // gimbal discovered, waiting for gimbal provide device info - bool _armed; // true once the gimbal has reached normal state bool _initialised; // true once all init steps have been passed bool _got_radio_rc_channels; // true when a RADIO_RC_CHANNELS message has been received @@ -204,20 +203,20 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend void determine_protocol(const mavlink_message_t &msg); // pre-arm and healthy checks + // some need to be made mutable to get around that healthy() is const + bool _gimbal_armed; // true once the gimbal has reached normal state uint32_t _gimbal_error_flags; // error flags in custom_mode field of the HEARTBEAT message (restricted to 16 bits) bool _gimbal_prearmchecks_ok; // true when the gimbal stops reporting prearm fail in the HEARTBEAT message - bool _armingchecks_enabled; // true when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA set, we know from healty() - bool _prearmchecks_passed; // becomes true when all checks were passed once at startup - uint32_t _prearmcheck_sendtext_tlast_ms; - bool _checks_last; // result of last check, to detect toggle from true -> false, false -> true - uint32_t _checks_tlast_ms; - - bool healthy_nonconst(void); // workaround to healthy() being const - bool update_prearmchecks(void); - void send_prearmchecks_txt(void); + mutable bool _armingchecks_enabled; // true when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA set, we know from healty() + mutable bool _prearmchecks_passed; // becomes true when all checks were passed once at startup + mutable uint32_t _prearmcheck_sendtext_tlast_ms; + mutable bool _checks_last; // result of last check, to detect toggle from true -> false, false -> true + mutable uint32_t _checks_tlast_ms; + + void send_prearmchecks_txt(void) const; + bool is_healthy(void) const; void update_checks(void); - bool is_healthy(void); uint32_t _request_send_banner_ms; void update_send_banner(void); @@ -234,7 +233,7 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend uint32_t received_tlast_ms; // time last GIMBAL_DEVICE_ATTITUDE_STATUS was received (used for health reporting) } _device_status; - uint16_t _flags_for_gimbal; // flags to be send to gimbal device + uint16_t _flags_for_gimbal; // flags to be send to gimbal device struct { float roll; From 34f162d5e6881a3729923118cd154d97bdac563a Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 18 Nov 2023 13:13:03 +0100 Subject: [PATCH 050/125] more on checks --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 98 ++++++++++++------- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 19 ++-- 2 files changed, 74 insertions(+), 43 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 3154be24ed98c..a48a1572cd1db 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -158,7 +158,7 @@ void BP_Mount_STorM32_MAVLink::init() _gimbal_armed = false; _gimbal_prearmchecks_ok = false; - _armingchecks_enabled = false; + _armingchecks_running = 0; _prearmchecks_passed = false; _mode = MAV_MOUNT_MODE_RC_TARGETING; // irrelevant, will be later set to default in frontend init() @@ -538,30 +538,27 @@ const uint32_t FAILURE_FLAGS = GIMBAL_DEVICE_ERROR_FLAGS_COMMS_ERROR; -void BP_Mount_STorM32_MAVLink::send_prearmchecks_txt(void) const +bool BP_Mount_STorM32_MAVLink::has_failures(char* s) { uint32_t failure_flags = (_protocol == PROTOCOL_GIMBAL_DEVICE) ? _device_status.received_failure_flags : _gimbal_error_flags; - if (!_initialised || !_gimbal_prearmchecks_ok || !_gimbal_armed) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: arm", _instance+1); - } else - if (failure_flags & FAILURE_FLAGS) { - char txt[255]; - strcpy(txt, ""); - if (failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR) strcat(txt, "mot,"); - if (failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR) strcat(txt, "enc,"); - if (failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR) strcat(txt, "volt,"); - if (txt[0] != '\0') { - txt[strlen(txt)-1] = '\0'; - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: %s", _instance+1, txt); + s[0] = '\0'; + if ((failure_flags & FAILURE_FLAGS) > 0) { + if (failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR) strcat(s, "mot,"); + if (failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR) strcat(s, "enc,"); + if (failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR) strcat(s, "volt,"); + if (s[0] != '\0') { + s[strlen(s)-1] = '\0'; } else { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: err flags", _instance+1); + strcpy(s, "err flags"); } + return true; } + return false; } -bool BP_Mount_STorM32_MAVLink::is_healthy(void) const +bool BP_Mount_STorM32_MAVLink::is_healthy(void) { if (_protocol == PROTOCOL_GIMBAL_DEVICE) { @@ -597,11 +594,20 @@ bool BP_Mount_STorM32_MAVLink::is_healthy(void) const // is called with 1 Hz from main loop void BP_Mount_STorM32_MAVLink::update_checks(void) { - if (!_armingchecks_enabled || (_prearmchecks_passed && AP::notify().flags.armed)) { +char txt[255]; + + if (_armingchecks_running) _armingchecks_running--; // count down + + if (!_armingchecks_running || (_prearmchecks_passed && AP::notify().flags.armed)) { bool checks = is_healthy(); if (_checks_last && !checks) { // checks went from true to false + if (has_failures(txt)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: failures: %s", _instance+1, txt); + } else { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: gimbal lost", _instance+1); + } GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "MNT%u: not healthy", _instance+1); } if (!_checks_last && checks) { // checks went from false to true @@ -610,24 +616,23 @@ void BP_Mount_STorM32_MAVLink::update_checks(void) _checks_last = checks; } -} + if (!_armingchecks_running) { // we don't do anything else if arming mechanims not enabled + return; + _healthy = true; + } -// return true if healthy -// this is called when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA is set, and if not armed, else not -// is called with 1 Hz -bool BP_Mount_STorM32_MAVLink::healthy() const -{ - _armingchecks_enabled = true; // to signal that ArduPilot arming check mechanism is active - - _checks_tlast_ms = AP_HAL::millis(); // to sync update_checks() - - // we do these only at startup + // do these only at startup if (!_prearmchecks_passed) { - if((AP_HAL::millis() - _prearmcheck_sendtext_tlast_ms) > 30000) { // we haven't send it for a long time + if ((AP_HAL::millis() - _prearmcheck_sendtext_tlast_ms) > 30000) { // we haven't send it for a long while _prearmcheck_sendtext_tlast_ms = AP_HAL::millis(); - send_prearmchecks_txt(); + if (!_initialised || !_gimbal_prearmchecks_ok || !_gimbal_armed) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: arm", _instance+1); + } else + if (has_failures(txt)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: %s", _instance+1, txt); + } } // unhealthy until gimbal has fully passed the startup sequence @@ -638,26 +643,36 @@ bool BP_Mount_STorM32_MAVLink::healthy() const // _gimbal_prearmchecks_ok: -> gimbal HB reported gimbal's prearmchecks ok // _gimbal_armed: -> gimbal HB reported gimbal is in normal state if (!_initialised || !_gimbal_prearmchecks_ok || !_gimbal_armed) { - return false; + _healthy = false; + return; } } - // these we do continuously + // do these continuously bool checks = is_healthy(); if (_prearmchecks_passed) { // we are past prearm checks if (_checks_last && !checks) { // checks went from true to false + if (has_failures(txt)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: %s", _instance+1, txt); + } else { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: gimbal lost", _instance+1); + } GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PreArm: Mount: not healthy"); } if (!_checks_last && checks) { // checks went from false to true GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); } _checks_last = checks; - return checks; + _healthy = checks; + return; } _checks_last = checks; - if (!checks) return false; + if (!checks) { + _healthy = false; + return; + } // if we get this far the mount is healthy @@ -667,7 +682,18 @@ bool BP_Mount_STorM32_MAVLink::healthy() const GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); } - return true; + _healthy = true; // report to healthy() +} + + +// return true if healthy +// this is called when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA is set, and if not armed, else not +// is called with 1 Hz +bool BP_Mount_STorM32_MAVLink::healthy() const +{ + _armingchecks_running = 2; // to signal that ArduPilot arming check mechanism is active + + return _healthy; } @@ -1164,6 +1190,8 @@ void BP_Mount_STorM32_MAVLink::update_send_banner(void) // we don't know yet anything GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: no gimbal yet", _instance+1); } + + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: protocol %u", _instance+1, _protocol); } diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 2886c02477ac1..d80061e210866 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -208,14 +208,17 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend bool _gimbal_armed; // true once the gimbal has reached normal state uint32_t _gimbal_error_flags; // error flags in custom_mode field of the HEARTBEAT message (restricted to 16 bits) bool _gimbal_prearmchecks_ok; // true when the gimbal stops reporting prearm fail in the HEARTBEAT message - mutable bool _armingchecks_enabled; // true when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA set, we know from healty() - mutable bool _prearmchecks_passed; // becomes true when all checks were passed once at startup - mutable uint32_t _prearmcheck_sendtext_tlast_ms; - mutable bool _checks_last; // result of last check, to detect toggle from true -> false, false -> true - mutable uint32_t _checks_tlast_ms; - - void send_prearmchecks_txt(void) const; - bool is_healthy(void) const; + + mutable uint8_t _armingchecks_running; // true when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA set, we know from healty() + bool _healthy; + + bool _prearmchecks_passed; // becomes true when all checks were passed once at startup + uint32_t _prearmcheck_sendtext_tlast_ms; + bool _checks_last; // result of last check, to detect toggle from true -> false, false -> true + uint32_t _checks_tlast_ms; + + bool has_failures(char* s); + bool is_healthy(void); void update_checks(void); uint32_t _request_send_banner_ms; From 500a167840f5839a6aca6b566572de47f1f82595 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Thu, 23 Nov 2023 08:21:44 +0100 Subject: [PATCH 051/125] add current omega --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 64 +++++++++++++------ libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 13 +++- libraries/bp_version.h | 2 +- 3 files changed, 56 insertions(+), 23 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index a48a1572cd1db..4ab32fcc89fca 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -526,6 +526,38 @@ bool BP_Mount_STorM32_MAVLink::has_pan_control() const } +//------------------------------------------------------ +// Gimbal attitude and rate +//------------------------------------------------------ + +bool BP_Mount_STorM32_MAVLink::get_attitude_quaternion(Quaternion &att_quat) +{ + if (!_initialised) { + return false; + } + + // we set roll to zero since wrong Euler's + att_quat.from_euler(0.0f, _current_angles.pitch, _current_angles.yaw_bf); + return true; +} + + +bool BP_Mount_STorM32_MAVLink::get_angular_velocity(Vector3f& rates) +{ + if (!_initialised) { + return false; + } + + if (isnan(_current_omega.x) || isnan(_current_omega.y) || isnan(_current_omega.z)) return false; + + rates.x = _current_omega.x; + rates.y = _current_omega.y; + rates.z = _current_omega.z; + + return true; +} + + //------------------------------------------------------ // Prearm & healthy functions //------------------------------------------------------ @@ -722,12 +754,12 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_information(const mavlink_me if (!isnan(_device_info.yaw_min)) _params.yaw_angle_min.set_default(degrees(_device_info.yaw_min)); if (!isnan(_device_info.yaw_max)) _params.yaw_angle_max.set_default(degrees(_device_info.yaw_max)); */ - if (!isnan(_device_info.roll_min)) _params.roll_angle_min.set(degrees(_device_info.roll_min)); - if (!isnan(_device_info.roll_max)) _params.roll_angle_max.set(degrees(_device_info.roll_max)); - if (!isnan(_device_info.pitch_min)) _params.pitch_angle_min.set(degrees(_device_info.pitch_min)); - if (!isnan(_device_info.pitch_max)) _params.pitch_angle_max.set(degrees(_device_info.pitch_max)); - if (!isnan(_device_info.yaw_min)) _params.yaw_angle_min.set(degrees(_device_info.yaw_min)); - if (!isnan(_device_info.yaw_max)) _params.yaw_angle_max.set(degrees(_device_info.yaw_max)); + if (!isnan(_device_info.roll_min)) _params.roll_angle_min.set(degrees(_device_info.roll_min) + 0.5f); + if (!isnan(_device_info.roll_max)) _params.roll_angle_max.set(degrees(_device_info.roll_max) + 0.5f); + if (!isnan(_device_info.pitch_min)) _params.pitch_angle_min.set(degrees(_device_info.pitch_min) + 0.5f); + if (!isnan(_device_info.pitch_max)) _params.pitch_angle_max.set(degrees(_device_info.pitch_max) + 0.5f); + if (!isnan(_device_info.yaw_min)) _params.yaw_angle_min.set(degrees(_device_info.yaw_min) + 0.5f); + if (!isnan(_device_info.yaw_max)) _params.yaw_angle_max.set(degrees(_device_info.yaw_max) + 0.5f); // mark it as having been found _got_device_info = true; @@ -765,6 +797,10 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_attitude_status(const mavlin _current_angles.delta_yaw = payload.delta_yaw; + _current_omega.x = payload.angular_velocity_x; + _current_omega.y = payload.angular_velocity_y; + _current_omega.z = payload.angular_velocity_z; + // logging BP_LOG("MTS0", BP_LOG_MTS_ATTITUDESTATUS_HEADER, @@ -826,6 +862,7 @@ void BP_Mount_STorM32_MAVLink::handle_msg_extra(const mavlink_message_t &msg) _current_angles.roll = radians((float)payload.pointing_b * 0.01f); _current_angles.yaw_bf = radians((float)payload.pointing_c * 0.01f); _current_angles.delta_yaw = NAN; + _current_omega = { NAN, NAN, NAN }; break; } } } @@ -1249,7 +1286,7 @@ uint32_t BP_Mount_STorM32_MAVLink::get_gimbal_manager_flags() const //------------------------------------------------------ -// Scripting accessors, and get attitude fakery +// Scripting accessors //------------------------------------------------------ // return target location if available @@ -1273,19 +1310,6 @@ void BP_Mount_STorM32_MAVLink::set_attitude_euler(float roll_deg, float pitch_de } -// get attitude as a quaternion. Returns true on success -bool BP_Mount_STorM32_MAVLink::get_attitude_quaternion(Quaternion &att_quat) -{ - if (!_initialised) { - return false; - } - - // we set roll to zero since wrong Euler's - att_quat.from_euler(0.0f, _current_angles.pitch, _current_angles.yaw_bf); - return true; -} - - //------------------------------------------------------ // Camera //------------------------------------------------------ diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index d80061e210866..fbd2f6ac53c95 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -24,7 +24,7 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend void update() override; // used for gimbals that need to read INS data at full rate - void update_fast() override {} + //not used void update_fast() override {} // return true if healthy // Is called by mount pre_arm_checks() (and nowhere else), which in turn is called @@ -61,9 +61,16 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // - AP_Mount_Backend::write_log() via calling AP_Mount::get_attitude_euler() // - scripts via calling AP_Mount::get_attitude_euler() // AP nonsense: uses inappropriate Euler angles. - // => we set roll to zero, to minimize harm. + // => set roll to zero, to minimize harm bool get_attitude_quaternion(Quaternion &att_quat) override; + // get angular velocity of mount. Only available on some backends + // This is used in + // - AP_Mount_Backend::send_gimbal_device_attitude_status() + // => not needed, but is supplied since the data may be available + bool get_angular_velocity(Vector3f& rates) override; + + // set yaw_lock. If true, the gimbal's yaw target is maintained in earth-frame meaning // it will lock onto an earth-frame heading (e.g. North). If false (aka "follow") the gimbal's yaw // is maintained in body-frame meaning the gimbal will rotate with the vehicle. @@ -245,6 +252,8 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend float delta_yaw; } _current_angles; // current angles, obtained from either MOUNT_STATUS or GIMBAL_DEVICE_ATTITUDE_STATUS + Vector3f _current_omega; // current angular velocities, obtained from GIMBAL_DEVICE_ATTITUDE_STATUS + struct { float roll; float pitch; diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 047cadf8f576a..42b67318d5aae 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,7 +1,7 @@ #pragma once #define BETAPILOTVERSION "v059c" -#define DATEOFBASEBRANCH "20231114" +#define DATEOFBASEBRANCH "20231118" /* search for //OW to find all changes From 26608db26e5d1e2642c6177ec16923132f971784 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Fri, 24 Nov 2023 10:50:35 +0100 Subject: [PATCH 052/125] cc --- libraries/bp_version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 42b67318d5aae..ebb5fb5f8aae0 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,7 +1,7 @@ #pragma once #define BETAPILOTVERSION "v059c" -#define DATEOFBASEBRANCH "20231118" +#define DATEOFBASEBRANCH "20231124" /* search for //OW to find all changes From eb080ab42ef333ddb23f214f311edc3ba6eeef5f Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 09:08:10 +0100 Subject: [PATCH 053/125] set mode 3pos --- libraries/AP_Mount/AP_Mount.cpp | 14 ++++++++++++++ libraries/AP_Mount/AP_Mount.h | 7 +++++++ libraries/AP_Mount/AP_Mount_Backend.cpp | 19 +++++++++++++++++++ libraries/AP_Mount/AP_Mount_Backend.h | 11 +++++++++++ libraries/RC_Channel/RC_Channel.cpp | 16 +--------------- libraries/RC_Channel/RC_Channel.h | 8 -------- 6 files changed, 52 insertions(+), 23 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 2627ed792dd9a..47f0b9c106105 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -260,6 +260,20 @@ void AP_Mount::set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode) backend->set_mode(mode); } +//OW +// set_mode_3pos - sets the mount's retract or default mode from an aux switch +void AP_Mount::set_mode_3pos(uint8_t instance, uint8_t ch_flag) +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return; + } + + // call backend's set_mode_3pos + backend->set_mode_3pos(ch_flag); +} +//OWEND + // set yaw_lock. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North) // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle void AP_Mount::set_yaw_lock(uint8_t instance, bool yaw_lock) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index de22b4e8fa848..1e7a1597f35b3 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -160,6 +160,13 @@ class AP_Mount void set_mode_to_default() { set_mode_to_default(_primary); } void set_mode_to_default(uint8_t instance); +//OW + // set_mode_3pos - sets the mount's retract or default mode from an aux switch + // ch_flag 0 = LOW enters default mode, 1 = MIDDLE return to previous mode, 2 = HIGH enter retract mode + void set_mode_3pos(uint8_t ch_flag) { set_mode_3pos(_primary, ch_flag); } + void set_mode_3pos(uint8_t instance, uint8_t ch_flag); +//OWEND + // set yaw_lock. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North) // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle void set_yaw_lock(bool yaw_lock) { set_yaw_lock(_primary, yaw_lock); } diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 6b39667623ffd..d480a5d30b344 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -847,4 +847,23 @@ void AP_Mount_Backend::send_warning_to_GCS(const char* warning_str) _last_warning_ms = now_ms; } +//OW +// set_mode_3pos - sets the mount's retract or default mode from an aux switch +void AP_Mount_Backend::set_mode_3pos(uint8_t ch_flag) +{ + switch (ch_flag) { + case 2: // = HIGH + if (get_mode() > MAV_MOUNT_MODE_NEUTRAL) _mode_last = get_mode(); + set_mode(MAV_MOUNT_MODE_RETRACT); + break; + case 1: // = MIDDLE + if (_mode_last > MAV_MOUNT_MODE_NEUTRAL) set_mode(_mode_last); + break; + case 0: // LOW: + set_mode((MAV_MOUNT_MODE)_params.default_mode.get()); + break; + } +} +//OWEND + #endif // HAL_MOUNT_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index c915967bf9220..0478f9f78e7c5 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -79,6 +79,12 @@ class AP_Mount_Backend // set mount's mode bool set_mode(enum MAV_MOUNT_MODE mode); +//OW + // set_mode_3pos - sets the mount's retract or default mode from an aux switch + // ch_flag 0 = LOW enters default mode, 1 = MIDDLE return to previous mode, 2 = HIGH enter retract mode + void set_mode_3pos(uint8_t ch_flag); +//OWEND + // set yaw_lock. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North) // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle //OW @@ -347,6 +353,11 @@ class AP_Mount_Backend uint8_t sysid; uint8_t compid; } mavlink_control_id; + +//OW + // aux switch handling + MAV_MOUNT_MODE _mode_last; // previous mode, used in set_mode_3pos() +//OWEND }; #endif // HAL_MOUNT_ENABLED diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 9b5e229097615..3d6b49af11b68 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -733,9 +733,6 @@ const RC_Channel::LookupTable RC_Channel::lookuptable[] = { { AUX_FUNC::PARACHUTE_3POS,"Parachute3Position"}, { AUX_FUNC::MISSION_RESET,"MissionReset"}, { AUX_FUNC::RETRACT_MOUNT1,"RetractMount1"}, -//OW -// { AUX_FUNC::RETRACT_MOUNT1_3POS,"RetractMount1 3pos"}, -//OWEND { AUX_FUNC::RELAY,"Relay1"}, { AUX_FUNC::MOTOR_ESTOP,"MotorEStop"}, { AUX_FUNC::MOTOR_INTERLOCK,"MotorInterlock"}, @@ -1592,18 +1589,7 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos if (mount == nullptr) { break; } - switch (ch_flag) { - case AuxSwitchPos::HIGH: - if (mount->get_mode(0) > MAV_MOUNT_MODE_NEUTRAL) _mount_mode_last = mount->get_mode(0); - mount->set_mode(0,MAV_MOUNT_MODE_RETRACT); - break; - case AuxSwitchPos::MIDDLE: - if (_mount_mode_last > MAV_MOUNT_MODE_NEUTRAL) mount->set_mode(0, _mount_mode_last); - break; - case AuxSwitchPos::LOW: - mount->set_mode_to_default(0); - break; - } + mount->set_mode_3pos(0, (uint8_t)ch_flag); break; } //OWEND diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index de6d0b4c06cdb..1a17052f86a08 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -6,9 +6,6 @@ #if AP_RC_CHANNEL_ENABLED -//OW -#include -//OWEND #include #include #include @@ -428,11 +425,6 @@ class RC_Channel { static const LookupTable lookuptable[]; #endif -//OW -//#if HAL_MOUNT_ENABLED - enum MAV_MOUNT_MODE _mount_mode_last = MAV_MOUNT_MODE_RETRACT; -//#endif -//OWEND }; From 238ec794603dffb90b601df9a0eba2ad90d5b856 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 09:09:37 +0100 Subject: [PATCH 054/125] no mode for update_fgimbal_device --- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 8 ++++---- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 4ab32fcc89fca..e0802edd17d2a 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -253,7 +253,7 @@ bool BP_Mount_STorM32_MAVLink::handle_gimbal_manager_flags(uint32_t flags) set_mode(MAV_MOUNT_MODE_NEUTRAL); } - update_gimbal_device_flags(get_mode()); + update_gimbal_device_flags(); // we currently do not support LOCK // front-end is digesting GIMBAL_MANAGER_FLAGS_YAW_LOCK to determine yaw_is_earth_frame @@ -276,11 +276,11 @@ bool BP_Mount_STorM32_MAVLink::handle_gimbal_manager_flags(uint32_t flags) } -void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags(enum MAV_MOUNT_MODE mntmode) +void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags(void) { _flags_for_gimbal = 0; - switch (mntmode) { + switch (get_mode()) { case MAV_MOUNT_MODE_RETRACT: _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RETRACT; break; @@ -306,7 +306,7 @@ void BP_Mount_STorM32_MAVLink::send_target_angles(void) { // just send stupidly at 12.5 Hz, no check if get_target_angles() made a change - update_gimbal_device_flags(get_mode()); + update_gimbal_device_flags(); if (mnt_target.target_type == MountTargetType::RATE) { // we ignore it. We may think to just send angle_rad, but if yaw is earth frame diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index fbd2f6ac53c95..e3258b5810c49 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -261,7 +261,7 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend } _script_control_angles; // angles set by script // set the flags for gimbal according to current condition - void update_gimbal_device_flags(enum MAV_MOUNT_MODE mntmode); + void update_gimbal_device_flags(void); // determines the target angles based on mount mode, does the crucial job of controlling void update_target_angles(void); From ff7c3930c867f75eb76b7ec7793ed510dbd61639 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 09:09:55 +0100 Subject: [PATCH 055/125] MOUNT_STATUS only for 1st gimbal --- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index e0802edd17d2a..5b4abb0a1d4ae 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -1448,6 +1448,10 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_device_attitude_status(mavlink_channe // space already checked by streamer // checked space of GIMBAL_DEVICE_ATTITUDE_STATUS, but MOUNT_STATUS is (much) smaller, so no issue + if (_compid != MAV_COMP_ID_GIMBAL) { // do it only for the 1st gimbal + return; + } + mavlink_msg_mount_status_send( chan, 0, // uint8_t target_system From 6bdc8df998f009fd1741125299be74c891bb3755 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 09:11:09 +0100 Subject: [PATCH 056/125] tidy --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 62 ++++++++----------- libraries/GCS_MAVLink/GCS_Common.cpp | 1 + 2 files changed, 27 insertions(+), 36 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 5b4abb0a1d4ae..d5356afb92c00 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -255,7 +255,7 @@ bool BP_Mount_STorM32_MAVLink::handle_gimbal_manager_flags(uint32_t flags) update_gimbal_device_flags(); - // we currently do not support LOCK + // we currently do not support yaw LOCK // front-end is digesting GIMBAL_MANAGER_FLAGS_YAW_LOCK to determine yaw_is_earth_frame // we could make it to modify the flag, but for moment let's be happy. if (flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK) { @@ -298,6 +298,7 @@ void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags(void) // if (_is_yaw_lock) _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_LOCK; // set either YAW_IN_VEHICLE_FRAME or YAW_IN_EARTH_FRAME, to indicate new message format, STorM32 will reject otherwise + _flags_for_gimbal &=~ GIMBAL_MANAGER_FLAGS_YAW_IN_EARTH_FRAME; _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME; } @@ -452,6 +453,7 @@ void BP_Mount_STorM32_MAVLink::find_gimbal(void) } // request GIMBAL_DEVICE_INFORMATION + // STorM32 gives this also for mount mode if (!_got_device_info) { if (tnow_ms - _request_device_info_tlast_ms > 1000) { _request_device_info_tlast_ms = tnow_ms; @@ -538,6 +540,7 @@ bool BP_Mount_STorM32_MAVLink::get_attitude_quaternion(Quaternion &att_quat) // we set roll to zero since wrong Euler's att_quat.from_euler(0.0f, _current_angles.pitch, _current_angles.yaw_bf); + return true; } @@ -593,7 +596,6 @@ bool BP_Mount_STorM32_MAVLink::has_failures(char* s) bool BP_Mount_STorM32_MAVLink::is_healthy(void) { if (_protocol == PROTOCOL_GIMBAL_DEVICE) { - // unhealthy if attitude status is not received within the last second if ((AP_HAL::millis() - _device_status.received_tlast_ms) > 1000) { return false; @@ -606,7 +608,6 @@ bool BP_Mount_STorM32_MAVLink::is_healthy(void) if ((_device_status.received_failure_flags & FAILURE_FLAGS) > 0) { return false; } - } else { // unhealthy if mount status is not received within the last second if ((AP_HAL::millis() - _mount_status.received_tlast_ms) > 1000) { @@ -649,7 +650,7 @@ char txt[255]; } - if (!_armingchecks_running) { // we don't do anything else if arming mechanims not enabled + if (!_armingchecks_running) { // we don't do anything else if arming mechanism not enabled return; _healthy = true; } @@ -784,7 +785,7 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_attitude_status(const mavlin // get relevant data _device_status.received_flags = payload.flags; - // TODO: handle case when received device_flags are not equal to those we send, set with update_gimbal_device_flags_for_gimbal() + // TODO: handle case when received device_flags are not equal to those we send, set with update_gimbal_device_flags() _device_status.received_failure_flags = payload.failure_flags; @@ -811,16 +812,6 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_attitude_status(const mavlin degrees(AP::ahrs().yaw), payload.flags, payload.failure_flags); - - // forward to ground as MOUNT_STATUS message - - if (_compid != MAV_COMP_ID_GIMBAL) { // do it only for the 1st gimbal - return; - } - - if (payload.target_system) { // send MOUNT_STATUS to ground only if target_sysid = 0 - return; - } } @@ -847,7 +838,7 @@ void BP_Mount_STorM32_MAVLink::handle_msg_extra(const mavlink_message_t &msg) mavlink_msg_heartbeat_decode(&msg, &payload); uint8_t storm32_state = (payload.custom_mode & 0xFF); _gimbal_armed = ((storm32_state == STORM32STATE_NORMAL) || (storm32_state == STORM32STATE_STARTUP_FASTLEVEL)); - if ((payload.custom_mode & 0x80000000) == 0) { // we don't follow all changes, but just toggle it to true once + if ((payload.custom_mode & 0x80000000) == 0) { // don't follow all changes, but just toggle it to true once _gimbal_prearmchecks_ok = true; } _gimbal_error_flags = (payload.custom_mode & 0x00FFFF00) >> 8; @@ -881,9 +872,10 @@ void BP_Mount_STorM32_MAVLink::send_cmd_request_gimbal_device_information(void) mavlink_msg_command_long_send( _chan, _sysid, _compid, - MAV_CMD_REQUEST_MESSAGE, // command - 0, // confirmation - MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, 0, 0, 0, 0, 0, 0 // param1 .. param7 + MAV_CMD_REQUEST_MESSAGE, // command + 0, // confirmation + MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, // param1 + 0, 0, 0, 0, 0, 0 // param2 .. param7 ); } @@ -913,7 +905,7 @@ void BP_Mount_STorM32_MAVLink::send_cmd_do_mount_control(void) // called by send_target_angles() -// flags_for_gimbal were just updated, so are correct for sure +// _flags_for_gimbal were just updated, so are correct for sure void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude(void) { if (!HAVE_PAYLOAD_SPACE(_chan, GIMBAL_DEVICE_SET_ATTITUDE)) { @@ -947,11 +939,11 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude(void) BP_LOG("MTC0", BP_LOG_MTC_GIMBALCONTROL_HEADER, - (uint8_t)1, // Type, GIMBAL_DEVICE_SET_ATTITUDE + (uint8_t)1, // Type, GIMBAL_DEVICE_SET_ATTITUDE degrees(mnt_target.angle_rad.roll), degrees(mnt_target.angle_rad.pitch), degrees(target_yaw_bf), // Roll, Pitch, Yaw (uint16_t)_flags_for_gimbal, (uint16_t)0, // GDFlags, GMFlags (uint8_t)mnt_target.target_type, // TMode - (uint8_t)0); // QMode + (uint8_t)0); // QMode } @@ -968,7 +960,7 @@ void BP_Mount_STorM32_MAVLink::send_autopilot_state_for_gimbal_device(void) const AP_AHRS &ahrs = AP::ahrs(); - // get vehicle attidude + // get vehicle attitude Quaternion q; if (!ahrs.get_quaternion(q)) { // it returns a bool, so it's a good idea to consider it q.q1 = q.q2 = q.q3 = q.q4 = NAN; @@ -1095,13 +1087,12 @@ landed state: uint8_t landed_state = (uint8_t)gcs().get_landed_state(); // AP::vehicle()->get_landed_state(); #if APM_BUILD_TYPE(APM_BUILD_ArduCopter) - // for copter we modify the landed state so as to reflect the 2 sec pre-take-off period - // the code below leads to a PREPARING_FOR_TAKEOFF before takeoff, but also after landing! - // for the latter we would have to catch that it was flying, but we don't need to care - // the gimbal will do inits when ON_GROUND, and refresh them when transitioning to PREPARING_FOR_TAKEOFF - // it won't do it for other transitions, so e.g. also not for plane - const AP_Notify ¬ify = AP::notify(); - if ((landed_state == MAV_LANDED_STATE_ON_GROUND) && notify.flags.armed) { + // for copter the landed state is modified such as to reflect the 2 sec pre-take-off period. + // The code below leads to a PREPARING_FOR_TAKEOFF before takeoff, but also after landing! + // For the latter one would have to catch that it was flying, but no need to care as + // the gimbal will do inits when ON_GROUND, and refreshes them when transitioning to PREPARING_FOR_TAKEOFF. + // It won't do it for other transitions, so e.g. also not for plane. + if ((landed_state == MAV_LANDED_STATE_ON_GROUND) && AP::notify().flags.armed) { landed_state = MAV_LANDED_STATE_PREPARING_FOR_TAKEOFF; } #endif @@ -1185,7 +1176,7 @@ void BP_Mount_STorM32_MAVLink::send_rc_channels(void) void BP_Mount_STorM32_MAVLink::send_banner() { - // we postpone sending it by few seconds, to avoid multiple sends + // postpone sending by few seconds, to avoid multiple sends _request_send_banner_ms = AP_HAL::millis(); } @@ -1202,13 +1193,12 @@ void BP_Mount_STorM32_MAVLink::update_send_banner(void) // we have lots of info GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: gimbal at %u", _instance+1, _compid); - // we can convert the firmware version to STorM32 convention + // convert firmware version to STorM32 convention char c = (_device_info.firmware_version & 0x00FF0000) >> 16; if (c == '\0') c = ' '; else c += 'a' - 1; - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: %s v%u.%u%c", //%s %s v%u.%u%c", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: %s v%u.%u%c", _instance + 1, - //_device_info.vendor_name, _device_info.model_name, (unsigned)(_device_info.firmware_version & 0x000000FF), (unsigned)((_device_info.firmware_version & 0x0000FF00) >> 8), @@ -1228,7 +1218,7 @@ void BP_Mount_STorM32_MAVLink::update_send_banner(void) GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: no gimbal yet", _instance+1); } - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: protocol %u", _instance+1, _protocol); +// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: protocol %u", _instance+1, _protocol); } @@ -1279,7 +1269,7 @@ uint32_t BP_Mount_STorM32_MAVLink::get_gimbal_manager_flags() const // ATTENTION: Not all capabilities are supported by this driver, but this // should never be a problem since any third party should strictly adhere - // to the capability flags! + // to the capability flags obtained from the gimbal manager ! return _device_status.received_flags; } diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 09fa39f45df93..b03826b5e2552 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1751,6 +1751,7 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status, } //OW #if HAL_MOUNT_ENABLED + // allow mounts to see all messages for this vehicle AP_Mount *mount = AP::mount(); if (mount != nullptr) mount->handle_msg_extra(chan, msg); #endif From de3c1822b923df9a5b7e0e7d9434d6d1dbe6a8ff Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 09:11:29 +0100 Subject: [PATCH 057/125] range lmits redo --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 40 ++++++++++++------- 1 file changed, 25 insertions(+), 15 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index d5356afb92c00..0de3e3eefd0d2 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -422,6 +422,17 @@ void BP_Mount_STorM32_MAVLink::update_target_angles(void) // we do not know this mode so do nothing break; } + + // account for range limits + // we only do teh yaw axis (should be done by StorM32 supervisor, but doesn't hurt + if (!_got_device_info) return; + + if (!isnan(_device_info.yaw_min) && !isnan(_device_info.yaw_max) && + !(_device_info.cap_flags & GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW)) { + + if (mnt_target.angle_rad.yaw < _device_info.yaw_min) mnt_target.angle_rad.yaw = _device_info.yaw_min; + if (mnt_target.angle_rad.yaw > _device_info.yaw_max) mnt_target.angle_rad.yaw = _device_info.yaw_max; + } } @@ -746,21 +757,20 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_information(const mavlink_me // we could check here for sanity of _device_info.gimbal_device_id, but let's just be happy // set parameter defaults from gimbal information - // Q: why default ?? why not actual value ?? I don't understand this -/* - if (!isnan(_device_info.roll_min)) _params.roll_angle_min.set_default(degrees(_device_info.roll_min)); - if (!isnan(_device_info.roll_max)) _params.roll_angle_max.set_default(degrees(_device_info.roll_max)); - if (!isnan(_device_info.pitch_min)) _params.pitch_angle_min.set_default(degrees(_device_info.pitch_min)); - if (!isnan(_device_info.pitch_max)) _params.pitch_angle_max.set_default(degrees(_device_info.pitch_max)); - if (!isnan(_device_info.yaw_min)) _params.yaw_angle_min.set_default(degrees(_device_info.yaw_min)); - if (!isnan(_device_info.yaw_max)) _params.yaw_angle_max.set_default(degrees(_device_info.yaw_max)); */ - - if (!isnan(_device_info.roll_min)) _params.roll_angle_min.set(degrees(_device_info.roll_min) + 0.5f); - if (!isnan(_device_info.roll_max)) _params.roll_angle_max.set(degrees(_device_info.roll_max) + 0.5f); - if (!isnan(_device_info.pitch_min)) _params.pitch_angle_min.set(degrees(_device_info.pitch_min) + 0.5f); - if (!isnan(_device_info.pitch_max)) _params.pitch_angle_max.set(degrees(_device_info.pitch_max) + 0.5f); - if (!isnan(_device_info.yaw_min)) _params.yaw_angle_min.set(degrees(_device_info.yaw_min) + 0.5f); - if (!isnan(_device_info.yaw_max)) _params.yaw_angle_max.set(degrees(_device_info.yaw_max) + 0.5f); + // Q: why set_default ?? why not actual value ?? allows the user to overwrite em? + + if (!isnan(_device_info.roll_min) && !isnan(_device_info.roll_max)) { + if (degrees(_device_info.roll_min) > _params.roll_angle_min) _params.roll_angle_min.set(degrees(_device_info.roll_min)); + if (degrees(_device_info.roll_max) < _params.roll_angle_max) _params.roll_angle_max.set(degrees(_device_info.roll_max)); + } + if (!isnan(_device_info.pitch_min) && !isnan(_device_info.pitch_max)) { + if (degrees(_device_info.pitch_min) > _params.pitch_angle_min) _params.pitch_angle_min.set(degrees(_device_info.pitch_min)); + if (degrees(_device_info.pitch_max) < _params.pitch_angle_max) _params.pitch_angle_max.set(degrees(_device_info.pitch_max)); + } + if (!isnan(_device_info.yaw_min) && !isnan(_device_info.yaw_max)) { + if (degrees(_device_info.yaw_min) > _params.yaw_angle_min) _params.yaw_angle_min.set(degrees(_device_info.yaw_min)); + if (degrees(_device_info.yaw_max) < _params.yaw_angle_max) _params.yaw_angle_max.set(degrees(_device_info.yaw_max)); + } // mark it as having been found _got_device_info = true; From 8338384b03f921dad2fa3d41de0bcbaf8472b086 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 11:18:10 +0100 Subject: [PATCH 058/125] classify enums --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 54 +++++++++---------- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 19 ++++--- 2 files changed, 36 insertions(+), 37 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 0de3e3eefd0d2..4fb7c906a97af 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -154,7 +154,7 @@ void BP_Mount_STorM32_MAVLink::init() _got_device_info = false; _initialised = false; - _protocol = PROTOCOL_UNDEFINED; + _protocol = Protocol::UNDEFINED; _gimbal_armed = false; _gimbal_prearmchecks_ok = false; @@ -172,7 +172,7 @@ void BP_Mount_STorM32_MAVLink::init() _yaw_lock = false; // can't be currently supported, so we need to ensure this is false. This is important! _got_radio_rc_channels = false; // disable sending rc channels when RADIO_RC_CHANNELS messages are detected - _camera_mode = CAMERA_MODE_UNDEFINED; + _camera_mode = CameraMode::UNDEFINED; _should_log = true; // for now do always log } @@ -317,7 +317,7 @@ void BP_Mount_STorM32_MAVLink::send_target_angles(void) return; } - if (_protocol == PROTOCOL_GIMBAL_DEVICE) { + if (_protocol == Protocol::GIMBAL_DEVICE) { send_gimbal_device_set_attitude(); } else { send_cmd_do_mount_control(); @@ -474,7 +474,7 @@ void BP_Mount_STorM32_MAVLink::find_gimbal(void) } // we don't know yet what we should do - if (_protocol == PROTOCOL_UNDEFINED) { + if (_protocol == Protocol::UNDEFINED) { return; } @@ -490,10 +490,10 @@ void BP_Mount_STorM32_MAVLink::determine_protocol(const mavlink_message_t &msg) switch (msg.msgid) { case MAVLINK_MSG_ID_MOUNT_STATUS: - _protocol = PROTOCOL_MOUNT; + _protocol = Protocol::MOUNT; break; case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS: - _protocol = PROTOCOL_GIMBAL_DEVICE; + _protocol = Protocol::GIMBAL_DEVICE; break; } } @@ -505,10 +505,10 @@ void BP_Mount_STorM32_MAVLink::determine_protocol(const mavlink_message_t &msg) bool BP_Mount_STorM32_MAVLink::has_roll_control() const { - if (_protocol == PROTOCOL_GIMBAL_DEVICE) { + if (_protocol == Protocol::GIMBAL_DEVICE) { return (_device_info.cap_flags & GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS); } - if (_protocol == PROTOCOL_MOUNT) { + if (_protocol == Protocol::MOUNT) { return (_params.roll_angle_min < _params.roll_angle_max); } return false; @@ -517,10 +517,10 @@ bool BP_Mount_STorM32_MAVLink::has_roll_control() const bool BP_Mount_STorM32_MAVLink::has_pitch_control() const { - if (_protocol == PROTOCOL_GIMBAL_DEVICE) { + if (_protocol == Protocol::GIMBAL_DEVICE) { return (_device_info.cap_flags & GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS); } - if (_protocol == PROTOCOL_MOUNT) { + if (_protocol == Protocol::MOUNT) { return (_params.pitch_angle_min < _params.pitch_angle_max); } return false; @@ -529,10 +529,10 @@ bool BP_Mount_STorM32_MAVLink::has_pitch_control() const bool BP_Mount_STorM32_MAVLink::has_pan_control() const { - if (_protocol == PROTOCOL_GIMBAL_DEVICE) { + if (_protocol == Protocol::GIMBAL_DEVICE) { return (_device_info.cap_flags & GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS); } - if (_protocol == PROTOCOL_MOUNT) { + if (_protocol == Protocol::MOUNT) { return yaw_range_valid(); } return false; @@ -586,7 +586,7 @@ const uint32_t FAILURE_FLAGS = bool BP_Mount_STorM32_MAVLink::has_failures(char* s) { - uint32_t failure_flags = (_protocol == PROTOCOL_GIMBAL_DEVICE) ? _device_status.received_failure_flags : _gimbal_error_flags; + uint32_t failure_flags = (_protocol == Protocol::GIMBAL_DEVICE) ? _device_status.received_failure_flags : _gimbal_error_flags; s[0] = '\0'; if ((failure_flags & FAILURE_FLAGS) > 0) { @@ -606,7 +606,7 @@ bool BP_Mount_STorM32_MAVLink::has_failures(char* s) bool BP_Mount_STorM32_MAVLink::is_healthy(void) { - if (_protocol == PROTOCOL_GIMBAL_DEVICE) { + if (_protocol == Protocol::GIMBAL_DEVICE) { // unhealthy if attitude status is not received within the last second if ((AP_HAL::millis() - _device_status.received_tlast_ms) > 1000) { return false; @@ -827,7 +827,7 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_attitude_status(const mavlin void BP_Mount_STorM32_MAVLink::handle_msg_extra(const mavlink_message_t &msg) { - if (_protocol == PROTOCOL_UNDEFINED) { // implies !_initialised && _compid + if (_protocol == Protocol::UNDEFINED) { // implies !_initialised && _compid determine_protocol(msg); return; } @@ -1316,12 +1316,12 @@ void BP_Mount_STorM32_MAVLink::set_attitude_euler(float roll_deg, float pitch_de bool BP_Mount_STorM32_MAVLink::take_picture() { - if (_camera_mode == CAMERA_MODE_UNDEFINED) { - _camera_mode = CAMERA_MODE_PHOTO; + if (_camera_mode == CameraMode::UNDEFINED) { + _camera_mode = CameraMode::PHOTO; send_cmd_do_digicam_configure(false); } - if (_camera_mode != CAMERA_MODE_PHOTO) return false; + if (_camera_mode != CameraMode::PHOTO) return false; send_cmd_do_digicam_control(true); @@ -1333,12 +1333,12 @@ bool BP_Mount_STorM32_MAVLink::take_picture() bool BP_Mount_STorM32_MAVLink::record_video(bool start_recording) { - if (_camera_mode == CAMERA_MODE_UNDEFINED) { - _camera_mode = CAMERA_MODE_VIDEO; + if (_camera_mode == CameraMode::UNDEFINED) { + _camera_mode = CameraMode::VIDEO; send_cmd_do_digicam_configure(true); } - if (_camera_mode != CAMERA_MODE_VIDEO) return false; + if (_camera_mode != CameraMode::VIDEO) return false; send_cmd_do_digicam_control(start_recording); @@ -1350,7 +1350,7 @@ bool BP_Mount_STorM32_MAVLink::record_video(bool start_recording) bool BP_Mount_STorM32_MAVLink::cam_set_mode(bool video_mode) { - _camera_mode = (video_mode) ? CAMERA_MODE_VIDEO : CAMERA_MODE_PHOTO; + _camera_mode = (video_mode) ? CameraMode::VIDEO : CameraMode::PHOTO; send_cmd_do_digicam_configure(video_mode); // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "cam set mode %u", video_mode); @@ -1363,8 +1363,8 @@ bool BP_Mount_STorM32_MAVLink::cam_do_photo_video_mode(PhotoVideoMode photo_vide { if (photo_video_mode == PhotoVideoMode::VIDEO_START) { - if (_camera_mode != CAMERA_MODE_VIDEO) { - _camera_mode = CAMERA_MODE_VIDEO; + if (_camera_mode != CameraMode::VIDEO) { + _camera_mode = CameraMode::VIDEO; send_cmd_do_digicam_configure(true); // set video mode } @@ -1379,15 +1379,15 @@ bool BP_Mount_STorM32_MAVLink::cam_do_photo_video_mode(PhotoVideoMode photo_vide _camera_is_recording = false; } - if (_camera_mode != CAMERA_MODE_PHOTO) { - _camera_mode = CAMERA_MODE_PHOTO; + if (_camera_mode != CameraMode::PHOTO) { + _camera_mode = CameraMode::PHOTO; send_cmd_do_digicam_configure(false); // set photo mode } send_cmd_do_digicam_control(true); // take picture } else { - if (_camera_mode == CAMERA_MODE_VIDEO) { + if (_camera_mode == CameraMode::VIDEO) { send_cmd_do_digicam_control(false); _camera_is_recording = false; } diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index e3258b5810c49..00b1cd4e743bc 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -199,10 +199,10 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // request GIMBAL_DEVICE_INFORMATION, we can get this also if 'old' MOUNT messages are used void send_cmd_request_gimbal_device_information(void); - enum Protocol { - PROTOCOL_UNDEFINED = 0, // we do not yet know - PROTOCOL_MOUNT, // gimbal uses 'old' MOUNT messages - PROTOCOL_GIMBAL_DEVICE, // gimbal is a v2 gimbal device + enum class Protocol { + UNDEFINED = 0, // we do not yet know + MOUNT, // gimbal uses 'old' MOUNT messages + GIMBAL_DEVICE, // gimbal is a v2 gimbal device }; Protocol _protocol; @@ -292,17 +292,16 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend TASK_SLOT0 = 0, TASK_SLOT1, TASK_SLOT2, - TASK_SLOT3, + TASK_SLOT3 }; - uint8_t _task_counter; // camera - enum CameraMode { - CAMERA_MODE_UNDEFINED = 0, // we do not yet know - CAMERA_MODE_PHOTO, - CAMERA_MODE_VIDEO, + enum class CameraMode { + UNDEFINED = 0, // we do not yet know + PHOTO, + VIDEO, }; CameraMode _camera_mode; // current camera mode bool _camera_is_recording; From 8f77937aeb064b3fc45f08cb948e62df93ef9653 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 11:20:25 +0100 Subject: [PATCH 059/125] un void --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 28 +++++++++---------- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 28 +++++++++---------- 2 files changed, 28 insertions(+), 28 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 4fb7c906a97af..024e8b9ed66a8 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -276,7 +276,7 @@ bool BP_Mount_STorM32_MAVLink::handle_gimbal_manager_flags(uint32_t flags) } -void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags(void) +void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags() { _flags_for_gimbal = 0; @@ -303,7 +303,7 @@ void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags(void) } -void BP_Mount_STorM32_MAVLink::send_target_angles(void) +void BP_Mount_STorM32_MAVLink::send_target_angles() { // just send stupidly at 12.5 Hz, no check if get_target_angles() made a change @@ -331,7 +331,7 @@ enum MountModeThisWouldBeGreatToHave { // update_angle_target_from_rate() assumes a 50hz update rate! -void BP_Mount_STorM32_MAVLink::update_target_angles(void) +void BP_Mount_STorM32_MAVLink::update_target_angles() { // update based on mount mode switch (get_mode()) { @@ -440,7 +440,7 @@ void BP_Mount_STorM32_MAVLink::update_target_angles(void) // Gimbal and protocol discovery //------------------------------------------------------ -void BP_Mount_STorM32_MAVLink::find_gimbal(void) +void BP_Mount_STorM32_MAVLink::find_gimbal() { // search for gimbal only until vehicle is armed if (hal.util->get_soft_armed()) { @@ -604,7 +604,7 @@ bool BP_Mount_STorM32_MAVLink::has_failures(char* s) } -bool BP_Mount_STorM32_MAVLink::is_healthy(void) +bool BP_Mount_STorM32_MAVLink::is_healthy() { if (_protocol == Protocol::GIMBAL_DEVICE) { // unhealthy if attitude status is not received within the last second @@ -636,7 +636,7 @@ bool BP_Mount_STorM32_MAVLink::is_healthy(void) // is called with 1 Hz from main loop -void BP_Mount_STorM32_MAVLink::update_checks(void) +void BP_Mount_STorM32_MAVLink::update_checks() { char txt[255]; @@ -873,7 +873,7 @@ void BP_Mount_STorM32_MAVLink::handle_msg_extra(const mavlink_message_t &msg) // MAVLink send functions //------------------------------------------------------ -void BP_Mount_STorM32_MAVLink::send_cmd_request_gimbal_device_information(void) +void BP_Mount_STorM32_MAVLink::send_cmd_request_gimbal_device_information() { if (!HAVE_PAYLOAD_SPACE(_chan, COMMAND_LONG)) { return; @@ -891,7 +891,7 @@ void BP_Mount_STorM32_MAVLink::send_cmd_request_gimbal_device_information(void) // called by send_target_angles() -void BP_Mount_STorM32_MAVLink::send_cmd_do_mount_control(void) +void BP_Mount_STorM32_MAVLink::send_cmd_do_mount_control() { if (!HAVE_PAYLOAD_SPACE(_chan, COMMAND_LONG)) { return; @@ -916,7 +916,7 @@ void BP_Mount_STorM32_MAVLink::send_cmd_do_mount_control(void) // called by send_target_angles() // _flags_for_gimbal were just updated, so are correct for sure -void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude(void) +void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude() { if (!HAVE_PAYLOAD_SPACE(_chan, GIMBAL_DEVICE_SET_ATTITUDE)) { return; @@ -962,7 +962,7 @@ enum LandedStateThisWouldBeGreatToHave { }; -void BP_Mount_STorM32_MAVLink::send_autopilot_state_for_gimbal_device(void) +void BP_Mount_STorM32_MAVLink::send_autopilot_state_for_gimbal_device() { if (!HAVE_PAYLOAD_SPACE(_chan, AUTOPILOT_STATE_FOR_GIMBAL_DEVICE)) { return; @@ -1047,7 +1047,7 @@ estimator status: ESTIMATOR_VELOCITY_VERT are set however even if there is no gps or alike! I find it hard to trust the data => we fake it so: for ESTIMATOR_ATTITUDE we delay the flag being raised by few secs - for ESTIMATOR_VELOCITY_VERT we check for gps like in AP_AHRS_DCM::groundspeed_vector(void) + for ESTIMATOR_VELOCITY_VERT we check for gps like in AP_AHRS_DCM::groundspeed_vector() btw STorM32 does also check for non-zero velocities for AHRSFix */ @@ -1141,7 +1141,7 @@ landed state: } -void BP_Mount_STorM32_MAVLink::send_system_time(void) +void BP_Mount_STorM32_MAVLink::send_system_time() { if (!HAVE_PAYLOAD_SPACE(_chan, SYSTEM_TIME)) { return; @@ -1160,7 +1160,7 @@ void BP_Mount_STorM32_MAVLink::send_system_time(void) } -void BP_Mount_STorM32_MAVLink::send_rc_channels(void) +void BP_Mount_STorM32_MAVLink::send_rc_channels() { if (!HAVE_PAYLOAD_SPACE(_chan, RC_CHANNELS)) { return; @@ -1191,7 +1191,7 @@ void BP_Mount_STorM32_MAVLink::send_banner() } -void BP_Mount_STorM32_MAVLink::update_send_banner(void) +void BP_Mount_STorM32_MAVLink::update_send_banner() { if (!_request_send_banner_ms) return; // no request diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 00b1cd4e743bc..092ff2048a014 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -191,13 +191,13 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // gimbal and protocol discovery // search for gimbal in GCS_MAVLink routing table - void find_gimbal(void); + void find_gimbal(); uint32_t _request_device_info_tlast_ms; mavlink_gimbal_device_information_t _device_info; // request GIMBAL_DEVICE_INFORMATION, we can get this also if 'old' MOUNT messages are used - void send_cmd_request_gimbal_device_information(void); + void send_cmd_request_gimbal_device_information(); enum class Protocol { UNDEFINED = 0, // we do not yet know @@ -225,11 +225,11 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend uint32_t _checks_tlast_ms; bool has_failures(char* s); - bool is_healthy(void); - void update_checks(void); + bool is_healthy(); + void update_checks(); uint32_t _request_send_banner_ms; - void update_send_banner(void); + void update_send_banner(); // gimbal target & control @@ -260,31 +260,31 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend float yaw_bf; } _script_control_angles; // angles set by script - // set the flags for gimbal according to current condition - void update_gimbal_device_flags(void); + // set the flags for gimbal according to current conditions + void update_gimbal_device_flags(); // determines the target angles based on mount mode, does the crucial job of controlling - void update_target_angles(void); + void update_target_angles(); // sends the target angles to gimbal, called in update loop with 12.5 Hz - void send_target_angles(void); + void send_target_angles(); // send do_mount_control command with latest angle targets to the gimbal - void send_cmd_do_mount_control(void); + void send_cmd_do_mount_control(); // send GIMBAL_DEVICE_SET_ATTITUDE with latest angle targets to the gimbal to control attitude // When the gimbal receives this message, it can assume it is coming from its gimbal manager // (as nobody else is allowed to send this to it). STorM32 thus identifies the message's // source sysid & compid as its associated gimbal manager's sysid & compid. - void send_gimbal_device_set_attitude(void); + void send_gimbal_device_set_attitude(); uint32_t _tahrs_healthy_ms; - void send_autopilot_state_for_gimbal_device(void); + void send_autopilot_state_for_gimbal_device(); - void send_rc_channels(void); + void send_rc_channels(); uint32_t _send_system_time_tlast_ms; - void send_system_time(void); + void send_system_time(); // task From 791d5c8153cf9f0c1c03261202924aaece0fd644 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 11:21:03 +0100 Subject: [PATCH 060/125] handle msg extra down to mount --- libraries/AP_Mount/AP_Mount.cpp | 4 ++++ libraries/AP_Mount/AP_Mount.h | 2 +- libraries/GCS_MAVLink/GCS_Common.cpp | 18 +++++++++++------- 3 files changed, 16 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 47f0b9c106105..c714edb12de2f 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -948,6 +948,10 @@ void AP_Mount::handle_gimbal_report(mavlink_channel_t chan, const mavlink_messag void AP_Mount::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) { +//OW + handle_msg_extra(chan, msg); +//OWEND + switch (msg.msgid) { case MAVLINK_MSG_ID_GIMBAL_REPORT: handle_gimbal_report(chan, msg); diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 1e7a1597f35b3..ce667c7e70a00 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -293,7 +293,7 @@ class AP_Mount bool cam_do_photo_video_mode(uint8_t instance, PhotoVideoMode photo_video_mode); // this is somewhat different to handle_message() in that it catches all messages - // with significant work it potentially could be combined, but let's not introduce side effects + // with work it potentially could be combined, but let's not introduce side effects void handle_msg_extra(mavlink_channel_t chan, const mavlink_message_t &msg); // send banner diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index b03826b5e2552..b96b08ac6881c 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1749,13 +1749,6 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status, // e.g. enforce-sysid says we shouldn't look at this packet return; } -//OW -#if HAL_MOUNT_ENABLED - // allow mounts to see all messages for this vehicle - AP_Mount *mount = AP::mount(); - if (mount != nullptr) mount->handle_msg_extra(chan, msg); -#endif -//OWEND handleMessage(msg); } @@ -3891,6 +3884,11 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) case MAVLINK_MSG_ID_HEARTBEAT: { handle_heartbeat(msg); +//OW +#if HAL_MOUNT_ENABLED + handle_mount_message(msg); +#endif +//OWEND break; } @@ -4002,6 +4000,9 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) handle_mount_message(msg); break; #endif +//OW + case MAVLINK_MSG_ID_MOUNT_STATUS: +//OWEND case MAVLINK_MSG_ID_GIMBAL_REPORT: case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION: case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS: @@ -4202,6 +4203,9 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) // MAVLINK_MSG_ID_RADIO_LINK_STATS, MAVLINK_MSG_ID_RADIO_LINK_FLOW_CONTROL are handled in the vehicle's GCS_Mavlink.cpp case MAVLINK_MSG_ID_RADIO_RC_CHANNELS: handle_radio_rc_channels(msg); +#if HAL_MOUNT_ENABLED + handle_mount_message(msg); +#endif break; //OWEND } From 1ab121a1de2075dab963ffb31fde000cf3a6b0ac Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 11:21:47 +0100 Subject: [PATCH 061/125] flags --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 70 +++++++++++++------ libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 1 + 2 files changed, 50 insertions(+), 21 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 024e8b9ed66a8..6076bc19ee5fa 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -165,6 +165,7 @@ void BP_Mount_STorM32_MAVLink::init() _mount_status = {}; _device_status = {}; + _flags_from_manager = UINT32_MAX; // the UINT32_MAX is important! _flags_for_gimbal = 0; _current_angles = {0.0f, 0.0f, 0.0f, NAN}; // the NAN is important! _script_control_angles = {}; @@ -241,7 +242,9 @@ void BP_Mount_STorM32_MAVLink::update() // Called from AP_Mount's gimbal manager handlers, but only if source is in control. // The front-end calls set_angle_target() and/or set_rate_target() depending // on the info in the gimbal manager messages. -// Return false to not do this angle/rate processing. +// Return false to skip this angle/rate processing. +// AP's gimbal manager should never ask to do things which the gimbal device +// can't do. Isn't the case but we know well our gimbal device. bool BP_Mount_STorM32_MAVLink::handle_gimbal_manager_flags(uint32_t flags) { // check flags for change to RETRACT @@ -253,9 +256,11 @@ bool BP_Mount_STorM32_MAVLink::handle_gimbal_manager_flags(uint32_t flags) set_mode(MAV_MOUNT_MODE_NEUTRAL); } + _flags_from_manager = flags; + update_gimbal_device_flags(); - // we currently do not support yaw LOCK + // driver currently does not support yaw LOCK // front-end is digesting GIMBAL_MANAGER_FLAGS_YAW_LOCK to determine yaw_is_earth_frame // we could make it to modify the flag, but for moment let's be happy. if (flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK) { @@ -291,14 +296,23 @@ void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags() break; } - // we currently only support pitch,roll lock, not pitch,roll follow + // account for gimbal manager flags + if (_flags_from_manager != UINT32_MAX) { + if (_flags_from_manager & GIMBAL_MANAGER_FLAGS_RC_EXCLUSIVE) _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RC_EXCLUSIVE; + if (_flags_from_manager & GIMBAL_MANAGER_FLAGS_RC_MIXED) _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RC_MIXED; + } else { + // for as long as the user isn't using the gimbal manager allow rc inputs. + // Helps to avoid user confusion with virtual channels. + _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RC_MIXED; + } + + // driver currently does not support pitch,roll follow, only pitch,roll lock _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK; - // we currently do not support yaw lock + // driver currently does not support yaw lock, only yaw follow // if (_is_yaw_lock) _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_LOCK; // set either YAW_IN_VEHICLE_FRAME or YAW_IN_EARTH_FRAME, to indicate new message format, STorM32 will reject otherwise - _flags_for_gimbal &=~ GIMBAL_MANAGER_FLAGS_YAW_IN_EARTH_FRAME; _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME; } @@ -1241,32 +1255,46 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_manager_information(mavlink_channel_t { // space already checked by streamer - // There are few specific gimbal manager capability flags, which we don't use. - // So we simply can carry forward the cap_flags received from the gimbal. + // The request to send this message should be NACKed for as long as + // the gimbal device was not found or its device info was not received. + // Not done currently, so as workaround don't send if not yet available. + // Should make third parties to repeat requests. + + if (!_got_device_info) return; + + // There are few specific gimbal manager capability flags, which are not used. + // So one simply can carry forward the cap_flags received from the gimbal. + // The STorM32 gimbal has these capabilities: + // - GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT | GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL + // - GIMBAL_DEVICE_CAP_FLAGS_HAS_xx_AXIS | GIMBAL_DEVICE_CAP_FLAGS_HAS_xx_FOLLOW | + // GIMBAL_DEVICE_CAP_FLAGS_HAS_xx_LOCK for each enabled axis + // - GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW if turn around enabled + // - GIMBAL_DEVICE_CAP_FLAGS_HAS_RC_INPUTS + // - !GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_YAW_IN_EARTH_FRAME is not currently supported by STorM32 + uint32_t cap_flags = _device_info.cap_flags; - // Not all capabilities are supported by this driver, so we erase them. + // This driver does not support all capabilities, so we erase them. // ATTENTION: This can mean that the gimbal device and gimbal manager capability flags // may be different, and any third party which mistakenly thinks it can use those from // the gimbal device messages may get confused ! + cap_flags &=~ (GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_FOLLOW | GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_FOLLOW | GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK | GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_YAW_IN_EARTH_FRAME); - uint8_t gimbal_device_id = _compid; - mavlink_msg_gimbal_manager_information_send( chan, - AP_HAL::millis(), // autopilot system time - cap_flags, // bitmap of gimbal manager capability flags - gimbal_device_id, // gimbal device id - radians(_params.roll_angle_min), // roll_min in radians - radians(_params.roll_angle_max), // roll_max in radians - radians(_params.pitch_angle_min), // pitch_min in radians - radians(_params.pitch_angle_max), // pitch_max in radians - radians(_params.yaw_angle_min), // yaw_min in radians - radians(_params.yaw_angle_max) // yaw_max in radians + AP_HAL::millis(), // autopilot system time + cap_flags, // bitmap of gimbal manager capability flags + _compid, // gimbal device id + _device_info.roll_min, // roll_min in radians + _device_info.roll_max, // roll_max in radians + _device_info.pitch_min, // pitch_min in radians + _device_info.pitch_max, // pitch_max in radians + _device_info.yaw_min, // yaw_min in radians + _device_info.yaw_max // yaw_max in radians ); } @@ -1274,10 +1302,10 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_manager_information(mavlink_channel_t // return gimbal manager flags used by GIMBAL_MANAGER_STATUS message uint32_t BP_Mount_STorM32_MAVLink::get_gimbal_manager_flags() const { - // There are currently no specific gimbal manager flags. So we simply + // There are currently no specific gimbal manager flags. So one simply // can carry forward the _flags received from the gimbal. - // ATTENTION: Not all capabilities are supported by this driver, but this + // ATTENTION: This driver does not support all capabilities, but this // should never be a problem since any third party should strictly adhere // to the capability flags obtained from the gimbal manager ! diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 092ff2048a014..6795733cfcb19 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -243,6 +243,7 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend uint32_t received_tlast_ms; // time last GIMBAL_DEVICE_ATTITUDE_STATUS was received (used for health reporting) } _device_status; + uint32_t _flags_from_manager; // flags received by gimbal manager uint16_t _flags_for_gimbal; // flags to be send to gimbal device struct { From 0e01bdf9979f7733b5be79b8a733e7c517f77973 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 11:22:07 +0100 Subject: [PATCH 062/125] magic script mode enum --- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 6076bc19ee5fa..052d86d5de19b 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -339,8 +339,8 @@ void BP_Mount_STorM32_MAVLink::send_target_angles() } -enum MountModeThisWouldBeGreatToHave { - MAV_MOUNT_MODE_SCRIPT = 7, +enum MountModeScriptMagic { + MAV_MOUNT_MODE_SCRIPT_MAGIC = 83, // 'S' }; @@ -348,7 +348,7 @@ enum MountModeThisWouldBeGreatToHave { void BP_Mount_STorM32_MAVLink::update_target_angles() { // update based on mount mode - switch (get_mode()) { + switch ((uint8_t)get_mode()) { // move mount to a "retracted" position // -> ANGLE @@ -424,7 +424,7 @@ void BP_Mount_STorM32_MAVLink::update_target_angles() // point mount to where a script wants it to point // -> ANGLE - case MAV_MOUNT_MODE_SCRIPT: + case MAV_MOUNT_MODE_SCRIPT_MAGIC: mnt_target.target_type = MountTargetType::ANGLE; mnt_target.angle_rad.roll = _script_control_angles.roll; mnt_target.angle_rad.pitch = _script_control_angles.pitch; From 9d4998cb2e20e1487dd8bd0dde3cf3292575d413 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 12:38:11 +0100 Subject: [PATCH 063/125] cc --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 21 +++++++++++-------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 052d86d5de19b..83411512aae1f 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -345,6 +345,7 @@ enum MountModeScriptMagic { // update_angle_target_from_rate() assumes a 50hz update rate! +// TODO: one should allow angles outside of +-PI, to go shortest path in case of turn around void BP_Mount_STorM32_MAVLink::update_target_angles() { // update based on mount mode @@ -438,15 +439,17 @@ void BP_Mount_STorM32_MAVLink::update_target_angles() } // account for range limits - // we only do teh yaw axis (should be done by StorM32 supervisor, but doesn't hurt - if (!_got_device_info) return; + // only do the yaw axis (should be done by STorM32 supervisor, but doesn't hurt) - if (!isnan(_device_info.yaw_min) && !isnan(_device_info.yaw_max) && - !(_device_info.cap_flags & GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW)) { + if (_got_device_info) return; + if (_device_info.cap_flags & GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW) return; + if (isnan(_device_info.yaw_min) || isnan(_device_info.yaw_max)) return; - if (mnt_target.angle_rad.yaw < _device_info.yaw_min) mnt_target.angle_rad.yaw = _device_info.yaw_min; - if (mnt_target.angle_rad.yaw > _device_info.yaw_max) mnt_target.angle_rad.yaw = _device_info.yaw_max; - } + // I believe currently angles are inside +-PI, no wrapPi voodoo needed + // TODO: if copter, copter might yaw by what the gimbal can't do + + if (mnt_target.angle_rad.yaw < _device_info.yaw_min) mnt_target.angle_rad.yaw = _device_info.yaw_min; + if (mnt_target.angle_rad.yaw > _device_info.yaw_max) mnt_target.angle_rad.yaw = _device_info.yaw_max; } @@ -870,7 +873,6 @@ void BP_Mount_STorM32_MAVLink::handle_msg_extra(const mavlink_message_t &msg) case MAVLINK_MSG_ID_MOUNT_STATUS: { _mount_status.received_tlast_ms = AP_HAL::millis(); // for health reporting - mavlink_mount_status_t payload; mavlink_msg_mount_status_decode(&msg, &payload); _current_angles.pitch = radians((float)payload.pointing_a * 0.01f); @@ -942,6 +944,7 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude() if (isnan(_current_angles.delta_yaw)) { // we don't have a valid yaw_ef target_yaw_bf = mnt_target.angle_rad.get_bf_yaw(); } else { + // TODO: handle turn around target_yaw_bf = wrap_PI(mnt_target.angle_rad.yaw - _current_angles.delta_yaw); } } else { @@ -1299,7 +1302,7 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_manager_information(mavlink_channel_t } -// return gimbal manager flags used by GIMBAL_MANAGER_STATUS message +// return gimbal manager flags. Used by GIMBAL_MANAGER_STATUS message. uint32_t BP_Mount_STorM32_MAVLink::get_gimbal_manager_flags() const { // There are currently no specific gimbal manager flags. So one simply From 1a96676d27096567d34788a25a0901b77c530348 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 12:56:50 +0100 Subject: [PATCH 064/125] fast prearm report --- libraries/AP_Arming/AP_Arming.cpp | 19 +++++++++++++++++-- libraries/AP_Arming/AP_Arming.h | 5 ++++- 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 10e53426784dc..597850f88644f 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -183,7 +183,12 @@ void AP_Arming::update(void) const uint32_t now_ms = AP_HAL::millis(); // perform pre-arm checks & display failures every 30 seconds bool display_fail = false; - if (now_ms - last_prearm_display_ms > PREARM_DISPLAY_PERIOD*1000) { +//OW +// if (now_ms - last_prearm_display_ms > PREARM_DISPLAY_PERIOD*1000) { + if ((report_immediately && (now_ms - last_prearm_display_ms > 4000)) || + (now_ms - last_prearm_display_ms > PREARM_DISPLAY_PERIOD*1000)) { + report_immediately = false; +//OWEND display_fail = true; last_prearm_display_ms = now_ms; } @@ -1539,7 +1544,10 @@ bool AP_Arming::pre_arm_checks(bool report) return true; } #endif - return hardware_safety_check(report) +//OW +// return hardware_safety_check(report) + bool checks_result = hardware_safety_check(report) +//OWEND #if HAL_HAVE_IMU_HEATER & heater_min_temperature_checks(report) #endif @@ -1574,6 +1582,13 @@ bool AP_Arming::pre_arm_checks(bool report) & opendroneid_checks(report) & serial_protocol_checks(report) & estop_checks(report); +//OW + if (!checks_result && last_prearm_checks_result) { // check went from true to false + report_immediately = true; + } + last_prearm_checks_result = checks_result; +//OWEND + return checks_result; } bool AP_Arming::arm_checks(AP_Arming::Method method) diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index a67a74bcd0450..5098387116127 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -303,7 +303,10 @@ class AP_Arming { uint32_t last_prearm_display_ms; // last time we send statustexts for prearm failures bool running_arming_checks; // true if the arming checks currently being performed are being done because the vehicle is trying to arm the vehicle - +//OW + bool last_prearm_checks_result; // result of last prearm check + bool report_immediately; // set to true when check goes from true to false, to trigger immediate report +//OWEND void update_arm_gpio(); }; From 2faee256a9d69ae8d0446b1ef9f24d0b0f121b17 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 13:08:40 +0100 Subject: [PATCH 065/125] rename to hanlde_message_extra() --- libraries/AP_Mount/AP_Mount.cpp | 6 +++--- libraries/AP_Mount/AP_Mount.h | 5 ++--- libraries/AP_Mount/AP_Mount_Backend.h | 2 +- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 2 +- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 4 ++-- 5 files changed, 9 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index c714edb12de2f..1fbddaa4e2ff6 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -949,7 +949,7 @@ void AP_Mount::handle_gimbal_report(mavlink_channel_t chan, const mavlink_messag void AP_Mount::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) { //OW - handle_msg_extra(chan, msg); + handle_message_extra(chan, msg); //OWEND switch (msg.msgid) { @@ -1141,11 +1141,11 @@ bool AP_Mount::cam_do_photo_video_mode(uint8_t instance, PhotoVideoMode photo_vi return backend->cam_do_photo_video_mode(photo_video_mode); } -void AP_Mount::handle_msg_extra(mavlink_channel_t chan, const mavlink_message_t &msg) +void AP_Mount::handle_message_extra(mavlink_channel_t chan, const mavlink_message_t &msg) { for (uint8_t instance=0; instancehandle_msg_extra(msg); + _backends[instance]->handle_message_extra(msg); } } } diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index ce667c7e70a00..dee1fe214b7db 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -292,9 +292,8 @@ class AP_Mount // momentary 3 pos switch to set to photo mode and take picture, set to video mode and start recording, or stop video recording bool cam_do_photo_video_mode(uint8_t instance, PhotoVideoMode photo_video_mode); - // this is somewhat different to handle_message() in that it catches all messages - // with work it potentially could be combined, but let's not introduce side effects - void handle_msg_extra(mavlink_channel_t chan, const mavlink_message_t &msg); + // this links into handle_message() to catch all messages + void handle_message_extra(mavlink_channel_t chan, const mavlink_message_t &msg); // send banner void send_banner(); diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 0478f9f78e7c5..515a169cc27de 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -219,7 +219,7 @@ class AP_Mount_Backend virtual bool cam_do_photo_video_mode(PhotoVideoMode photo_video_mode) { return false; } // handle msg - allows to process a msg from a gimbal - virtual void handle_msg_extra(const mavlink_message_t &msg) {} + virtual void handle_message_extra(const mavlink_message_t &msg) {} // send banner virtual void send_banner() {} diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 83411512aae1f..2612858d50767 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -842,7 +842,7 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_attitude_status(const mavlin } -void BP_Mount_STorM32_MAVLink::handle_msg_extra(const mavlink_message_t &msg) +void BP_Mount_STorM32_MAVLink::handle_message_extra(const mavlink_message_t &msg) { if (_protocol == Protocol::UNDEFINED) { // implies !_initialised && _compid determine_protocol(msg); diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 6795733cfcb19..6724a22fdc02c 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -120,8 +120,8 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // empty => we need to overwrite it void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) override; - // added: handle msg - allows to process a msg from a gimbal - void handle_msg_extra(const mavlink_message_t &msg) override; + // added: handle_message_extra - allows to process a msg from a gimbal + void handle_message_extra(const mavlink_message_t &msg) override; // added: send banner void send_banner() override; From 9d3b7c3a0a478f5af3444ae374e7eec3760df073 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 14:09:19 +0100 Subject: [PATCH 066/125] sotrm32 state enum --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 18 +----------------- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 11 +++++++++++ 2 files changed, 12 insertions(+), 17 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 2612858d50767..6fa96916fdf4b 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -123,22 +123,6 @@ void GimbalQuaternion::to_gimbal_euler(float &roll, float &pitch, float &yaw) co "Qfffffff" -//****************************************************** -// STorM32 states -//****************************************************** - -enum STORM32STATEENUM { - STORM32STATE_STARTUP_MOTORS = 0, - STORM32STATE_STARTUP_SETTLE, - STORM32STATE_STARTUP_CALIBRATE, - STORM32STATE_STARTUP_LEVEL, - STORM32STATE_STARTUP_MOTORDIRDETECT, - STORM32STATE_STARTUP_RELEVEL, - STORM32STATE_NORMAL, - STORM32STATE_STARTUP_FASTLEVEL, -}; - - //****************************************************** // BP_Mount_STorM32_MAVLink, main class //****************************************************** @@ -864,7 +848,7 @@ void BP_Mount_STorM32_MAVLink::handle_message_extra(const mavlink_message_t &msg mavlink_heartbeat_t payload; mavlink_msg_heartbeat_decode(&msg, &payload); uint8_t storm32_state = (payload.custom_mode & 0xFF); - _gimbal_armed = ((storm32_state == STORM32STATE_NORMAL) || (storm32_state == STORM32STATE_STARTUP_FASTLEVEL)); + _gimbal_armed = ((storm32_state == STorM32State::NORMAL) || (storm32_state == STorM32State::STARTUP_FASTLEVEL)); if ((payload.custom_mode & 0x80000000) == 0) { // don't follow all changes, but just toggle it to true once _gimbal_prearmchecks_ok = true; } diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 6724a22fdc02c..d4317cfbc1d99 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -212,6 +212,17 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // pre-arm and healthy checks // some need to be made mutable to get around that healthy() is const + enum STorM32State { + STARTUP_MOTORS = 0, + STARTUP_SETTLE, + STARTUP_CALIBRATE, + STARTUP_LEVEL, + STARTUP_MOTORDIRDETECT, + STARTUP_RELEVEL, + NORMAL, + STARTUP_FASTLEVEL, + }; + bool _gimbal_armed; // true once the gimbal has reached normal state uint32_t _gimbal_error_flags; // error flags in custom_mode field of the HEARTBEAT message (restricted to 16 bits) bool _gimbal_prearmchecks_ok; // true when the gimbal stops reporting prearm fail in the HEARTBEAT message From 63391ca67584e2bfc4ea970b3d546241030d0ddf Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 14:09:56 +0100 Subject: [PATCH 067/125] move check --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 337 +++++++++--------- 1 file changed, 168 insertions(+), 169 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 6fa96916fdf4b..efc0dafe04378 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -573,175 +573,6 @@ bool BP_Mount_STorM32_MAVLink::get_angular_velocity(Vector3f& rates) } -//------------------------------------------------------ -// Prearm & healthy functions -//------------------------------------------------------ - -const uint32_t FAILURE_FLAGS = - GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR | - GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR | - GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR | - GIMBAL_DEVICE_ERROR_FLAGS_SOFTWARE_ERROR | - GIMBAL_DEVICE_ERROR_FLAGS_COMMS_ERROR; - - -bool BP_Mount_STorM32_MAVLink::has_failures(char* s) -{ - uint32_t failure_flags = (_protocol == Protocol::GIMBAL_DEVICE) ? _device_status.received_failure_flags : _gimbal_error_flags; - - s[0] = '\0'; - if ((failure_flags & FAILURE_FLAGS) > 0) { - if (failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR) strcat(s, "mot,"); - if (failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR) strcat(s, "enc,"); - if (failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR) strcat(s, "volt,"); - if (s[0] != '\0') { - s[strlen(s)-1] = '\0'; - } else { - strcpy(s, "err flags"); - } - return true; - } - return false; -} - - -bool BP_Mount_STorM32_MAVLink::is_healthy() -{ - if (_protocol == Protocol::GIMBAL_DEVICE) { - // unhealthy if attitude status is not received within the last second - if ((AP_HAL::millis() - _device_status.received_tlast_ms) > 1000) { - return false; - } - - // check failure flags - // We should also check for GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER - // which means that gimbal did not got GIMBAL_DEVICE_SET_ATTITUDE messages. -// if ((_device_status.received_failure_flags & (FAILURE_FLAGS | GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER)) > 0) { - if ((_device_status.received_failure_flags & FAILURE_FLAGS) > 0) { - return false; - } - } else { - // unhealthy if mount status is not received within the last second - if ((AP_HAL::millis() - _mount_status.received_tlast_ms) > 1000) { - return false; - } - - // check failure flags // are set since v2.68b - if ((_gimbal_error_flags & FAILURE_FLAGS) > 0) { - return false; - } - } - - return true; -} - - -// is called with 1 Hz from main loop -void BP_Mount_STorM32_MAVLink::update_checks() -{ -char txt[255]; - - if (_armingchecks_running) _armingchecks_running--; // count down - - if (!_armingchecks_running || (_prearmchecks_passed && AP::notify().flags.armed)) { - - bool checks = is_healthy(); - - if (_checks_last && !checks) { // checks went from true to false - if (has_failures(txt)) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: failures: %s", _instance+1, txt); - } else { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: gimbal lost", _instance+1); - } - GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "MNT%u: not healthy", _instance+1); - } - if (!_checks_last && checks) { // checks went from false to true - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: healthy again", _instance+1); - } - _checks_last = checks; - - } - - if (!_armingchecks_running) { // we don't do anything else if arming mechanism not enabled - return; - _healthy = true; - } - - // do these only at startup - if (!_prearmchecks_passed) { - - if ((AP_HAL::millis() - _prearmcheck_sendtext_tlast_ms) > 30000) { // we haven't send it for a long while - _prearmcheck_sendtext_tlast_ms = AP_HAL::millis(); - if (!_initialised || !_gimbal_prearmchecks_ok || !_gimbal_armed) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: arm", _instance+1); - } else - if (has_failures(txt)) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: %s", _instance+1, txt); - } - } - - // unhealthy until gimbal has fully passed the startup sequence - // implies gimbal has been found, has replied with device info, and status message was received - // _initialised: -> gimbal found (HB received,_compid != 0) - // -> device info obtained (_got_device_info = true) - // -> status message received, protocol set (_protocol != PROTOCOL_UNDEFINED) - // _gimbal_prearmchecks_ok: -> gimbal HB reported gimbal's prearmchecks ok - // _gimbal_armed: -> gimbal HB reported gimbal is in normal state - if (!_initialised || !_gimbal_prearmchecks_ok || !_gimbal_armed) { - _healthy = false; - return; - } - } - - // do these continuously - bool checks = is_healthy(); - - if (_prearmchecks_passed) { // we are past prearm checks - if (_checks_last && !checks) { // checks went from true to false - if (has_failures(txt)) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: %s", _instance+1, txt); - } else { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: gimbal lost", _instance+1); - } - GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PreArm: Mount: not healthy"); - } - if (!_checks_last && checks) { // checks went from false to true - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); - } - _checks_last = checks; - _healthy = checks; - return; - } - - _checks_last = checks; - if (!checks) { - _healthy = false; - return; - } - - // if we get this far the mount is healthy - - // if we got this far the first time we inform the gcs - if (!_prearmchecks_passed) { - _prearmchecks_passed = true; - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); - } - - _healthy = true; // report to healthy() -} - - -// return true if healthy -// this is called when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA is set, and if not armed, else not -// is called with 1 Hz -bool BP_Mount_STorM32_MAVLink::healthy() const -{ - _armingchecks_running = 2; // to signal that ArduPilot arming check mechanism is active - - return _healthy; -} - - //------------------------------------------------------ // MAVLink handle functions //------------------------------------------------------ @@ -1300,6 +1131,174 @@ uint32_t BP_Mount_STorM32_MAVLink::get_gimbal_manager_flags() const } +//------------------------------------------------------ +// Prearm & healthy functions +//------------------------------------------------------ + +const uint32_t FAILURE_FLAGS = + GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR | + GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR | + GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR | + GIMBAL_DEVICE_ERROR_FLAGS_SOFTWARE_ERROR | + GIMBAL_DEVICE_ERROR_FLAGS_COMMS_ERROR; + + +bool BP_Mount_STorM32_MAVLink::has_failures(char* s) +{ + uint32_t failure_flags = (_protocol == Protocol::GIMBAL_DEVICE) ? _device_status.received_failure_flags : _gimbal_error_flags; + + s[0] = '\0'; + if ((failure_flags & FAILURE_FLAGS) > 0) { + if (failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR) strcat(s, "mot,"); + if (failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR) strcat(s, "enc,"); + if (failure_flags & GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR) strcat(s, "volt,"); + if (s[0] != '\0') { + s[strlen(s)-1] = '\0'; + } else { + strcpy(s, "err flags"); + } + return true; + } + return false; +} + + +bool BP_Mount_STorM32_MAVLink::is_healthy() +{ + if (_protocol == Protocol::GIMBAL_DEVICE) { + // unhealthy if attitude status is not received within the last second + if ((AP_HAL::millis() - _device_status.received_tlast_ms) > 1000) { + return false; + } + + // check failure flags + // We should also check for GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER + // which means that gimbal did not got GIMBAL_DEVICE_SET_ATTITUDE messages. +// if ((_device_status.received_failure_flags & (FAILURE_FLAGS | GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER)) > 0) { + if ((_device_status.received_failure_flags & FAILURE_FLAGS) > 0) { + return false; + } + } else { + // unhealthy if mount status is not received within the last second + if ((AP_HAL::millis() - _mount_status.received_tlast_ms) > 1000) { + return false; + } + + // check failure flags // are set since v2.68b + if ((_gimbal_error_flags & FAILURE_FLAGS) > 0) { + return false; + } + } + + return true; +} + + +// is called with 1 Hz from main loop +void BP_Mount_STorM32_MAVLink::update_checks() +{ +char txt[255]; + + if (_armingchecks_running) _armingchecks_running--; // count down + + if (_prearmchecks_passed && AP::notify().flags.armed && + !AP::arming().option_enabled(AP_Arming::Option::DISABLE_PREARM_DISPLAY)) { + bool checks = is_healthy(); + + if (_checks_last && !checks) { // checks went from true to false + if (has_failures(txt)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: failures: %s", _instance+1, txt); + } else { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: failure: gimbal lost", _instance+1); + } + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "MNT%u: not healthy", _instance+1); + } + if (!_checks_last && checks) { // checks went from false to true + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: healthy again", _instance+1); + } + _checks_last = checks; + } + + // don't do anything further if arming mechanism is not enabled + if (!_armingchecks_running) { + return; + _healthy = true; + } + + // do these only at startup + if (!_prearmchecks_passed) { + + if ((AP_HAL::millis() - _prearmcheck_sendtext_tlast_ms) > 30000) { // we haven't send it for a long while + _prearmcheck_sendtext_tlast_ms = AP_HAL::millis(); + if (!_initialised || !_gimbal_prearmchecks_ok || !_gimbal_armed) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: arm", _instance+1); + } else + if (has_failures(txt)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: %s", _instance+1, txt); + } + } + + // unhealthy until gimbal has fully passed the startup sequence + // implies gimbal has been found, has replied with device info, and status message was received + // _initialised: -> gimbal found (HB received,_compid != 0) + // -> device info obtained (_got_device_info = true) + // -> status message received, protocol set (_protocol != PROTOCOL_UNDEFINED) + // _gimbal_prearmchecks_ok: -> gimbal HB reported gimbal's prearmchecks ok + // _gimbal_armed: -> gimbal HB reported gimbal is in normal state + if (!_initialised || !_gimbal_prearmchecks_ok || !_gimbal_armed) { + _healthy = false; + return; + } + } + + // do these continuously + bool checks = is_healthy(); + + if (_prearmchecks_passed) { // we are past prearm checks + if (_checks_last && !checks) { // checks went from true to false + if (has_failures(txt)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: %s", _instance+1, txt); + } else { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: gimbal lost", _instance+1); + } + } + if (!_checks_last && checks) { // checks went from false to true + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); + } + _checks_last = checks; + _healthy = checks; + return; + } + + _checks_last = checks; + if (!checks) { + _healthy = false; + return; + } + + // if we get this far the mount is healthy + + // if we got this far the first time we inform the gcs + if (!_prearmchecks_passed) { + _prearmchecks_passed = true; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); + } + + _healthy = true; // report to healthy() +} + + +// return true if healthy +// this is called when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA is set, and if not armed, else not +// is called with 1 Hz +bool BP_Mount_STorM32_MAVLink::healthy() const +{ + _armingchecks_running = 2; // to signal that ArduPilot arming check mechanism is active + + return _healthy; +} + + //------------------------------------------------------ // Scripting accessors //------------------------------------------------------ From 0d99601a38cf2182cd8f8937ca2b94fccb0af373 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 14:10:35 +0100 Subject: [PATCH 068/125] forbid mountstatus, gimbaldeviceattitudestatus if wrong protocol --- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index efc0dafe04378..eae897a1287a3 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -614,6 +614,10 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_information(const mavlink_me void BP_Mount_STorM32_MAVLink::handle_gimbal_device_attitude_status(const mavlink_message_t &msg) { + if (_protocol != Protocol::GIMBAL_DEVICE) { + return; + } + // this msg is not from our gimbal if (msg.sysid != _sysid || msg.compid != _compid) { return; @@ -687,7 +691,8 @@ void BP_Mount_STorM32_MAVLink::handle_message_extra(const mavlink_message_t &msg break; } case MAVLINK_MSG_ID_MOUNT_STATUS: { - _mount_status.received_tlast_ms = AP_HAL::millis(); // for health reporting + if (_protocol != Protocol::MOUNT) break; + _mount_status.received_tlast_ms = AP_HAL::millis(); // used for health check mavlink_mount_status_t payload; mavlink_msg_mount_status_decode(&msg, &payload); _current_angles.pitch = radians((float)payload.pointing_a * 0.01f); From a293ffbbc096cf12eb52e6feb3c7e01161fe4e3d Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 14:10:48 +0100 Subject: [PATCH 069/125] assert mav landed state --- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index eae897a1287a3..9cbcfa46d8fea 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -794,6 +794,9 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude() } +// ensure that MAV_LANDED_STATE enum hasn't been extended +static_assert((uint8_t)MAV_LANDED_STATE_ENUM_END == 5, "MAV_LANDED_STATE enum has changed"); + enum LandedStateThisWouldBeGreatToHave { MAV_LANDED_STATE_PREPARING_FOR_TAKEOFF = 5, }; From 4507c92c15e575a7316a75f5f414682ee972b077 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 Nov 2023 14:12:37 +0100 Subject: [PATCH 070/125] don't allow rc mixed if gimbal manger not used --- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 9cbcfa46d8fea..0e3a2720df7cd 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -284,10 +284,6 @@ void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags() if (_flags_from_manager != UINT32_MAX) { if (_flags_from_manager & GIMBAL_MANAGER_FLAGS_RC_EXCLUSIVE) _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RC_EXCLUSIVE; if (_flags_from_manager & GIMBAL_MANAGER_FLAGS_RC_MIXED) _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RC_MIXED; - } else { - // for as long as the user isn't using the gimbal manager allow rc inputs. - // Helps to avoid user confusion with virtual channels. - _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RC_MIXED; } // driver currently does not support pitch,roll follow, only pitch,roll lock From c58ad104bedb45786150f8e363a130a5a9127079 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sun, 26 Nov 2023 04:39:26 +0100 Subject: [PATCH 071/125] cc --- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 0e3a2720df7cd..eec422d546101 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -584,9 +584,7 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_information(const mavlink_me // we could check here for sanity of _device_info.gimbal_device_id, but let's just be happy - // set parameter defaults from gimbal information - // Q: why set_default ?? why not actual value ?? allows the user to overwrite em? - + // correct parameters from gimbal information if (!isnan(_device_info.roll_min) && !isnan(_device_info.roll_max)) { if (degrees(_device_info.roll_min) > _params.roll_angle_min) _params.roll_angle_min.set(degrees(_device_info.roll_min)); if (degrees(_device_info.roll_max) < _params.roll_angle_max) _params.roll_angle_max.set(degrees(_device_info.roll_max)); From e9a1c108a5273c2faa2c9f0666cbcfe6377280ac Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sun, 26 Nov 2023 04:58:30 +0100 Subject: [PATCH 072/125] bump master --- libraries/bp_version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/bp_version.h b/libraries/bp_version.h index ebb5fb5f8aae0..b1b943d116a51 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,7 +1,7 @@ #pragma once #define BETAPILOTVERSION "v059c" -#define DATEOFBASEBRANCH "20231124" +#define DATEOFBASEBRANCH "20231126" /* search for //OW to find all changes From 146b835a121a11e3eebba7c7823211c33036e384 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Mon, 27 Nov 2023 19:33:31 +0100 Subject: [PATCH 073/125] script target --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 53 ++++++++++++------- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 4 +- 2 files changed, 38 insertions(+), 19 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index eec422d546101..420ae40a56811 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -23,11 +23,11 @@ extern const AP_HAL::HAL& hal; //****************************************************** // Quaternion & Euler for Gimbal //****************************************************** -// It is inappropriate to use NED (roll-pitch-yaw) to convert received quaternion to Euler angles -// and vice versa. For most gimbals pitch-roll-yaw is appropriate. When the roll angle is zero, -// both are equivalent, which should be the majority of cases. -// The issue with NED is the gimbal lock at pitch +-90 deg, but pitch +-90 deg is a common -// operation point for gimbals. +// For most gimbals it is inappropriate to use NED (roll-pitch-yaw) to convert received +// quaternion to Euler angles and vice versa. For most gimbals pitch-roll-yaw is +// appropriate. When the roll angle is zero, both are equivalent, which should be the +// majority of cases. The issue with NED is the gimbal lock at pitch +-90 deg, but pitch +// +-90 deg is a common operation point for gimbals. // The angles in this driver are thus pitch-roll-yaw Euler. class GimbalQuaternion : public Quaternion @@ -228,7 +228,7 @@ void BP_Mount_STorM32_MAVLink::update() // on the info in the gimbal manager messages. // Return false to skip this angle/rate processing. // AP's gimbal manager should never ask to do things which the gimbal device -// can't do. Isn't the case but we know well our gimbal device. +// can't do. Isn't so but we know well our gimbal device. bool BP_Mount_STorM32_MAVLink::handle_gimbal_manager_flags(uint32_t flags) { // check flags for change to RETRACT @@ -284,6 +284,10 @@ void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags() if (_flags_from_manager != UINT32_MAX) { if (_flags_from_manager & GIMBAL_MANAGER_FLAGS_RC_EXCLUSIVE) _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RC_EXCLUSIVE; if (_flags_from_manager & GIMBAL_MANAGER_FLAGS_RC_MIXED) _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RC_MIXED; + } else { + // for as long as no gimbal manager message has been sent to the fc, enable rc mixed. + // Should avoid user confusion when choosing the gimbal device operation mode. + _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RC_MIXED; } // driver currently does not support pitch,roll follow, only pitch,roll lock @@ -337,6 +341,7 @@ void BP_Mount_STorM32_MAVLink::update_target_angles() const Vector3f &target = _params.retract_angles.get(); mnt_target.target_type = MountTargetType::ANGLE; mnt_target.angle_rad.set(target*DEG_TO_RAD, false); + mnt_target_loc_valid = false; break; } @@ -346,6 +351,7 @@ void BP_Mount_STorM32_MAVLink::update_target_angles() const Vector3f &target = _params.neutral_angles.get(); mnt_target.target_type = MountTargetType::ANGLE; mnt_target.angle_rad.set(target*DEG_TO_RAD, false); + mnt_target_loc_valid = false; break; } @@ -356,6 +362,7 @@ void BP_Mount_STorM32_MAVLink::update_target_angles() // SToRM32 doesn't support rate, so update target angle from rate if necessary. if (mnt_target.target_type == MountTargetType::RATE) { update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); + mnt_target_loc_valid = false; } break; @@ -373,6 +380,7 @@ void BP_Mount_STorM32_MAVLink::update_target_angles() mnt_target.rate_rads = rc_target; break; } + mnt_target_loc_valid = false; break; } @@ -382,6 +390,8 @@ void BP_Mount_STorM32_MAVLink::update_target_angles() case MAV_MOUNT_MODE_GPS_POINT: if (get_angle_target_to_roi(mnt_target.angle_rad)) { mnt_target.target_type = MountTargetType::ANGLE; + mnt_target_loc_valid = true; + mnt_target_loc = _roi_target; } break; @@ -391,6 +401,8 @@ void BP_Mount_STorM32_MAVLink::update_target_angles() case MAV_MOUNT_MODE_HOME_LOCATION: if (get_angle_target_to_home(mnt_target.angle_rad)) { mnt_target.target_type = MountTargetType::ANGLE; + mnt_target_loc_valid = true; + mnt_target_loc = AP::ahrs().get_home(); } break; @@ -400,6 +412,8 @@ void BP_Mount_STorM32_MAVLink::update_target_angles() case MAV_MOUNT_MODE_SYSID_TARGET: if (get_angle_target_to_sysid(mnt_target.angle_rad)) { mnt_target.target_type = MountTargetType::ANGLE; + mnt_target_loc_valid = true; + mnt_target_loc = _target_sysid_location; } break; @@ -411,6 +425,7 @@ void BP_Mount_STorM32_MAVLink::update_target_angles() mnt_target.angle_rad.pitch = _script_control_angles.pitch; mnt_target.angle_rad.yaw = _script_control_angles.yaw_bf; mnt_target.angle_rad.yaw_is_ef = false; + mnt_target_loc_valid = false; break; default: @@ -425,7 +440,7 @@ void BP_Mount_STorM32_MAVLink::update_target_angles() if (_device_info.cap_flags & GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW) return; if (isnan(_device_info.yaw_min) || isnan(_device_info.yaw_max)) return; - // I believe currently angles are inside +-PI, no wrapPi voodoo needed + // I believe currently angles are inside +-PI, no wrapPI needed // TODO: if copter, copter might yaw by what the gimbal can't do if (mnt_target.angle_rad.yaw < _device_info.yaw_min) mnt_target.angle_rad.yaw = _device_info.yaw_min; @@ -1196,7 +1211,7 @@ bool BP_Mount_STorM32_MAVLink::is_healthy() } -// is called with 1 Hz from main loop +// is called with 1 Hz from update loop void BP_Mount_STorM32_MAVLink::update_checks() { char txt[255]; @@ -1227,13 +1242,13 @@ char txt[255]; _healthy = true; } - // do these only at startup + // do these only at startup, until prearm checks have been passed once if (!_prearmchecks_passed) { if ((AP_HAL::millis() - _prearmcheck_sendtext_tlast_ms) > 30000) { // we haven't send it for a long while _prearmcheck_sendtext_tlast_ms = AP_HAL::millis(); if (!_initialised || !_gimbal_prearmchecks_ok || !_gimbal_armed) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: arm", _instance+1); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: not armed", _instance+1); } else if (has_failures(txt)) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks FAIL: %s", _instance+1, txt); @@ -1241,7 +1256,6 @@ char txt[255]; } // unhealthy until gimbal has fully passed the startup sequence - // implies gimbal has been found, has replied with device info, and status message was received // _initialised: -> gimbal found (HB received,_compid != 0) // -> device info obtained (_got_device_info = true) // -> status message received, protocol set (_protocol != PROTOCOL_UNDEFINED) @@ -1254,6 +1268,8 @@ char txt[255]; } // do these continuously + // - check failures + // - check connection to gimbal bool checks = is_healthy(); if (_prearmchecks_passed) { // we are past prearm checks @@ -1273,12 +1289,13 @@ char txt[255]; } _checks_last = checks; + if (!checks) { _healthy = false; return; } - // if we get this far the mount is healthy + // if we get this far in prearm state, then mount is healthy // if we got this far the first time we inform the gcs if (!_prearmchecks_passed) { @@ -1286,7 +1303,7 @@ char txt[255]; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); } - _healthy = true; // report to healthy() + _healthy = true; } @@ -1308,11 +1325,11 @@ bool BP_Mount_STorM32_MAVLink::healthy() const // return target location if available // returns true if a target location is available and fills in target_loc argument bool BP_Mount_STorM32_MAVLink::get_location_target(Location &_target_loc) -{/* - if (target_loc_valid) { - _target_loc = target_loc; +{ + if (mnt_target_loc_valid) { + _target_loc = mnt_target_loc; return true; - } */ + } return false; } @@ -1462,7 +1479,7 @@ void BP_Mount_STorM32_MAVLink::send_cmd_do_digicam_control(bool shoot) void BP_Mount_STorM32_MAVLink::send_gimbal_device_attitude_status(mavlink_channel_t chan) { // space already checked by streamer - // checked space of GIMBAL_DEVICE_ATTITUDE_STATUS, but MOUNT_STATUS is (much) smaller, so no issue + // has checked for space of GIMBAL_DEVICE_ATTITUDE_STATUS, but MOUNT_STATUS is (much) smaller, so no issue if (_compid != MAV_COMP_ID_GIMBAL) { // do it only for the 1st gimbal return; diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index d4317cfbc1d99..1c0eb7f621834 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -263,7 +263,6 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend float yaw_bf; float delta_yaw; } _current_angles; // current angles, obtained from either MOUNT_STATUS or GIMBAL_DEVICE_ATTITUDE_STATUS - Vector3f _current_omega; // current angular velocities, obtained from GIMBAL_DEVICE_ATTITUDE_STATUS struct { @@ -272,6 +271,9 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend float yaw_bf; } _script_control_angles; // angles set by script + bool mnt_target_loc_valid; // true when a location is set by a mount mode + Location mnt_target_loc; // target location set by a mount mode + // set the flags for gimbal according to current conditions void update_gimbal_device_flags(); From 6e99482809777f45f76433fea1c416f082fcece2 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Mon, 27 Nov 2023 20:14:33 +0100 Subject: [PATCH 074/125] sync with radiolink PR --- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 24 +++++++++++------------ libraries/AP_RCProtocol/AP_RCProtocol.h | 2 -- libraries/GCS_MAVLink/GCS.h | 21 +++++++++++--------- libraries/GCS_MAVLink/GCS_Common.cpp | 23 +++++++++++----------- libraries/GCS_MAVLink/MAVLink_routing.cpp | 2 +- 5 files changed, 36 insertions(+), 36 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index a1fadd601220f..1d126f63f18c5 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -33,11 +33,11 @@ #include "AP_RCProtocol_FPort.h" #include "AP_RCProtocol_FPort2.h" #include "AP_RCProtocol_DroneCAN.h" -#include -#include //OW RADIOLINK #include "AP_RCProtocol_MavlinkRadio.h" //OWEND +#include +#include #include @@ -549,20 +549,12 @@ bool AP_RCProtocol::protocol_enabled(rcprotocol_t protocol) const return ((1U<<(uint8_t(protocol)+1)) & rc_protocols_mask) != 0; } -namespace AP { - AP_RCProtocol &RC() - { - static AP_RCProtocol rcprot; - return rcprot; - } -}; - //OW RADIOLINK void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) { - // this message is also used to check if the receiver is present + // receiving this message is also used to check if the receiver is present // so let's first do the receiver detection - if (_detected_protocol == AP_RCProtocol::NONE) { // we are still searching + if (_detected_protocol == AP_RCProtocol::NONE) { // still searching //#ifndef IOMCU_FW //? we need this, but originally it is enclosed by an #ifndef IOMCU_FW // how does this work? @@ -612,4 +604,12 @@ void AP_RCProtocol::handle_radio_link_stats(mavlink_radio_link_stats_t* packet) } //OWEND +namespace AP { + AP_RCProtocol &RC() + { + static AP_RCProtocol rcprot; + return rcprot; + } +}; + #endif // AP_RCPROTOCOL_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index 54c0de69cbe46..10e7b0bb1249b 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -20,9 +20,7 @@ #include #include -//OW RADIOLINK #include -//OWEND #define MAX_RCIN_CHANNELS 18 #define MIN_RCIN_CHANNELS 5 diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 69f481dab14b9..95fabcebe27dc 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -485,15 +485,6 @@ class GCS_MAVLINK protected: -//OW RADIOLINK - // called from vehicle class handler - mavlink_radio_t _mavlink_radio_packet; - void handle_radio_link_stats(const mavlink_message_t &msg, bool log_radio); - void handle_radio_link_flow_control(const mavlink_message_t &msg, bool log_radio); - // called from common handler - void handle_radio_rc_channels(const mavlink_message_t &msg); -//OWEND - bool mavlink_coordinate_frame_to_location_alt_frame(MAV_FRAME coordinate_frame, Location::AltFrame &frame); @@ -692,6 +683,13 @@ class GCS_MAVLINK void handle_optical_flow(const mavlink_message_t &msg); void handle_manual_control(const mavlink_message_t &msg); +//OW RADIOLINK + // called from vehicle class handler + void handle_radio_link_stats(const mavlink_message_t &msg, bool log_radio); + void handle_radio_link_flow_control(const mavlink_message_t &msg, bool log_radio); + // called from common handler + void handle_radio_rc_channels(const mavlink_message_t &msg); +//OWEND // default empty handling of LANDING_TARGET virtual void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) { } @@ -755,6 +753,11 @@ class GCS_MAVLINK mavlink_message_t _channel_buffer; mavlink_status_t _channel_status; +//OW RADIOLINK + // for mavlink radio + mavlink_radio_t _mavlink_radio_packet; +//OWEND + const AP_SerialManager::UARTState *uartstate; // last time we got a non-zero RSSI from RADIO_STATUS diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index b96b08ac6881c..5eff4c73cf743 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4110,6 +4110,16 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: handle_rc_channels_override(msg); break; +//OW RADIOLINK +// like with old RADIO_STATUS, the messages +// MAVLINK_MSG_ID_RADIO_LINK_STATS, MAVLINK_MSG_ID_RADIO_LINK_FLOW_CONTROL are handled in the vehicle's GCS_Mavlink.cpp + case MAVLINK_MSG_ID_RADIO_RC_CHANNELS: + handle_radio_rc_channels(msg); +#if HAL_MOUNT_ENABLED + handle_mount_message(msg); +#endif + break; +//OWEND #if AP_OPTICALFLOW_ENABLED case MAVLINK_MSG_ID_OPTICAL_FLOW: @@ -4197,17 +4207,6 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) break; } #endif - -//OW RADIOLINK -// like with old RADIO_STATUS, the messages -// MAVLINK_MSG_ID_RADIO_LINK_STATS, MAVLINK_MSG_ID_RADIO_LINK_FLOW_CONTROL are handled in the vehicle's GCS_Mavlink.cpp - case MAVLINK_MSG_ID_RADIO_RC_CHANNELS: - handle_radio_rc_channels(msg); -#if HAL_MOUNT_ENABLED - handle_mount_message(msg); -#endif - break; -//OWEND } } @@ -6434,7 +6433,7 @@ bool GCS_MAVLINK::accept_packet(const mavlink_status_t &status, } //OW RADIOLINK - // we want to handle messages from a radio + // handle messages from a mavlink receiver // we currently narrow it down to "ours" to play it safe if ((msg.compid == MAV_COMP_ID_TELEMETRY_RADIO) && (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS || msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_STATS || diff --git a/libraries/GCS_MAVLink/MAVLink_routing.cpp b/libraries/GCS_MAVLink/MAVLink_routing.cpp index 3933360c33519..b59c6ee338049 100644 --- a/libraries/GCS_MAVLink/MAVLink_routing.cpp +++ b/libraries/GCS_MAVLink/MAVLink_routing.cpp @@ -141,7 +141,7 @@ bool MAVLink_routing::check_and_forward(GCS_MAVLINK &in_link, const mavlink_mess get_targets(msg, target_system, target_component); //OW RADIOLINK - // we want to handle messages from a radio as if they had target_system = our system, target_component = 0 + // handle messages from a mavlink receiver as if it had target_system = our system, target_component = 0 // we currently narrow it down to "our" messages to play it safe if ((msg.compid == MAV_COMP_ID_TELEMETRY_RADIO) && (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS || msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_STATS || From e8ce4baecd22fabfe186e4c1a65b44fe403b932f Mon Sep 17 00:00:00 2001 From: olliw42 Date: Mon, 27 Nov 2023 20:22:47 +0100 Subject: [PATCH 075/125] more script --- libraries/AP_Scripting/generator/description/bindings.desc | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index a37476a5252d9..28ac1930567db 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -669,7 +669,9 @@ include AP_Mount/AP_Mount.h singleton AP_Mount depends HAL_MOUNT_ENABLED == 1 singleton AP_Mount rename mount singleton AP_Mount method get_mode MAV_MOUNT_MODE'enum uint8_t'skip_check -singleton AP_Mount method set_mode void uint8_t'skip_check MAV_MOUNT_MODE'enum MAV_MOUNT_MODE_RETRACT MAV_MOUNT_MODE_HOME_LOCATION +-- //OW singleton AP_Mount method set_mode void uint8_t'skip_check MAV_MOUNT_MODE'enum MAV_MOUNT_MODE_RETRACT MAV_MOUNT_MODE_HOME_LOCATION +singleton AP_Mount method set_mode void uint8_t'skip_check MAV_MOUNT_MODE'enum MAV_MOUNT_MODE_RETRACT UINT8_MAX +-- //OWEND singleton AP_Mount method set_angle_target void uint8_t'skip_check float'skip_check float'skip_check float'skip_check boolean singleton AP_Mount method set_rate_target void uint8_t'skip_check float'skip_check float'skip_check float'skip_check boolean singleton AP_Mount method set_roi_target void uint8_t'skip_check Location From e7509bce264fe00577027cdd77896d2ed1574dac Mon Sep 17 00:00:00 2001 From: olliw42 Date: Mon, 27 Nov 2023 20:25:08 +0100 Subject: [PATCH 076/125] bump master --- libraries/bp_version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/bp_version.h b/libraries/bp_version.h index b1b943d116a51..59b27f732497d 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,7 +1,7 @@ #pragma once #define BETAPILOTVERSION "v059c" -#define DATEOFBASEBRANCH "20231126" +#define DATEOFBASEBRANCH "20231127" /* search for //OW to find all changes From e4ee5dd429a3e0c3c264c2f5c695ae19059023ad Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 28 Nov 2023 21:44:58 +0100 Subject: [PATCH 077/125] script take and give instead of magic mode --- libraries/AP_Mount/AP_Mount.cpp | 20 ++++++++ libraries/AP_Mount/AP_Mount.h | 4 ++ libraries/AP_Mount/AP_Mount_Backend.h | 4 ++ .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 51 +++++++++++-------- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 9 +++- .../generator/description/bindings.desc | 8 +-- 6 files changed, 70 insertions(+), 26 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 1fbddaa4e2ff6..4b691317d3594 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -1158,6 +1158,26 @@ void AP_Mount::send_banner() } } } + +bool AP_Mount::take_control() +{ + auto *backend = get_primary(); + if (backend == nullptr) { + return false; + } + + return backend->take_control(); +} + +bool AP_Mount::give_control() +{ + auto *backend = get_primary(); + if (backend == nullptr) { + return false; + } + + return backend->give_control(); +} //OWEND #endif /* HAL_MOUNT_ENABLED */ diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index dee1fe214b7db..e9531b95e2352 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -297,6 +297,10 @@ class AP_Mount // send banner void send_banner(); + + // used for scripting + bool take_control(); + bool give_control(); //OWEND protected: diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 515a169cc27de..ada4218225522 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -233,6 +233,10 @@ class AP_Mount_Backend // The function may modify the flags according to its capabilities. // Return false to abort angle/rate processing. virtual bool handle_gimbal_manager_flags(uint32_t flags); + + // used for scripting + virtual bool take_control() { return false; } + virtual bool give_control() { return false; } //OWEND // diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 420ae40a56811..6af6e2aecf446 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -152,7 +152,7 @@ void BP_Mount_STorM32_MAVLink::init() _flags_from_manager = UINT32_MAX; // the UINT32_MAX is important! _flags_for_gimbal = 0; _current_angles = {0.0f, 0.0f, 0.0f, NAN}; // the NAN is important! - _script_control_angles = {}; + _script_angles = {}; _yaw_lock = false; // can't be currently supported, so we need to ensure this is false. This is important! @@ -323,11 +323,6 @@ void BP_Mount_STorM32_MAVLink::send_target_angles() } -enum MountModeScriptMagic { - MAV_MOUNT_MODE_SCRIPT_MAGIC = 83, // 'S' -}; - - // update_angle_target_from_rate() assumes a 50hz update rate! // TODO: one should allow angles outside of +-PI, to go shortest path in case of turn around void BP_Mount_STorM32_MAVLink::update_target_angles() @@ -417,22 +412,22 @@ void BP_Mount_STorM32_MAVLink::update_target_angles() } break; - // point mount to where a script wants it to point - // -> ANGLE - case MAV_MOUNT_MODE_SCRIPT_MAGIC: - mnt_target.target_type = MountTargetType::ANGLE; - mnt_target.angle_rad.roll = _script_control_angles.roll; - mnt_target.angle_rad.pitch = _script_control_angles.pitch; - mnt_target.angle_rad.yaw = _script_control_angles.yaw_bf; - mnt_target.angle_rad.yaw_is_ef = false; - mnt_target_loc_valid = false; - break; - default: // we do not know this mode so do nothing break; } + if (_script_angles.control) { + // point mount to where a script wants it to point + // -> ANGLE + mnt_target.target_type = MountTargetType::ANGLE; + mnt_target.angle_rad.roll = _script_angles.roll; + mnt_target.angle_rad.pitch = _script_angles.pitch; + mnt_target.angle_rad.yaw = _script_angles.yaw_bf; + mnt_target.angle_rad.yaw_is_ef = false; + mnt_target_loc_valid = false; + } + // account for range limits // only do the yaw axis (should be done by STorM32 supervisor, but doesn't hurt) @@ -1319,7 +1314,7 @@ bool BP_Mount_STorM32_MAVLink::healthy() const //------------------------------------------------------ -// Scripting accessors +// Scripting accessors & bindings //------------------------------------------------------ // return target location if available @@ -1337,9 +1332,23 @@ bool BP_Mount_STorM32_MAVLink::get_location_target(Location &_target_loc) // update mount's actual angles (to be called by script communicating with mount) void BP_Mount_STorM32_MAVLink::set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) { - _script_control_angles.roll = radians(roll_deg); - _script_control_angles.pitch = radians(pitch_deg); - _script_control_angles.yaw_bf = radians(yaw_bf_deg); + _script_angles.roll = radians(roll_deg); + _script_angles.pitch = radians(pitch_deg); + _script_angles.yaw_bf = radians(yaw_bf_deg); +} + + +bool BP_Mount_STorM32_MAVLink::take_control() +{ + _script_angles.control = true; + return true; //we assume only one script trying this, so KIS +} + + +bool BP_Mount_STorM32_MAVLink::give_control() +{ + _script_angles.control = false; + return true; //we assume only one script trying this, so KIS } diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 1c0eb7f621834..270001c3148ce 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -126,11 +126,15 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // added: send banner void send_banner() override; - // accessors for scripting backends + // script accessors & bindings // AP provides them only for script backend, but can be useful generally. bool get_location_target(Location &target_loc) override; void set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) override; + // added: handle control + bool take_control() override; + bool give_control() override; + // // camera controls for gimbals that include a camera // @@ -266,10 +270,11 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend Vector3f _current_omega; // current angular velocities, obtained from GIMBAL_DEVICE_ATTITUDE_STATUS struct { + bool control; float roll; float pitch; float yaw_bf; - } _script_control_angles; // angles set by script + } _script_angles; // control and angles set by script bool mnt_target_loc_valid; // true when a location is set by a mount mode Location mnt_target_loc; // target location set by a mount mode diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 28ac1930567db..48e147c1406ab 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -669,9 +669,7 @@ include AP_Mount/AP_Mount.h singleton AP_Mount depends HAL_MOUNT_ENABLED == 1 singleton AP_Mount rename mount singleton AP_Mount method get_mode MAV_MOUNT_MODE'enum uint8_t'skip_check --- //OW singleton AP_Mount method set_mode void uint8_t'skip_check MAV_MOUNT_MODE'enum MAV_MOUNT_MODE_RETRACT MAV_MOUNT_MODE_HOME_LOCATION -singleton AP_Mount method set_mode void uint8_t'skip_check MAV_MOUNT_MODE'enum MAV_MOUNT_MODE_RETRACT UINT8_MAX --- //OWEND +singleton AP_Mount method set_mode void uint8_t'skip_check MAV_MOUNT_MODE'enum MAV_MOUNT_MODE_RETRACT MAV_MOUNT_MODE_HOME_LOCATION singleton AP_Mount method set_angle_target void uint8_t'skip_check float'skip_check float'skip_check float'skip_check boolean singleton AP_Mount method set_rate_target void uint8_t'skip_check float'skip_check float'skip_check float'skip_check boolean singleton AP_Mount method set_roi_target void uint8_t'skip_check Location @@ -680,6 +678,10 @@ singleton AP_Mount method get_rate_target boolean uint8_t'skip_check float'Null singleton AP_Mount method get_angle_target boolean uint8_t'skip_check float'Null float'Null float'Null boolean'Null singleton AP_Mount method get_location_target boolean uint8_t'skip_check Location'Null singleton AP_Mount method set_attitude_euler void uint8_t'skip_check float'skip_check float'skip_check float'skip_check +-- //OW +singleton AP_Mount method take_control boolean +singleton AP_Mount method give_control boolean +-- //OWEND include AP_Camera/AP_Camera.h singleton AP_Camera depends AP_CAMERA_ENABLED && (AP_CAMERA_SCRIPTING_ENABLED == 1) From 9d494556efe8ea7b397ab1f3b9a7a9e1f9b4af30 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 28 Nov 2023 22:05:08 +0100 Subject: [PATCH 078/125] sync with radiolink PR --- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 16 +++++++++------- libraries/AP_RCProtocol/AP_RCProtocol.h | 5 +++++ .../AP_RCProtocol_MavlinkRadio.cpp | 19 ++++++++++++------- .../AP_RCProtocol_MavlinkRadio.h | 11 +++++++++-- .../AP_RCProtocol/AP_RCProtocol_config.h | 6 ++++++ libraries/GCS_MAVLink/GCS_Common.cpp | 9 +++++++++ 6 files changed, 50 insertions(+), 16 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 1d126f63f18c5..392dc7eb5f732 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -34,7 +34,7 @@ #include "AP_RCProtocol_FPort2.h" #include "AP_RCProtocol_DroneCAN.h" //OW RADIOLINK -#include "AP_RCProtocol_MavlinkRadio.h" +#include "AP_RCProtocol_MAVLinkRadio.h" //OWEND #include #include @@ -86,7 +86,9 @@ void AP_RCProtocol::init() backend[AP_RCProtocol::DRONECAN] = new AP_RCProtocol_DroneCAN(*this); #endif //OW RADIOLINK - backend[AP_RCProtocol::MAVLINK_RADIO] = new AP_RCProtocol_MavlinkRadio(*this); +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + backend[AP_RCProtocol::MAVLINK_RADIO] = new AP_RCProtocol_MAVLinkRadio(*this); +#endif //OWEND } @@ -512,8 +514,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol) return "DroneCAN"; #endif //OW RADIOLINK +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED case MAVLINK_RADIO: - return "MavRadio"; + return "MAVRadio"; +#endif //OWEND case NONE: break; @@ -555,11 +559,9 @@ void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_t* // receiving this message is also used to check if the receiver is present // so let's first do the receiver detection if (_detected_protocol == AP_RCProtocol::NONE) { // still searching -//#ifndef IOMCU_FW -//? we need this, but originally it is enclosed by an #ifndef IOMCU_FW -// how does this work? +#if AP_RC_CHANNEL_ENABLED rc_protocols_mask = rc().enabled_protocols(); -//#endif +#endif if (!protocol_enabled(MAVLINK_RADIO)) return; // not our turn _detected_protocol = AP_RCProtocol::MAVLINK_RADIO; } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index 10e7b0bb1249b..884d9f5e0d954 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -72,7 +72,9 @@ class AP_RCProtocol { DRONECAN = 13, #endif //OW RADIOLINK +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED MAVLINK_RADIO = 14, // RC_PROTOCOLS +2^15 = 32768 +#endif //OWEND NONE //last enum always is None }; @@ -154,7 +156,9 @@ class AP_RCProtocol { case DRONECAN: #endif //OW RADIOLINK +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED case MAVLINK_RADIO: +#endif //OWEND case NONE: return false; @@ -203,6 +207,7 @@ class AP_RCProtocol { } //OW RADIOLINK + // handle mavlink radio void handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet); void handle_radio_link_stats(mavlink_radio_link_stats_t* packet); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp index f113a72aeaa2d..7074a457f8371 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp @@ -4,17 +4,19 @@ // for mLRS //***************************************************** -#include "AP_RCProtocol_MavlinkRadio.h" -#include +#include "AP_RCProtocol_config.h" +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED -AP_RCProtocol_MavlinkRadio::AP_RCProtocol_MavlinkRadio(AP_RCProtocol &_frontend) : - AP_RCProtocol_Backend(_frontend) -{ -} +#include "AP_RCProtocol_MAVLinkRadio.h" +// constructor +AP_RCProtocol_MAVLinkRadio::AP_RCProtocol_MAVLinkRadio(AP_RCProtocol &_frontend) : + AP_RCProtocol_Backend(_frontend) +{} -void AP_RCProtocol_MavlinkRadio::update(void) +// update function to populate input +void AP_RCProtocol_MAVLinkRadio::update(void) { if (frontend.mavlink_radio.rc_channels_updated) { frontend.mavlink_radio.rc_channels_updated = false; @@ -47,3 +49,6 @@ void AP_RCProtocol_MavlinkRadio::update(void) } } } + +#endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h index 74efd5040c4cd..fe973d853d624 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h @@ -5,13 +5,17 @@ //***************************************************** #pragma once +#include "AP_RCProtocol_config.h" + +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + #include "AP_RCProtocol.h" -class AP_RCProtocol_MavlinkRadio : public AP_RCProtocol_Backend { +class AP_RCProtocol_MAVLinkRadio : public AP_RCProtocol_Backend { public: - AP_RCProtocol_MavlinkRadio(AP_RCProtocol &_frontend); + AP_RCProtocol_MAVLinkRadio(AP_RCProtocol &_frontend); void update(void) override; @@ -20,3 +24,6 @@ class AP_RCProtocol_MavlinkRadio : public AP_RCProtocol_Backend { int16_t rssi = -1; // TODO: can't we just use the backend's fields??? int16_t link_quality = -1; }; + +#endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_config.h b/libraries/AP_RCProtocol/AP_RCProtocol_config.h index 5619c5373c081..89bbd15f5540c 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_config.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_config.h @@ -61,3 +61,9 @@ #ifndef AP_RCPROTOCOL_FASTSBUS_ENABLED #define AP_RCPROTOCOL_FASTSBUS_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && AP_RCPROTOCOL_SBUS_ENABLED #endif + +//OW +#ifndef AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED +#define AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED +#endif +//OWEND diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 5eff4c73cf743..fddf303dbee18 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4113,12 +4113,14 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) //OW RADIOLINK // like with old RADIO_STATUS, the messages // MAVLINK_MSG_ID_RADIO_LINK_STATS, MAVLINK_MSG_ID_RADIO_LINK_FLOW_CONTROL are handled in the vehicle's GCS_Mavlink.cpp +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED case MAVLINK_MSG_ID_RADIO_RC_CHANNELS: handle_radio_rc_channels(msg); #if HAL_MOUNT_ENABLED handle_mount_message(msg); #endif break; +#endif //OWEND #if AP_OPTICALFLOW_ENABLED @@ -6433,6 +6435,7 @@ bool GCS_MAVLINK::accept_packet(const mavlink_status_t &status, } //OW RADIOLINK +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED // handle messages from a mavlink receiver // we currently narrow it down to "ours" to play it safe if ((msg.compid == MAV_COMP_ID_TELEMETRY_RADIO) && @@ -6440,6 +6443,7 @@ bool GCS_MAVLINK::accept_packet(const mavlink_status_t &status, msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_FLOW_CONTROL)) { return true; } +#endif //OWEND if (!sysid_enforce()) { @@ -6816,6 +6820,8 @@ MAV_RESULT GCS_MAVLINK::handle_control_high_latency(const mavlink_command_int_t #endif // HAL_HIGH_LATENCY2_ENABLED //OW RADIOLINK +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + void GCS_MAVLINK::handle_radio_link_stats(const mavlink_message_t &msg, bool log_radio) { mavlink_radio_link_stats_t packet; @@ -6929,8 +6935,11 @@ void GCS_MAVLINK::handle_radio_rc_channels(const mavlink_message_t &msg) mavlink_radio_rc_channels_t packet; mavlink_msg_radio_rc_channels_decode(&msg, &packet); +#if AP_RCPROTOCOL_ENABLED AP::RC().handle_radio_rc_channels(&packet); +#endif } +#endif //OWEND #endif // HAL_GCS_ENABLED From 024ad9e7d50698789d99971240c1b1fa720c881d Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 28 Nov 2023 22:05:37 +0100 Subject: [PATCH 079/125] go to in class initialization --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 24 +------------------ libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 7 +++--- 2 files changed, 4 insertions(+), 27 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 6af6e2aecf446..ed793adc6a530 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -131,33 +131,11 @@ void BP_Mount_STorM32_MAVLink::init() { AP_Mount_Backend::init(); - _sysid = 0; - _compid = 0; // gimbal not yet discovered - _chan = MAVLINK_COMM_0; // this is a dummy, will be set correctly by find_gimbal() - - _got_device_info = false; - _initialised = false; - - _protocol = Protocol::UNDEFINED; - - _gimbal_armed = false; - _gimbal_prearmchecks_ok = false; - _armingchecks_running = 0; - _prearmchecks_passed = false; - _mode = MAV_MOUNT_MODE_RC_TARGETING; // irrelevant, will be later set to default in frontend init() - _mount_status = {}; - _device_status = {}; _flags_from_manager = UINT32_MAX; // the UINT32_MAX is important! - _flags_for_gimbal = 0; - _current_angles = {0.0f, 0.0f, 0.0f, NAN}; // the NAN is important! - _script_angles = {}; - _yaw_lock = false; // can't be currently supported, so we need to ensure this is false. This is important! - - _got_radio_rc_channels = false; // disable sending rc channels when RADIO_RC_CHANNELS messages are detected - _camera_mode = CameraMode::UNDEFINED; + _current_angles = {0.0f, 0.0f, 0.0f, NAN}; // the NAN is important! _should_log = true; // for now do always log } diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 270001c3148ce..0c29cf91755f4 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -258,7 +258,7 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend uint32_t received_tlast_ms; // time last GIMBAL_DEVICE_ATTITUDE_STATUS was received (used for health reporting) } _device_status; - uint32_t _flags_from_manager; // flags received by gimbal manager + uint32_t _flags_from_manager = UINT32_MAX; // flags received by gimbal manager, the UINT32_MAX is important! uint16_t _flags_for_gimbal; // flags to be send to gimbal device struct { @@ -266,7 +266,7 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend float pitch; float yaw_bf; float delta_yaw; - } _current_angles; // current angles, obtained from either MOUNT_STATUS or GIMBAL_DEVICE_ATTITUDE_STATUS + } _current_angles = {0.0f, 0.0f, 0.0f, NAN}; // current angles, obtained from MOUNT_STATUS or GIMBAL_DEVICE_ATTITUDE_STATUS, the NAN is important! Vector3f _current_omega; // current angular velocities, obtained from GIMBAL_DEVICE_ATTITUDE_STATUS struct { @@ -330,7 +330,6 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // logging - bool _should_log; - + bool _should_log = true; }; From fb9224f1d5a2468dd16b52336cb8d165c5dfecc4 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 29 Nov 2023 09:44:39 +0100 Subject: [PATCH 080/125] more radio rc channels --- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 14 ++--- libraries/AP_RCProtocol/AP_RCProtocol.h | 9 +--- .../AP_RCProtocol/AP_RCProtocol_Backend.h | 6 +++ .../AP_RCProtocol_MavlinkRadio.cpp | 51 +++++++++---------- .../AP_RCProtocol_MavlinkRadio.h | 4 +- .../AP_RCProtocol/AP_RCProtocol_config.h | 4 +- 6 files changed, 38 insertions(+), 50 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 392dc7eb5f732..be2f61d7d2061 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -572,12 +572,8 @@ void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_t* return; } - // update the field, so that the backend can see it - memcpy(&(mavlink_radio.rc_channels), packet, sizeof(mavlink_radio_rc_channels_t)); - mavlink_radio.rc_channels_updated = true; - // now update the backend - backend[_detected_protocol]->update(); + backend[_detected_protocol]->update_radio_rc_channels(packet); // now we can ask the backend if it got a new input if (backend[_detected_protocol]->new_input()) { @@ -586,7 +582,7 @@ void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_t* } }; -void AP_RCProtocol::handle_radio_link_stats(mavlink_radio_link_stats_t* packet) +void AP_RCProtocol::handle_radio_link_stats(const mavlink_radio_link_stats_t* packet) { // can be handled like CRSF (= receiver) or like RADIO_STATUS (= telemetry) // the user does decide it via RssiType::RECEIVER or RssiType::TELEMETRY_RADIO_RSSI setting @@ -597,12 +593,8 @@ void AP_RCProtocol::handle_radio_link_stats(mavlink_radio_link_stats_t* packet) return; } - // update the field, so that the backend can see it - memcpy(&(mavlink_radio.link_stats), packet, sizeof(mavlink_radio_link_stats_t)); - mavlink_radio.link_stats_updated = true; - // now update the backend - backend[_detected_protocol]->update(); + backend[_detected_protocol]->update_radio_link_stats(packet); } //OWEND diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index 884d9f5e0d954..cf471f9b5dc23 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -209,14 +209,7 @@ class AP_RCProtocol { //OW RADIOLINK // handle mavlink radio void handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet); - void handle_radio_link_stats(mavlink_radio_link_stats_t* packet); - - struct { - mavlink_radio_rc_channels_t rc_channels; - bool rc_channels_updated = false; - mavlink_radio_link_stats_t link_stats; - bool link_stats_updated = false; - } mavlink_radio; + void handle_radio_link_stats(const mavlink_radio_link_stats_t* packet); //OWEND private: diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index cbb0b1135be71..f363ab7fe680c 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -48,6 +48,12 @@ class AP_RCProtocol_Backend { PARSE_TYPE_SERIAL }; +//OW RADIOLINK + // update from mavlink messages + virtual void update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) {} + virtual void update_radio_link_stats(const mavlink_radio_link_stats_t* packet) {} +//END + // get number of frames, ignoring failsafe uint32_t get_rc_frame_count(void) const { return rc_frame_count; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp index 7074a457f8371..668314e16438c 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp @@ -15,38 +15,33 @@ AP_RCProtocol_MAVLinkRadio::AP_RCProtocol_MAVLinkRadio(AP_RCProtocol &_frontend) AP_RCProtocol_Backend(_frontend) {} -// update function to populate input -void AP_RCProtocol_MAVLinkRadio::update(void) +void AP_RCProtocol_MAVLinkRadio::update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) { - if (frontend.mavlink_radio.rc_channels_updated) { - frontend.mavlink_radio.rc_channels_updated = false; - - uint8_t count = frontend.mavlink_radio.rc_channels.count; - if (count >= MAX_RCIN_CHANNELS) count = MAX_RCIN_CHANNELS; - - uint16_t rc_chan[MAX_RCIN_CHANNELS]; - for (uint8_t i = 0; i < count; i++) { - rc_chan[i] = ((int32_t)frontend.mavlink_radio.rc_channels.channels[i] * 5) / 32 + 1500; - } + uint8_t count = packet->count; + if (count >= MAX_RCIN_CHANNELS) count = MAX_RCIN_CHANNELS; + + uint16_t rc_chan[MAX_RCIN_CHANNELS]; + for (uint8_t i = 0; i < count; i++) { + // The channel values are in centered 13 bit format. Range is [-4096,4096], center is 0. + // According to specification, the conversion to PWM is x * 5/32 + 1500. + rc_chan[i] = ((int32_t)packet->channels[i] * 5) / 32 + 1500; + } - bool failsafe = (frontend.mavlink_radio.rc_channels.flags & RADIO_RC_CHANNELS_FLAGS_FAILSAFE); + bool failsafe = (packet->flags & RADIO_RC_CHANNELS_FLAGS_FAILSAFE); - add_input(count, rc_chan, failsafe, rssi, link_quality); - } + add_input(count, rc_chan, failsafe, rssi, link_quality); +} - if (frontend.mavlink_radio.link_stats_updated) { - frontend.mavlink_radio.link_stats_updated = false; - - link_quality = frontend.mavlink_radio.link_stats.rx_LQ; - if (frontend.mavlink_radio.link_stats.rx_receive_antenna == UINT8_MAX || - frontend.mavlink_radio.link_stats.rx_rssi2 == UINT8_MAX) { - rssi = frontend.mavlink_radio.link_stats.rx_rssi1; - } else - if (frontend.mavlink_radio.link_stats.rx_receive_antenna == 1) { - rssi = frontend.mavlink_radio.link_stats.rx_rssi2; - } else { - rssi = frontend.mavlink_radio.link_stats.rx_rssi1; - } +void AP_RCProtocol_MAVLinkRadio::update_radio_link_stats(const mavlink_radio_link_stats_t* packet) +{ + link_quality = packet->rx_LQ; + if (packet->rx_receive_antenna == UINT8_MAX || packet->rx_rssi2 == UINT8_MAX) { + rssi = packet->rx_rssi1; + } else + if (packet->rx_receive_antenna == 1) { + rssi = packet->rx_rssi2; + } else { + rssi = packet->rx_rssi1; } } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h index fe973d853d624..a2df0546c6325 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h @@ -17,7 +17,9 @@ class AP_RCProtocol_MAVLinkRadio : public AP_RCProtocol_Backend { AP_RCProtocol_MAVLinkRadio(AP_RCProtocol &_frontend); - void update(void) override; + // update from mavlink messages + void update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) override; + void update_radio_link_stats(const mavlink_radio_link_stats_t* packet) override; private: diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_config.h b/libraries/AP_RCProtocol/AP_RCProtocol_config.h index 89bbd15f5540c..16ee2aa2df031 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_config.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_config.h @@ -62,8 +62,8 @@ #define AP_RCPROTOCOL_FASTSBUS_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && AP_RCPROTOCOL_SBUS_ENABLED #endif -//OW +//OW RADIOLINK #ifndef AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED -#define AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED +#define AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024 #endif //OWEND From 90261ed5ebbf968f61a6ec34a44cac38dfe553d9 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Thu, 30 Nov 2023 06:36:16 +0100 Subject: [PATCH 081/125] v bump --- libraries/bp_version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 59b27f732497d..7bf4779977c12 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,7 +1,7 @@ #pragma once #define BETAPILOTVERSION "v059c" -#define DATEOFBASEBRANCH "20231127" +#define DATEOFBASEBRANCH "20231129" /* search for //OW to find all changes From b67d23294a09a6f930998664a860786cdab1aea7 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Fri, 1 Dec 2023 03:22:01 +0100 Subject: [PATCH 082/125] radio link stats simplify/improve --- ArduCopter/GCS_Mavlink.cpp | 4 - libraries/AP_Logger/AP_Logger.h | 3 + libraries/AP_Logger/LogFile.cpp | 33 +++++ libraries/AP_Logger/LogStructure.h | 24 ++++ .../AP_RCProtocol/AP_RCProtocol_Backend.h | 3 + .../AP_RCProtocol_MavlinkRadio.cpp | 35 ++++- .../AP_RCProtocol_MavlinkRadio.h | 5 - libraries/GCS_MAVLink/GCS.h | 6 - libraries/GCS_MAVLink/GCS_Common.cpp | 127 +++--------------- libraries/GCS_MAVLink/MAVLink_routing.cpp | 3 +- 10 files changed, 111 insertions(+), 132 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 531a942b22043..2e5c131b94dbb 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1495,10 +1495,6 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) case MAVLINK_MSG_ID_RADIO_LINK_STATS: handle_radio_link_stats(msg, copter.should_log(MASK_LOG_PM)); break; - - case MAVLINK_MSG_ID_RADIO_LINK_FLOW_CONTROL: - handle_radio_link_flow_control(msg, copter.should_log(MASK_LOG_PM)); - break; //OWEND case MAVLINK_MSG_ID_TERRAIN_DATA: diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index cf6d232f85c58..5c12a3c5f93dd 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -251,6 +251,9 @@ class AP_Logger void Write_NamedValueFloat(const char *name, float value); void Write_Power(void); void Write_Radio(const mavlink_radio_t &packet); +//OW RADIOLINK + void Write_RadioLinkStats(const mavlink_radio_link_stats_t &packet); +//OWEND void Write_Message(const char *message); void Write_MessageF(const char *fmt, ...); void Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct, diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 0666943e94b87..3791308f9f9a9 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -395,6 +395,39 @@ void AP_Logger::Write_Radio(const mavlink_radio_t &packet) WriteBlock(&pkt, sizeof(pkt)); } +//OW RADIOLINK +void AP_Logger::Write_RadioLinkStats(const mavlink_radio_link_stats_t &packet) +{ + const struct log_RadioLinkStats pktrx{ + LOG_PACKET_HEADER_INIT(LOG_RADIO_LINK_STATS_MSG_RX), + time_us : AP_HAL::micros64(), + LQ : packet.rx_LQ, + rssi1 : packet.rx_rssi1, + snr1 : packet.rx_snr1, + rssi2 : packet.rx_rssi2, + snr2 : packet.rx_snr2, + receive_antenna : packet.rx_receive_antenna, + transmit_antenna : packet.rx_transmit_antenna, + flags : packet.flags + }; + WriteBlock(&pktrx, sizeof(pktrx)); + + const struct log_RadioLinkStats pkttx{ + LOG_PACKET_HEADER_INIT(LOG_RADIO_LINK_STATS_MSG_TX), + time_us : AP_HAL::micros64(), + LQ : packet.tx_LQ, + rssi1 : packet.tx_rssi1, + snr1 : packet.tx_snr1, + rssi2 : packet.tx_rssi2, + snr2 : packet.tx_snr2, + receive_antenna : packet.tx_receive_antenna, + transmit_antenna : packet.tx_transmit_antenna, + flags : packet.flags + }; + WriteBlock(&pkttx, sizeof(pkttx)); +} +//OWEND + void AP_Logger::Write_Compass_instance(const uint64_t time_us, const uint8_t mag_instance) { const Compass &compass = AP::compass(); diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 1259c6417cd75..08f527e8fecef 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -395,6 +395,21 @@ struct PACKED log_Radio { uint16_t fixed; }; +//OW RADIOLINK +struct PACKED log_RadioLinkStats { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t LQ; + uint8_t rssi1; + int8_t snr1; + uint8_t rssi2; + int8_t snr2; + uint8_t receive_antenna; + uint8_t transmit_antenna; + uint8_t flags; +}; +//OWEND + struct PACKED log_PID { LOG_PACKET_HEADER; uint64_t time_us; @@ -1237,6 +1252,10 @@ LOG_STRUCTURE_FROM_PRECLAND \ "MAVC", "QBBBBBHffffiifBB","TimeUS,TS,TC,SS,SC,Fr,Cmd,P1,P2,P3,P4,X,Y,Z,Res,WL", "s---------------", "F---------------" }, \ { LOG_RADIO_MSG, sizeof(log_Radio), \ "RAD", "QBBBBBHH", "TimeUS,RSSI,RemRSSI,TxBuf,Noise,RemNoise,RxErrors,Fixed", "s-------", "F-------", true }, \ + { LOG_RADIO_LINK_STATS_MSG_RX, sizeof(log_RadioLinkStats), \ + "RDRX", "QBBbBbBBB", "TimeUS,rxLQ,rxRssi1,rxSnr1,rxRssi2,rxSnr2,rxRAnt,rxTAnt,flags", "s--------", "F--------", true }, \ + { LOG_RADIO_LINK_STATS_MSG_TX, sizeof(log_RadioLinkStats), \ + "RDTX", "QBBbBbBBB", "TimeUS,txLQ,txRssi1,txSnr1,txRssi2,txSnr2,txRAnt,txTAnt,flags", "s--------", "F--------", true }, \ LOG_STRUCTURE_FROM_CAMERA \ LOG_STRUCTURE_FROM_MOUNT \ { LOG_ARSP_MSG, sizeof(log_ARSP), "ARSP", "QBffcffBBffB", "TimeUS,I,Airspeed,DiffPress,Temp,RawPress,Offset,U,H,Hp,TR,Pri", "s#nPOPP-----", "F-00B00-----", true }, \ @@ -1413,6 +1432,11 @@ enum LogMessages : uint8_t { LOG_RCOUT3_MSG, LOG_IDS_FROM_FENCE, +//OW RADIOLINK + LOG_RADIO_LINK_STATS_MSG_RX, + LOG_RADIO_LINK_STATS_MSG_TX, +//OWEND + _LOG_LAST_MSG_ }; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index f363ab7fe680c..29cd25e75d2c9 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -137,6 +137,9 @@ class AP_RCProtocol_Backend { uint16_t _pwm_values[MAX_RCIN_CHANNELS]; uint8_t _num_channels; +//OW RADIOLINK +protected: +//OWEND int16_t rssi = -1; int16_t rx_link_quality = -1; }; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp index 668314e16438c..eeeb6db13ba51 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp @@ -29,19 +29,44 @@ void AP_RCProtocol_MAVLinkRadio::update_radio_rc_channels(const mavlink_radio_rc bool failsafe = (packet->flags & RADIO_RC_CHANNELS_FLAGS_FAILSAFE); - add_input(count, rc_chan, failsafe, rssi, link_quality); + add_input(count, rc_chan, failsafe); } void AP_RCProtocol_MAVLinkRadio::update_radio_link_stats(const mavlink_radio_link_stats_t* packet) { - link_quality = packet->rx_LQ; + // update the backend's fields + + if (packet->rx_LQ != UINT8_MAX) rx_link_quality = packet->rx_LQ; + + int32_t _rssi = -1; + if (packet->rx_receive_antenna == UINT8_MAX || packet->rx_rssi2 == UINT8_MAX) { - rssi = packet->rx_rssi1; + // no diversity + if (packet->rx_rssi1 != UINT8_MAX) _rssi = packet->rx_rssi1; } else if (packet->rx_receive_antenna == 1) { - rssi = packet->rx_rssi2; + // diversity, receiving on antenna 1 + if (packet->rx_rssi2 != UINT8_MAX) _rssi = packet->rx_rssi2; // UINT8_MAX should not happen, but play it safe + } else { + // diversity, receiving on antenna 0 + if (packet->rx_rssi1 != UINT8_MAX) _rssi = packet->rx_rssi1; // UINT8_MAX should not happen, but play it safe + } + + if (_rssi == -1) return; // no rssi value set + + if (packet->flags & RADIO_LINK_STATS_FLAGS_RSSI_DBM) { + // rssi is in dBm, convert to AP rssi using the same logic as in CRSF driver + // AP rssi: -1 for unknown, 0 for no link, 255 for maximum link + if (_rssi < 50) { + rssi = 255; + } else if (_rssi > 120) { + rssi = 0; + } else { + rssi = int16_t(roundf((1.0f - (_rssi - 50.0f) / 70.0f) * 255.0f)); + } } else { - rssi = packet->rx_rssi1; + // _rssi is 0..254, scale it to 0..255 with rounding + rssi = (_rssi * 255 + 127) / 254; } } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h index a2df0546c6325..0f3e9734b99c5 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h @@ -20,11 +20,6 @@ class AP_RCProtocol_MAVLinkRadio : public AP_RCProtocol_Backend { // update from mavlink messages void update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) override; void update_radio_link_stats(const mavlink_radio_link_stats_t* packet) override; - -private: - - int16_t rssi = -1; // TODO: can't we just use the backend's fields??? - int16_t link_quality = -1; }; #endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 95fabcebe27dc..8b609445eb767 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -686,7 +686,6 @@ class GCS_MAVLINK //OW RADIOLINK // called from vehicle class handler void handle_radio_link_stats(const mavlink_message_t &msg, bool log_radio); - void handle_radio_link_flow_control(const mavlink_message_t &msg, bool log_radio); // called from common handler void handle_radio_rc_channels(const mavlink_message_t &msg); //OWEND @@ -753,11 +752,6 @@ class GCS_MAVLINK mavlink_message_t _channel_buffer; mavlink_status_t _channel_status; -//OW RADIOLINK - // for mavlink radio - mavlink_radio_t _mavlink_radio_packet; -//OWEND - const AP_SerialManager::UARTState *uartstate; // last time we got a non-zero RSSI from RADIO_STATUS diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index fddf303dbee18..20d7120dac135 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4111,8 +4111,6 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) handle_rc_channels_override(msg); break; //OW RADIOLINK -// like with old RADIO_STATUS, the messages -// MAVLINK_MSG_ID_RADIO_LINK_STATS, MAVLINK_MSG_ID_RADIO_LINK_FLOW_CONTROL are handled in the vehicle's GCS_Mavlink.cpp #if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED case MAVLINK_MSG_ID_RADIO_RC_CHANNELS: handle_radio_rc_channels(msg); @@ -6439,8 +6437,7 @@ bool GCS_MAVLINK::accept_packet(const mavlink_status_t &status, // handle messages from a mavlink receiver // we currently narrow it down to "ours" to play it safe if ((msg.compid == MAV_COMP_ID_TELEMETRY_RADIO) && - (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS || msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_STATS || - msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_FLOW_CONTROL)) { + (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS || msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_STATS)) { return true; } #endif @@ -6822,124 +6819,34 @@ MAV_RESULT GCS_MAVLINK::handle_control_high_latency(const mavlink_command_int_t //OW RADIOLINK #if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED -void GCS_MAVLINK::handle_radio_link_stats(const mavlink_message_t &msg, bool log_radio) +void GCS_MAVLINK::handle_radio_rc_channels(const mavlink_message_t &msg) { - mavlink_radio_link_stats_t packet; - mavlink_msg_radio_link_stats_decode(&msg, &packet); - - // let AP_RCProtocol do its thing - // is doing something if AP_RCProtocol::MAVLINK_RADIO is selected by the user - // will provide rssi if also AP_RSSI::RssiType::RECEIVER is selected by the user - AP::RC().handle_radio_link_stats(&packet); - - // handle the rssi info - // jump out if AP_RSSI::RssiType::TELEMETRY_RADIO_RSSI is not selected by the user - AP_RSSI* rssi = AP::rssi(); - if ((rssi != nullptr) && !rssi->enabled(AP_RSSI::RssiType::TELEMETRY_RADIO_RSSI)) return; - - // convert it to what one would get from RADIO_STATUS - uint8_t _rssi = packet.rx_rssi1; - uint8_t _remrssi = (packet.tx_rssi1 == UINT8_MAX) ? 0 : packet.tx_rssi1; - uint8_t _noise = 0; - uint8_t _remnoise = 0; - - if (packet.flags & RADIO_LINK_STATS_FLAGS_RSSI_DBM) { - // the rssi values are not in MAVLink standard, but negative dBm - // so convert using ArduPilot's CRSF conversion, see AP_RCProtocol_CRSF::process_link_stats_frame() - if (_rssi < 50) { - _rssi = 254; - } else if (_rssi > 120) { - _rssi = 0; - } else { - _rssi = int16_t(roundf((1.0f - (_rssi - 50.0f) / 70.0f) * 254.0f)); - } - if (_remrssi < 50) { - _remrssi = 254; - } else if (_remrssi > 120) { - _remrssi = 0; - } else { - _remrssi = int16_t(roundf((1.0f - (_remrssi - 50.0f) / 70.0f) * 254.0f)); - } - } - - // now do what it does for RADIO_STATUS - const uint32_t now = AP_HAL::millis(); - - last_radio_status.received_ms = now; - last_radio_status.rssi = _rssi; - - // record if the GCS has been receiving radio messages from the vehicle - if (_remrssi != 0) { - last_radio_status.remrssi_ms = now; - } - - // prepare for logging - _mavlink_radio_packet.rssi = _rssi; - _mavlink_radio_packet.remrssi = _remrssi; - _mavlink_radio_packet.noise = _noise; - _mavlink_radio_packet.remnoise = _remnoise; + mavlink_radio_rc_channels_t packet; + mavlink_msg_radio_rc_channels_decode(&msg, &packet); - // log rssi, noise, etc if logging Performance monitoring data - if (log_radio) { - AP::logger().Write_Radio(_mavlink_radio_packet); - } +#if AP_RCPROTOCOL_ENABLED + AP::RC().handle_radio_rc_channels(&packet); +#endif } -void GCS_MAVLINK::handle_radio_link_flow_control(const mavlink_message_t &msg, bool log_radio) +// AP_RSSI::RssiType::TELEMETRY_RADIO_RSSI -> rssi is taken from RADIO_STATUS +// AP_RSSI::RssiType::RECEIVER -> rssi is taken from RADIO_LINK_STATS +void GCS_MAVLINK::handle_radio_link_stats(const mavlink_message_t &msg, bool log_radio) { - mavlink_radio_link_flow_control_t packet; - mavlink_msg_radio_link_flow_control_decode(&msg, &packet); - - if (packet.txbuf == UINT8_MAX) return; - - // convert it to what we would get from RADIO_STATUS - uint8_t _txbuf = packet.txbuf; - - // now what it would do for RADIO_STATUS - last_txbuf = _txbuf; - - // use the state of the transmit buffer in the radio to - // control the stream rate, giving us adaptive software - // flow control - if (_txbuf < 20 && stream_slowdown_ms < 2000) { - // we are very low on space - slow down a lot - stream_slowdown_ms += 60; - } else if (_txbuf < 50 && stream_slowdown_ms < 2000) { - // we are a bit low on space, slow down slightly - stream_slowdown_ms += 20; - } else if (_txbuf > 95 && stream_slowdown_ms > 200) { - // the buffer has plenty of space, speed up a lot - stream_slowdown_ms -= 40; - } else if (_txbuf > 90 && stream_slowdown_ms != 0) { - // the buffer has enough space, speed up a bit - stream_slowdown_ms -= 20; - } + mavlink_radio_link_stats_t packet; + mavlink_msg_radio_link_stats_decode(&msg, &packet); -#if GCS_DEBUG_SEND_MESSAGE_TIMINGS - if (stream_slowdown_ms > max_slowdown_ms) { - max_slowdown_ms = stream_slowdown_ms; - } +#if AP_RCPROTOCOL_ENABLED + AP::RC().handle_radio_link_stats(&packet); #endif - // prepare for logging - _mavlink_radio_packet.txbuf = _txbuf; - - // log rssi, noise, etc if logging Performance monitoring data + // log link stats if logging Performance monitoring data if (log_radio) { - AP::logger().Write_Radio(_mavlink_radio_packet); + AP::logger().Write_RadioLinkStats(packet); } } -void GCS_MAVLINK::handle_radio_rc_channels(const mavlink_message_t &msg) -{ - mavlink_radio_rc_channels_t packet; - mavlink_msg_radio_rc_channels_decode(&msg, &packet); - -#if AP_RCPROTOCOL_ENABLED - AP::RC().handle_radio_rc_channels(&packet); -#endif -} -#endif +#endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED //OWEND #endif // HAL_GCS_ENABLED diff --git a/libraries/GCS_MAVLink/MAVLink_routing.cpp b/libraries/GCS_MAVLink/MAVLink_routing.cpp index b59c6ee338049..29fa2f41f0043 100644 --- a/libraries/GCS_MAVLink/MAVLink_routing.cpp +++ b/libraries/GCS_MAVLink/MAVLink_routing.cpp @@ -144,8 +144,7 @@ bool MAVLink_routing::check_and_forward(GCS_MAVLINK &in_link, const mavlink_mess // handle messages from a mavlink receiver as if it had target_system = our system, target_component = 0 // we currently narrow it down to "our" messages to play it safe if ((msg.compid == MAV_COMP_ID_TELEMETRY_RADIO) && - (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS || msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_STATS || - msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_FLOW_CONTROL)) { + (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS || msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_STATS)) { target_system = mavlink_system.sysid; target_component = 0; } From a0848166dd0ed58a164e24c1e8d1cd31f6dd0afe Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 2 Dec 2023 10:55:20 +0100 Subject: [PATCH 083/125] cc --- libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp index eeeb6db13ba51..75f02b5076ec5 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp @@ -36,7 +36,7 @@ void AP_RCProtocol_MAVLinkRadio::update_radio_link_stats(const mavlink_radio_lin { // update the backend's fields - if (packet->rx_LQ != UINT8_MAX) rx_link_quality = packet->rx_LQ; + rx_link_quality = (packet->rx_LQ != UINT8_MAX) ? packet->rx_LQ : -1; int32_t _rssi = -1; @@ -52,7 +52,10 @@ void AP_RCProtocol_MAVLinkRadio::update_radio_link_stats(const mavlink_radio_lin if (packet->rx_rssi1 != UINT8_MAX) _rssi = packet->rx_rssi1; // UINT8_MAX should not happen, but play it safe } - if (_rssi == -1) return; // no rssi value set + if (_rssi == -1) { // no rssi value set + rssi = -1; + return; + } if (packet->flags & RADIO_LINK_STATS_FLAGS_RSSI_DBM) { // rssi is in dBm, convert to AP rssi using the same logic as in CRSF driver From 7d233e2ee38dd20c0fb6d0673b3f985907f3f178 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 6 Dec 2023 06:30:52 +0100 Subject: [PATCH 084/125] HAL_MOUNT_BP_STORM32_MAVLINK_ENABLED --- libraries/AP_Mount/AP_Mount.cpp | 2 ++ libraries/AP_Mount/AP_Mount.h | 2 ++ libraries/AP_Mount/AP_Mount_config.h | 7 +++++++ libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 9 ++++++++- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 6 ++++-- 5 files changed, 23 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 4b691317d3594..d1b3597cdbe51 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -155,11 +155,13 @@ void AP_Mount::init() #endif // HAL_MOUNT_VIEWPRO_ENABLED //OW +#if HAL_MOUNT_BP_STORM32_MAVLINK_ENABLED // check for STorM32_MAVLink mounts using MAVLink protocol case Type::STorM32_MAVLink: _backends[instance] = new BP_Mount_STorM32_MAVLink(*this, _params[instance], instance); _num_instances++; break; +#endif // HAL_MOUNT_STORM32_MAVLINK_ENABLED //OWEND } diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index e9531b95e2352..987f83270325b 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -122,7 +122,9 @@ class AP_Mount Viewpro = 11, /// Viewpro gimbal using a custom serial protocol #endif //OW +#if HAL_MOUNT_BP_STORM32_MAVLINK_ENABLED STorM32_MAVLink = 83 +#endif //OWEND }; diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index 9159653dcc79e..5b3c536f8cec4 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/libraries/AP_Mount/AP_Mount_config.h @@ -42,6 +42,12 @@ #define HAL_MOUNT_STORM32SERIAL_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED #endif +//OW +#ifndef HAL_MOUNT_BP_STORM32_MAVLINK_ENABLED +#define HAL_MOUNT_BP_STORM32_MAVLINK_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED +#endif +//OWEND + #ifndef HAL_MOUNT_XACTI_ENABLED #define HAL_MOUNT_XACTI_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS && BOARD_FLASH_SIZE > 1024 #endif @@ -53,3 +59,4 @@ #ifndef AP_MOUNT_POI_TO_LATLONALT_ENABLED #define AP_MOUNT_POI_TO_LATLONALT_ENABLED HAL_MOUNT_ENABLED && AP_TERRAIN_AVAILABLE && BOARD_FLASH_SIZE > 1024 #endif + diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index ed793adc6a530..a2bb29a989154 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -4,6 +4,10 @@ // STorM32 mount backend class //***************************************************** +#include "BP_Mount_STorM32_MAVLink.h" + +#if HAL_MOUNT_BP_STORM32_MAVLINK_ENABLED + #include #include #include @@ -15,7 +19,6 @@ #include #include #include -#include "BP_Mount_STorM32_MAVLink.h" extern const AP_HAL::HAL& hal; @@ -1484,6 +1487,9 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_device_attitude_status(mavlink_channe } +#endif // HAL_MOUNT_BP_STORM32_MAVLINK_ENABLED + + /* STorM32-Link tests @@ -1567,3 +1573,4 @@ from tests 2021-08-28, in loiter with takeoff/land button, I conclude with taking-off & landing in loiter with sticks, we get the same behavior at takeoff, but when landing the state MAV_LANDED_STATE_LANDING is not there, makes sense as we just drop to ground */ + diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 0c29cf91755f4..8cdaac2a7030d 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -5,9 +5,9 @@ //***************************************************** #pragma once -#include "AP_Mount.h" #include "AP_Mount_Backend.h" -#include "AP_Camera/AP_Camera_shareddefs.h" + +#if HAL_MOUNT_BP_STORM32_MAVLINK_ENABLED class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend @@ -333,3 +333,5 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend bool _should_log = true; }; + +#endif // HAL_MOUNT_BP_STORM32_MAVLINK_ENABLED From 86641cbed3c10eaf4de3c4cca069518233094bed Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 13 Dec 2023 18:01:02 +0100 Subject: [PATCH 085/125] MAV_LANDED_STATE_PREPARING_FOR_TAKEOFF 5 -> 128 --- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 24 +++++++++---------- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 1 + 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index a2bb29a989154..0b1da11fae38b 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -589,6 +589,10 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_information(const mavlink_me if (degrees(_device_info.yaw_max) < _params.yaw_angle_max) _params.yaw_angle_max.set(degrees(_device_info.yaw_max)); } + // extract version int + // v2.68b <-> firmware_version 00 01(a) 68 2 <-> version int 268 (ignore char part) + _device_version_int = (_device_info.firmware_version & 0x000000FF) * 100 + ((_device_info.firmware_version & 0x0000FF00) >> 8); + // mark it as having been found _got_device_info = true; @@ -654,9 +658,11 @@ void BP_Mount_STorM32_MAVLink::handle_message_extra(const mavlink_message_t &msg } // listen to RADIO_RC_CHANNELS messages to stop sending RC_CHANNELS +#ifdef MAVLINK_MSG_ID_RADIO_RC_CHANNELS if (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS) { // 60045 _got_radio_rc_channels = true; } +#endif // this msg is not from our gimbal if (msg.sysid != _sysid || msg.compid != _compid) { @@ -779,14 +785,6 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude() } -// ensure that MAV_LANDED_STATE enum hasn't been extended -static_assert((uint8_t)MAV_LANDED_STATE_ENUM_END == 5, "MAV_LANDED_STATE enum has changed"); - -enum LandedStateThisWouldBeGreatToHave { - MAV_LANDED_STATE_PREPARING_FOR_TAKEOFF = 5, -}; - - void BP_Mount_STorM32_MAVLink::send_autopilot_state_for_gimbal_device() { if (!HAVE_PAYLOAD_SPACE(_chan, AUTOPILOT_STATE_FOR_GIMBAL_DEVICE)) { @@ -922,13 +920,15 @@ landed state: uint8_t landed_state = (uint8_t)gcs().get_landed_state(); // AP::vehicle()->get_landed_state(); #if APM_BUILD_TYPE(APM_BUILD_ArduCopter) - // for copter the landed state is modified such as to reflect the 2 sec pre-take-off period. + // MAV_LANDED_STATE_PREPARING_FOR_TAKEOFF: <= v2.69 5, >= v2.70 128 + // For copter the landed state is modified such as to reflect the 2 sec pre-take-off period. // The code below leads to a PREPARING_FOR_TAKEOFF before takeoff, but also after landing! - // For the latter one would have to catch that it was flying, but no need to care as - // the gimbal will do inits when ON_GROUND, and refreshes them when transitioning to PREPARING_FOR_TAKEOFF. + // For the latter one would have to catch that it was flying, but no need to care as the gimbal + // will do its inits when ON_GROUND, and refreshes them when transitioning to PREPARING_FOR_TAKEOFF. // It won't do it for other transitions, so e.g. also not for plane. + // Could make sense for other vehicles too, like rover. if ((landed_state == MAV_LANDED_STATE_ON_GROUND) && AP::notify().flags.armed) { - landed_state = MAV_LANDED_STATE_PREPARING_FOR_TAKEOFF; + landed_state = (_device_version_int < 270) ? 5 : 128; } #endif diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 8cdaac2a7030d..035a097107776 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -199,6 +199,7 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend uint32_t _request_device_info_tlast_ms; mavlink_gimbal_device_information_t _device_info; + uint16_t _device_version_int; // request GIMBAL_DEVICE_INFORMATION, we can get this also if 'old' MOUNT messages are used void send_cmd_request_gimbal_device_information(); From 6d56e3d39c1d3cf00bf88b3e10f1015246ced3be Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 13 Dec 2023 18:36:44 +0100 Subject: [PATCH 086/125] master bump --- libraries/bp_version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 7bf4779977c12..ff406a60bfceb 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,7 +1,7 @@ #pragma once #define BETAPILOTVERSION "v059c" -#define DATEOFBASEBRANCH "20231129" +#define DATEOFBASEBRANCH "20231213" /* search for //OW to find all changes From 509ef4faeee9ebf82ea48696d1e888117661cff9 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sun, 24 Dec 2023 12:23:19 +0100 Subject: [PATCH 087/125] plane radio link stats --- ArduPlane/GCS_Mavlink.cpp | 5 +++++ libraries/bp_version.h | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 9427b99e0cdac..404b4524719fe 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1233,6 +1233,11 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg) handle_radio_status(msg, plane.should_log(MASK_LOG_PM)); break; } +//OW RADIOLINK + case MAVLINK_MSG_ID_RADIO_LINK_STATS: + handle_radio_link_stats(msg, plane.should_log(MASK_LOG_PM)); + break; +//OWEND case MAVLINK_MSG_ID_TERRAIN_DATA: case MAVLINK_MSG_ID_TERRAIN_CHECK: diff --git a/libraries/bp_version.h b/libraries/bp_version.h index ff406a60bfceb..1d8ccc0f0add5 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -28,7 +28,7 @@ on-top features: 2023.01.16: v055 aux functions AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_EXT - revise handling of RADIO_LINL_STATS a bit, add some comments + revise handling of RADIO_LINK_STATS a bit, add some comments do not send rc channels if RADIO_RC_CHANNELS is detected put mLRS/RADLIO_LINK_xxx messages under the hood of storm32.xml upgraded to Copter4.3.2 From 8a7bb4daa0caf13e1e58a48ca2589b92512a75ec Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 26 Dec 2023 14:58:09 +0100 Subject: [PATCH 088/125] snapshot - cleanup, more gimbal manager stuff --- libraries/AP_Mount/AP_Mount.cpp | 83 ++++++++++++++++--- libraries/AP_Mount/AP_Mount.h | 9 +- libraries/AP_Mount/AP_Mount_Backend.cpp | 26 +++++- libraries/AP_Mount/AP_Mount_Backend.h | 6 ++ .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 73 +++++++++++----- libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 7 +- libraries/bp_version.h | 1 + 7 files changed, 164 insertions(+), 41 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index d1b3597cdbe51..fe086a1fb33c0 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -338,10 +338,36 @@ MAV_RESULT AP_Mount::handle_command_do_mount_control(const mavlink_command_int_t return backend->handle_command_do_mount_control(packet); } -MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet) +//OW +AP_Mount_Backend* AP_Mount::get_backend_from_gimbal_device_id(const uint8_t id) { - AP_Mount_Backend *backend; + // check gimbal device id. + // 0 is primary, 1 or MAV_COMP_ID_GIMBAL is 1st gimbal, 2 or MAV_COMP_ID_GIMBAL2 is 2nd gimbal, etc + if (id == 0) { + return get_primary(); + } + if (id == MAV_COMP_ID_GIMBAL) { + return get_instance(0); + } + if (id >= MAV_COMP_ID_GIMBAL2 && id <= MAV_COMP_ID_GIMBAL6) { + return get_instance(id - MAV_COMP_ID_GIMBAL2 + 1); + } + if (id <= 6) { + return get_instance(id - 1); + } + return nullptr; +} +//OWEND + +//OW +//MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet) +MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +//OWEND +{ + AP_Mount_Backend *backend; +//OW +/* // check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is // 2nd gimbal, etc const uint8_t instance = packet.z; @@ -355,8 +381,6 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com return MAV_RESULT_FAILED; } -//OW -/* // check flags for change to RETRACT const uint32_t flags = packet.x; if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) { @@ -369,6 +393,15 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com return MAV_RESULT_ACCEPTED; } */ + backend = get_backend_from_gimbal_device_id(packet.z); + +//OW +// if (backend == nullptr) { //OW ARGH TODO: need to also check for !backend->is_in_control(msg.sysid, msg.compid) !! + if (backend == nullptr || !backend->is_in_control(msg.sysid, msg.compid)) { +//OWEND + return MAV_RESULT_FAILED; + } + const uint32_t flags = packet.x; if (!backend->handle_gimbal_manager_flags(flags)) { @@ -401,7 +434,8 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { AP_Mount_Backend *backend; - +//OW +/* // check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is 2nd gimbal, etc const uint8_t instance = packet.z; if (instance == 0) { @@ -409,6 +443,9 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_configure(const mavlink_co } else { backend = get_instance(instance - 1); } +*/ + backend = get_backend_from_gimbal_device_id(packet.z); +//OWEND if (backend == nullptr) { return MAV_RESULT_FAILED; @@ -417,12 +454,14 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_configure(const mavlink_co return backend->handle_command_do_gimbal_manager_configure(packet, msg); } -void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg) { +void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg) +{ mavlink_gimbal_manager_set_attitude_t packet; mavlink_msg_gimbal_manager_set_attitude_decode(&msg,&packet); AP_Mount_Backend *backend; - +//OW +/* // check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is // 2nd gimbal, etc const uint8_t instance = packet.gimbal_device_id; @@ -436,8 +475,6 @@ void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg) return; } -//OW -/* // check flags for change to RETRACT const uint32_t flags = packet.flags; if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) { @@ -451,6 +488,12 @@ void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg) return; } */ + backend = get_backend_from_gimbal_device_id(packet.gimbal_device_id); + + if (backend == nullptr || !backend->is_in_control(msg.sysid, msg.compid)) { + return; + } + const uint32_t flags = packet.flags; if (!backend->handle_gimbal_manager_flags(flags)) { @@ -497,7 +540,8 @@ void AP_Mount::handle_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg) mavlink_msg_gimbal_manager_set_pitchyaw_decode(&msg,&packet); AP_Mount_Backend *backend; - +//OW +/* // check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is // 2nd gimbal, etc const uint8_t instance = packet.gimbal_device_id; @@ -511,8 +555,6 @@ void AP_Mount::handle_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg) return; } -//OW -/* // check flags for change to RETRACT uint32_t flags = (uint32_t)packet.flags; if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) { @@ -525,6 +567,18 @@ void AP_Mount::handle_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg) return; } */ + backend = get_backend_from_gimbal_device_id(packet.gimbal_device_id); + + if (backend == nullptr || !backend->is_in_control(msg.sysid, msg.compid)) { + return; + } + + // sanity check, should never be true + // it also checks if the gimbal is a MAVLink gimbal and jumps out if the gimbal is not yet found +// if ( packet.z > 6 && id != backend->get_gimbal_device_id()) { +// return; +// } + const uint32_t flags = packet.flags; if (!backend->handle_gimbal_manager_flags(flags)) { @@ -568,7 +622,10 @@ MAV_RESULT AP_Mount::handle_command(const mavlink_command_int_t &packet, const m case MAV_CMD_DO_MOUNT_CONTROL: return handle_command_do_mount_control(packet); case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: - return handle_command_do_gimbal_manager_pitchyaw(packet); +//OW +// return handle_command_do_gimbal_manager_pitchyaw(packet); + return handle_command_do_gimbal_manager_pitchyaw(packet, msg); +//OWEND case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: return handle_command_do_gimbal_manager_configure(packet, msg); case MAV_CMD_DO_SET_ROI_SYSID: diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 987f83270325b..5907d2a8878ad 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -332,7 +332,10 @@ class AP_Mount MAV_RESULT handle_command_do_mount_configure(const mavlink_command_int_t &packet); MAV_RESULT handle_command_do_mount_control(const mavlink_command_int_t &packet); - MAV_RESULT handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet); +//OW +// MAV_RESULT handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet); + MAV_RESULT handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet, const mavlink_message_t &msg); +//OWEND MAV_RESULT handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg); void handle_gimbal_manager_set_attitude(const mavlink_message_t &msg); void handle_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg); @@ -340,6 +343,10 @@ class AP_Mount void handle_gimbal_device_information(const mavlink_message_t &msg); void handle_gimbal_device_attitude_status(const mavlink_message_t &msg); +//OW + AP_Mount_Backend* get_backend_from_gimbal_device_id(const uint8_t id); +//OWEND + // perform any required parameter conversion void convert_params(); }; diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index d480a5d30b344..8de955b457eb1 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -243,6 +243,13 @@ uint32_t AP_Mount_Backend::get_gimbal_manager_flags() const return flags; } +// return gimbal device id used by GIMBAL_MANAGER_STATUS message +uint8_t AP_Mount_Backend::get_gimbal_device_id() const +{ + return _instance + 1; +} + + // set gimbal manager flags, called from frontend's gimbal manager handlers bool AP_Mount_Backend::handle_gimbal_manager_flags(uint32_t flags) { @@ -256,6 +263,17 @@ bool AP_Mount_Backend::handle_gimbal_manager_flags(uint32_t flags) } return true; } + +bool AP_Mount_Backend::is_in_control(uint8_t sysid, uint8_t compid) +{ + if (mavlink_control_id.sysid == 0 && mavlink_control_id.compid == 0) { + mavlink_control_id.sysid = sysid; + mavlink_control_id.compid = compid; + return true; + } + + return (mavlink_control_id.sysid == sysid && mavlink_control_id.compid == compid); +} //OWEND @@ -270,12 +288,16 @@ void AP_Mount_Backend::send_gimbal_manager_status(mavlink_channel_t chan) flags |= GIMBAL_MANAGER_FLAGS_YAW_LOCK; } */ uint32_t flags = get_gimbal_manager_flags(); + uint8_t gimbal_device_id = get_gimbal_device_id(); //OWEND mavlink_msg_gimbal_manager_status_send(chan, AP_HAL::millis(), // autopilot system time flags, // bitmap of gimbal manager flags - _instance + 1, // gimbal device id +//OW +// _instance + 1, // gimbal device id + gimbal_device_id, // gimbal device id +//OWEND mavlink_control_id.sysid, // primary control system id mavlink_control_id.compid, // primary control component id 0, // secondary control system id @@ -857,7 +879,7 @@ void AP_Mount_Backend::set_mode_3pos(uint8_t ch_flag) set_mode(MAV_MOUNT_MODE_RETRACT); break; case 1: // = MIDDLE - if (_mode_last > MAV_MOUNT_MODE_NEUTRAL) set_mode(_mode_last); + if ((get_mode() != _mode_last) && (_mode_last > MAV_MOUNT_MODE_NEUTRAL)) set_mode(_mode_last); break; case 0: // LOW: set_mode((MAV_MOUNT_MODE)_params.default_mode.get()); diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index ada4218225522..0f95c21c9dc08 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -227,6 +227,12 @@ class AP_Mount_Backend // return gimbal manager flags used by GIMBAL_MANAGER_STATUS message virtual uint32_t get_gimbal_manager_flags() const; + // return gimbal device id used by GIMBAL_MANAGER_STATUS message + virtual uint8_t get_gimbal_device_id() const; + + // return true if source of gimbal manager message/command is in control + bool is_in_control(uint8_t sysid, uint8_t compid); + // handle gimbal manager flags received from gimbal manager messages // GIMBAL_MANAGER_FLAGS_RETRACT, GIMBAL_MANAGER_FLAGS_NEUTRAL are handled in frontend // and sets mode accordingly. Not so nice but it's not my cup of tea. diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 0b1da11fae38b..16cc7642a1160 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -136,7 +136,7 @@ void BP_Mount_STorM32_MAVLink::init() _mode = MAV_MOUNT_MODE_RC_TARGETING; // irrelevant, will be later set to default in frontend init() - _flags_from_manager = UINT32_MAX; // the UINT32_MAX is important! + _flags_from_gimbal_client = UINT32_MAX; // the UINT32_MAX is important! _current_angles = {0.0f, 0.0f, 0.0f, NAN}; // the NAN is important! @@ -187,9 +187,13 @@ void BP_Mount_STorM32_MAVLink::update() uint32_t tnow_ms = AP_HAL::millis(); - if ((tnow_ms - _checks_tlast_ms) >= 1000) { + if ((tnow_ms - _checks_tlast_ms) >= 1000) { // do every 1 sec _checks_tlast_ms = tnow_ms; update_checks(); + + if (_protocol == Protocol::GIMBAL_DEVICE) { + gcs().send_message(MSG_GIMBAL_MANAGER_STATUS); + } } if ((tnow_ms - _send_system_time_tlast_ms) >= 5000) { // every 5 sec is really plenty @@ -221,10 +225,17 @@ bool BP_Mount_STorM32_MAVLink::handle_gimbal_manager_flags(uint32_t flags) set_mode(MAV_MOUNT_MODE_NEUTRAL); } - _flags_from_manager = flags; + _flags_from_gimbal_client = flags; update_gimbal_device_flags(); + // if not in mavlink targeting don't accept the angle/rate settings + // this is needed since backend's set_angle_target(), set_rate_target() set mode to mavlink targeting + // dirty, I think one should change backend's functions, but also makes sense somewhat + if (get_mode() != MAV_MOUNT_MODE_MAVLINK_TARGETING) { + return false; + } + // driver currently does not support yaw LOCK // front-end is digesting GIMBAL_MANAGER_FLAGS_YAW_LOCK to determine yaw_is_earth_frame // we could make it to modify the flag, but for moment let's be happy. @@ -232,7 +243,7 @@ bool BP_Mount_STorM32_MAVLink::handle_gimbal_manager_flags(uint32_t flags) return false; // don't accept angle/rate setting } - // STorM32 expects one of them to be set, otherwise it rejects + // STorM32 expects that only one of them is set, otherwise it rejects if (!(flags & GIMBAL_MANAGER_FLAGS_YAW_IN_VEHICLE_FRAME) && !(flags & GIMBAL_MANAGER_FLAGS_YAW_IN_EARTH_FRAME)) { return false; // don't accept angle/rate setting } @@ -248,37 +259,46 @@ bool BP_Mount_STorM32_MAVLink::handle_gimbal_manager_flags(uint32_t flags) void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags() { - _flags_for_gimbal = 0; + _flags_for_gimbal_device = 0; + + // map mode switch (get_mode()) { case MAV_MOUNT_MODE_RETRACT: - _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RETRACT; + _flags_for_gimbal_device |= GIMBAL_DEVICE_FLAGS_RETRACT; break; case MAV_MOUNT_MODE_NEUTRAL: - _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_NEUTRAL; + _flags_for_gimbal_device |= GIMBAL_DEVICE_FLAGS_NEUTRAL; break; default: break; } // account for gimbal manager flags - if (_flags_from_manager != UINT32_MAX) { - if (_flags_from_manager & GIMBAL_MANAGER_FLAGS_RC_EXCLUSIVE) _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RC_EXCLUSIVE; - if (_flags_from_manager & GIMBAL_MANAGER_FLAGS_RC_MIXED) _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RC_MIXED; + + if (_flags_from_gimbal_client != UINT32_MAX) { + if (_flags_from_gimbal_client & GIMBAL_MANAGER_FLAGS_RC_EXCLUSIVE) { // exclusive overrules mixed + _flags_for_gimbal_device |= GIMBAL_DEVICE_FLAGS_RC_EXCLUSIVE; + } else + if (_flags_from_gimbal_client & GIMBAL_MANAGER_FLAGS_RC_MIXED) { + _flags_for_gimbal_device |= GIMBAL_DEVICE_FLAGS_RC_MIXED; + } } else { // for as long as no gimbal manager message has been sent to the fc, enable rc mixed. // Should avoid user confusion when choosing the gimbal device operation mode. - _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_RC_MIXED; + _flags_for_gimbal_device |= GIMBAL_DEVICE_FLAGS_RC_MIXED; } // driver currently does not support pitch,roll follow, only pitch,roll lock - _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK; + _flags_for_gimbal_device |= GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK; // driver currently does not support yaw lock, only yaw follow // if (_is_yaw_lock) _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_LOCK; + // frame flags + // set either YAW_IN_VEHICLE_FRAME or YAW_IN_EARTH_FRAME, to indicate new message format, STorM32 will reject otherwise - _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME; + _flags_for_gimbal_device |= GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME; } @@ -371,25 +391,25 @@ void BP_Mount_STorM32_MAVLink::update_target_angles() } break; - // point mount to Home location + // point mount to another vehicle // ATTENTION: angle_rad.yaw_is_ef = true // -> ANGLE - case MAV_MOUNT_MODE_HOME_LOCATION: - if (get_angle_target_to_home(mnt_target.angle_rad)) { + case MAV_MOUNT_MODE_SYSID_TARGET: + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { mnt_target.target_type = MountTargetType::ANGLE; mnt_target_loc_valid = true; - mnt_target_loc = AP::ahrs().get_home(); + mnt_target_loc = _target_sysid_location; } break; - // point mount to another vehicle + // point mount to Home location // ATTENTION: angle_rad.yaw_is_ef = true // -> ANGLE - case MAV_MOUNT_MODE_SYSID_TARGET: - if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + case MAV_MOUNT_MODE_HOME_LOCATION: + if (get_angle_target_to_home(mnt_target.angle_rad)) { mnt_target.target_type = MountTargetType::ANGLE; mnt_target_loc_valid = true; - mnt_target_loc = _target_sysid_location; + mnt_target_loc = AP::ahrs().get_home(); } break; @@ -770,7 +790,7 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude() mavlink_msg_gimbal_device_set_attitude_send( _chan, _sysid, _compid, - _flags_for_gimbal, // gimbal device flags + _flags_for_gimbal_device, // gimbal device flags qa, // attitude as a quaternion NAN, NAN, NAN // angular velocities ); @@ -779,7 +799,7 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude() BP_LOG("MTC0", BP_LOG_MTC_GIMBALCONTROL_HEADER, (uint8_t)1, // Type, GIMBAL_DEVICE_SET_ATTITUDE degrees(mnt_target.angle_rad.roll), degrees(mnt_target.angle_rad.pitch), degrees(target_yaw_bf), // Roll, Pitch, Yaw - (uint16_t)_flags_for_gimbal, (uint16_t)0, // GDFlags, GMFlags + (uint16_t)_flags_for_gimbal_device, (uint16_t)0, // GDFlags, GMFlags (uint8_t)mnt_target.target_type, // TMode (uint8_t)0); // QMode } @@ -1124,6 +1144,13 @@ uint32_t BP_Mount_STorM32_MAVLink::get_gimbal_manager_flags() const } +// return gimbal device id used by GIMBAL_MANAGER_STATUS message +uint8_t BP_Mount_STorM32_MAVLink::get_gimbal_device_id() const +{ + return _compid; +} + + //------------------------------------------------------ // Prearm & healthy functions //------------------------------------------------------ diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index 035a097107776..eb1862ec658a7 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -104,6 +104,9 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // => we need to overwrite it uint32_t get_gimbal_manager_flags() const override; + // added: return gimbal device id used by GIMBAL_MANAGER_STATUS message + uint8_t get_gimbal_device_id() const override; + // added: handle gimbal manager flags received from gimbal manager messages // AP nonsense: does it wrong, just good for its limits. // => we need to overwrite it @@ -259,8 +262,8 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend uint32_t received_tlast_ms; // time last GIMBAL_DEVICE_ATTITUDE_STATUS was received (used for health reporting) } _device_status; - uint32_t _flags_from_manager = UINT32_MAX; // flags received by gimbal manager, the UINT32_MAX is important! - uint16_t _flags_for_gimbal; // flags to be send to gimbal device + uint32_t _flags_from_gimbal_client = UINT32_MAX; // flags received via gimbal manager messages/commands, the UINT32_MAX is important! + uint16_t _flags_for_gimbal_device; // flags to be send to gimbal device struct { float roll; diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 1d8ccc0f0add5..85ed0429b14ad 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -50,6 +50,7 @@ ArduCopter specific - version.h: 1x ArduPlane specific +- GCS_Mavlink.cpp: 1x RADIOLINK - ArduPlane.cpp: 1x - version.h: 1x From b42f2869322e489f7a6ba6f6f8c577c81e250b08 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 26 Dec 2023 22:32:06 +0100 Subject: [PATCH 089/125] more cleanup and little improvements, looks good --- libraries/AP_Camera/AP_Camera_Backend.cpp | 10 +- libraries/AP_Mount/AP_Mount.cpp | 205 ++++++++++-------- libraries/AP_Mount/AP_Mount.h | 19 +- libraries/AP_Mount/AP_Mount_Backend.cpp | 143 ++++++++++-- libraries/AP_Mount/AP_Mount_Backend.h | 14 +- .../AP_Mount/BP_Mount_STorM32_MAVLink.cpp | 7 - libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h | 13 +- libraries/bp_version.h | 2 +- 8 files changed, 279 insertions(+), 134 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/libraries/AP_Camera/AP_Camera_Backend.cpp index f6cf2c2d2b608..24eb9ae2d32f9 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.cpp +++ b/libraries/AP_Camera/AP_Camera_Backend.cpp @@ -111,15 +111,17 @@ uint8_t AP_Camera_Backend::get_gimbal_device_id() const const uint8_t mount_instance = get_mount_instance(); AP_Mount* mount = AP::mount(); if (mount != nullptr) { - if (mount->get_mount_type(mount_instance) != AP_Mount::Type::None) { - return (mount_instance + 1); - } +//OW +// if (mount->get_mount_type(mount_instance) != AP_Mount::Type::None) { +// return (mount_instance + 1); +// } + return mount->get_gimbal_device_id(mount_instance); // returns 0 if instance not available +//OWEND } #endif return 0; } - // take a picture. returns true on success bool AP_Camera_Backend::take_picture() { diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index fe086a1fb33c0..dcdf7ed6aa1c3 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -315,6 +315,8 @@ void AP_Mount::set_rate_target(uint8_t instance, float roll_degs, float pitch_de backend->set_rate_target(roll_degs, pitch_degs, yaw_degs, yaw_lock); } +//OW +/* MAV_RESULT AP_Mount::handle_command_do_mount_configure(const mavlink_command_int_t &packet) { auto *backend = get_primary(); @@ -327,7 +329,6 @@ MAV_RESULT AP_Mount::handle_command_do_mount_configure(const mavlink_command_int return MAV_RESULT_ACCEPTED; } - MAV_RESULT AP_Mount::handle_command_do_mount_control(const mavlink_command_int_t &packet) { auto *backend = get_primary(); @@ -338,36 +339,9 @@ MAV_RESULT AP_Mount::handle_command_do_mount_control(const mavlink_command_int_t return backend->handle_command_do_mount_control(packet); } -//OW -AP_Mount_Backend* AP_Mount::get_backend_from_gimbal_device_id(const uint8_t id) -{ - // check gimbal device id. - // 0 is primary, 1 or MAV_COMP_ID_GIMBAL is 1st gimbal, 2 or MAV_COMP_ID_GIMBAL2 is 2nd gimbal, etc - if (id == 0) { - return get_primary(); - } - if (id == MAV_COMP_ID_GIMBAL) { - return get_instance(0); - } - if (id >= MAV_COMP_ID_GIMBAL2 && id <= MAV_COMP_ID_GIMBAL6) { - return get_instance(id - MAV_COMP_ID_GIMBAL2 + 1); - } - if (id <= 6) { - return get_instance(id - 1); - } - - return nullptr; -} -//OWEND - -//OW -//MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet) -MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet, const mavlink_message_t &msg) -//OWEND +MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet) { AP_Mount_Backend *backend; -//OW -/* // check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is // 2nd gimbal, etc const uint8_t instance = packet.z; @@ -392,22 +366,6 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com backend->set_mode(MAV_MOUNT_MODE_NEUTRAL); return MAV_RESULT_ACCEPTED; } -*/ - backend = get_backend_from_gimbal_device_id(packet.z); - -//OW -// if (backend == nullptr) { //OW ARGH TODO: need to also check for !backend->is_in_control(msg.sysid, msg.compid) !! - if (backend == nullptr || !backend->is_in_control(msg.sysid, msg.compid)) { -//OWEND - return MAV_RESULT_FAILED; - } - - const uint32_t flags = packet.x; - - if (!backend->handle_gimbal_manager_flags(flags)) { - return MAV_RESULT_ACCEPTED; - } -//OWEND // param1 : pitch_angle (in degrees) // param2 : yaw angle (in degrees) @@ -434,8 +392,6 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { AP_Mount_Backend *backend; -//OW -/* // check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is 2nd gimbal, etc const uint8_t instance = packet.z; if (instance == 0) { @@ -443,9 +399,6 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_configure(const mavlink_co } else { backend = get_instance(instance - 1); } -*/ - backend = get_backend_from_gimbal_device_id(packet.z); -//OWEND if (backend == nullptr) { return MAV_RESULT_FAILED; @@ -460,8 +413,6 @@ void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg) mavlink_msg_gimbal_manager_set_attitude_decode(&msg,&packet); AP_Mount_Backend *backend; -//OW -/* // check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is // 2nd gimbal, etc const uint8_t instance = packet.gimbal_device_id; @@ -487,19 +438,6 @@ void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg) backend->set_mode(MAV_MOUNT_MODE_NEUTRAL); return; } -*/ - backend = get_backend_from_gimbal_device_id(packet.gimbal_device_id); - - if (backend == nullptr || !backend->is_in_control(msg.sysid, msg.compid)) { - return; - } - - const uint32_t flags = packet.flags; - - if (!backend->handle_gimbal_manager_flags(flags)) { - return; - } -//OWEND const Quaternion att_quat{packet.q}; const Vector3f att_rate_degs { @@ -540,8 +478,6 @@ void AP_Mount::handle_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg) mavlink_msg_gimbal_manager_set_pitchyaw_decode(&msg,&packet); AP_Mount_Backend *backend; -//OW -/* // check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is // 2nd gimbal, etc const uint8_t instance = packet.gimbal_device_id; @@ -566,25 +502,6 @@ void AP_Mount::handle_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg) backend->set_mode(MAV_MOUNT_MODE_NEUTRAL); return; } -*/ - backend = get_backend_from_gimbal_device_id(packet.gimbal_device_id); - - if (backend == nullptr || !backend->is_in_control(msg.sysid, msg.compid)) { - return; - } - - // sanity check, should never be true - // it also checks if the gimbal is a MAVLink gimbal and jumps out if the gimbal is not yet found -// if ( packet.z > 6 && id != backend->get_gimbal_device_id()) { -// return; -// } - - const uint32_t flags = packet.flags; - - if (!backend->handle_gimbal_manager_flags(flags)) { - return; - } -//OWEND // Do not allow both angle and rate to be specified at the same time if (!isnan(packet.pitch) && !isnan(packet.yaw) && !isnan(packet.pitch_rate) && !isnan(packet.yaw_rate)) { @@ -622,18 +539,128 @@ MAV_RESULT AP_Mount::handle_command(const mavlink_command_int_t &packet, const m case MAV_CMD_DO_MOUNT_CONTROL: return handle_command_do_mount_control(packet); case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + return handle_command_do_gimbal_manager_pitchyaw(packet); + case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: + return handle_command_do_gimbal_manager_configure(packet, msg); + case MAV_CMD_DO_SET_ROI_SYSID: + return handle_command_do_set_roi_sysid(packet); + default: + return MAV_RESULT_UNSUPPORTED; + } +} +*/ +//OWEND + //OW -// return handle_command_do_gimbal_manager_pitchyaw(packet); +uint8_t AP_Mount::get_gimbal_device_id(uint8_t instance) const +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return 0; + } + return backend->get_gimbal_device_id(); +} + +MAV_RESULT AP_Mount::handle_command_do_mount_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +{ + for (uint8_t instance=0; instanceis_in_control(msg.sysid, msg.compid, 0)) { + _backends[instance]->set_mode((MAV_MOUNT_MODE)packet.param1); + } + } + return MAV_RESULT_ACCEPTED; +} + +MAV_RESULT AP_Mount::handle_command_do_mount_control(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +{ + auto *backend = get_primary(); + if (backend == nullptr) { + return MAV_RESULT_FAILED; + } + if (backend->is_in_control(msg.sysid, msg.compid, 0)) { + return backend->handle_command_do_mount_control(packet); + } + return MAV_RESULT_FAILED; +} + +MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +{ + for (uint8_t instance=0; instanceget_gimbal_device_id() == (uint8_t)packet.z) { + _backends[instance]->handle_command_do_gimbal_manager_configure(packet, msg); + } + } + return MAV_RESULT_ACCEPTED; +} + +void AP_Mount::handle_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg) +{ + mavlink_gimbal_manager_set_pitchyaw_t packet; + mavlink_msg_gimbal_manager_set_pitchyaw_decode(&msg, &packet); + + for (uint8_t instance=0; instanceis_in_control(msg.sysid, msg.compid, packet.gimbal_device_id)) { + _backends[instance]->handle_gimbal_manager_set_pitchyaw(packet); + } + } +} + +void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg) +{ + mavlink_gimbal_manager_set_attitude_t packet; + mavlink_msg_gimbal_manager_set_attitude_decode(&msg, &packet); + + for (uint8_t instance=0; instanceis_in_control(msg.sysid, msg.compid, packet.gimbal_device_id)) { + _backends[instance]->handle_gimbal_manager_set_attitude(packet); + } + } +} + +MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +{ + for (uint8_t instance=0; instanceis_in_control(msg.sysid, msg.compid, packet.z)) { + _backends[instance]->handle_command_do_gimbal_manager_pitchyaw(packet); + } + } + return MAV_RESULT_ACCEPTED; +} + +MAV_RESULT AP_Mount::handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +{ + for (uint8_t instance=0; instanceis_in_control(msg.sysid, msg.compid, (uint8_t)packet.param2)) { + _backends[instance]->set_target_sysid((uint8_t)packet.param1); + } + } + return MAV_RESULT_ACCEPTED; +} + +MAV_RESULT AP_Mount::handle_command(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +{ + switch (packet.command) { + case MAV_CMD_DO_MOUNT_CONFIGURE: + return handle_command_do_mount_configure(packet, msg); + case MAV_CMD_DO_MOUNT_CONTROL: + return handle_command_do_mount_control(packet, msg); + case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: return handle_command_do_gimbal_manager_pitchyaw(packet, msg); -//OWEND case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: return handle_command_do_gimbal_manager_configure(packet, msg); case MAV_CMD_DO_SET_ROI_SYSID: - return handle_command_do_set_roi_sysid(packet); + return handle_command_do_set_roi_sysid(packet, msg); default: return MAV_RESULT_UNSUPPORTED; } } +//OWEND /// Change the configuration of the mount void AP_Mount::handle_global_position_int(const mavlink_message_t &msg) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 5907d2a8878ad..1a7eb39800377 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -197,7 +197,9 @@ class AP_Mount void set_target_sysid(uint8_t instance, uint8_t sysid); // handling of set_roi_sysid message - MAV_RESULT handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet); +//OW +// MAV_RESULT handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet); +//OWEND // mavlink message handling: MAV_RESULT handle_command(const mavlink_command_int_t &packet, const mavlink_message_t &msg); @@ -297,6 +299,9 @@ class AP_Mount // this links into handle_message() to catch all messages void handle_message_extra(mavlink_channel_t chan, const mavlink_message_t &msg); + // returns the gimbal device id for this instance, or 0 if the instance is not available + uint8_t get_gimbal_device_id(uint8_t instance) const; + // send banner void send_banner(); @@ -330,23 +335,23 @@ class AP_Mount void handle_mount_control(const mavlink_message_t &msg); #endif - MAV_RESULT handle_command_do_mount_configure(const mavlink_command_int_t &packet); - MAV_RESULT handle_command_do_mount_control(const mavlink_command_int_t &packet); //OW +// MAV_RESULT handle_command_do_mount_configure(const mavlink_command_int_t &packet); +// MAV_RESULT handle_command_do_mount_control(const mavlink_command_int_t &packet); // MAV_RESULT handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet); + MAV_RESULT handle_command_do_mount_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg); + MAV_RESULT handle_command_do_mount_control(const mavlink_command_int_t &packet, const mavlink_message_t &msg); MAV_RESULT handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet, const mavlink_message_t &msg); + MAV_RESULT handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet, const mavlink_message_t &msg); //OWEND MAV_RESULT handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg); void handle_gimbal_manager_set_attitude(const mavlink_message_t &msg); void handle_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg); + void handle_global_position_int(const mavlink_message_t &msg); void handle_gimbal_device_information(const mavlink_message_t &msg); void handle_gimbal_device_attitude_status(const mavlink_message_t &msg); -//OW - AP_Mount_Backend* get_backend_from_gimbal_device_id(const uint8_t id); -//OWEND - // perform any required parameter conversion void convert_params(); }; diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 8de955b457eb1..cfef0fbf5f78d 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -223,7 +223,10 @@ void AP_Mount_Backend::send_gimbal_manager_information(mavlink_channel_t chan) mavlink_msg_gimbal_manager_information_send(chan, AP_HAL::millis(), // autopilot system time get_gimbal_manager_capability_flags(), // bitmap of gimbal manager capability flags - _instance + 1, // gimbal device id +//OW +// _instance + 1, // gimbal device id + get_gimbal_device_id(), // gimbal device id +//OWEND radians(_params.roll_angle_min), // roll_min in radians radians(_params.roll_angle_max), // roll_max in radians radians(_params.pitch_angle_min), // pitch_min in radians @@ -233,6 +236,27 @@ void AP_Mount_Backend::send_gimbal_manager_information(mavlink_channel_t chan) } //OW +uint8_t AP_Mount_Backend::get_gimbal_device_id() const +{ + return _instance + 1; +} + +bool AP_Mount_Backend::is_in_control(uint8_t sysid, uint8_t compid, uint8_t gimbal_device_id) +{ + if (gimbal_device_id != 0 && gimbal_device_id != get_gimbal_device_id()) { + return false; + } + + // allow the source to claim control if nobody is in control + if (mavlink_control_id.sysid == 0 && mavlink_control_id.compid == 0) { + mavlink_control_id.sysid = sysid; + mavlink_control_id.compid = compid; + return true; + } + + return (mavlink_control_id.sysid == sysid && mavlink_control_id.compid == compid); +} + // return gimbal manager flags used by GIMBAL_MANAGER_STATUS message uint32_t AP_Mount_Backend::get_gimbal_manager_flags() const { @@ -243,13 +267,6 @@ uint32_t AP_Mount_Backend::get_gimbal_manager_flags() const return flags; } -// return gimbal device id used by GIMBAL_MANAGER_STATUS message -uint8_t AP_Mount_Backend::get_gimbal_device_id() const -{ - return _instance + 1; -} - - // set gimbal manager flags, called from frontend's gimbal manager handlers bool AP_Mount_Backend::handle_gimbal_manager_flags(uint32_t flags) { @@ -264,18 +281,107 @@ bool AP_Mount_Backend::handle_gimbal_manager_flags(uint32_t flags) return true; } -bool AP_Mount_Backend::is_in_control(uint8_t sysid, uint8_t compid) +void AP_Mount_Backend::handle_gimbal_manager_set_pitchyaw(const mavlink_gimbal_manager_set_pitchyaw_t &packet) { - if (mavlink_control_id.sysid == 0 && mavlink_control_id.compid == 0) { - mavlink_control_id.sysid = sysid; - mavlink_control_id.compid = compid; - return true; + const uint32_t flags = packet.flags; + + if (!handle_gimbal_manager_flags(flags)) { + return; } - return (mavlink_control_id.sysid == sysid && mavlink_control_id.compid == compid); + // Do not allow both angle and rate to be specified at the same time + if (!isnan(packet.pitch) && !isnan(packet.yaw) && !isnan(packet.pitch_rate) && !isnan(packet.yaw_rate)) { + return; + } + + // pitch and yaw from packet are in radians + if (!isnan(packet.pitch) && !isnan(packet.yaw)) { + const float pitch_angle_deg = degrees(packet.pitch); + const float yaw_angle_deg = degrees(packet.yaw); + set_angle_target(0, pitch_angle_deg, yaw_angle_deg, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); + return; + } + + // pitch_rate and yaw_rate from packet are in rad/s + if (!isnan(packet.pitch_rate) && !isnan(packet.yaw_rate)) { + const float pitch_rate_degs = degrees(packet.pitch_rate); + const float yaw_rate_degs = degrees(packet.yaw_rate); + set_rate_target(0, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); + } } -//OWEND +void AP_Mount_Backend::handle_gimbal_manager_set_attitude(const mavlink_gimbal_manager_set_attitude_t &packet) +{ + const uint32_t flags = packet.flags; + + if (!handle_gimbal_manager_flags(flags)) { + return; + } + + const Quaternion att_quat{packet.q}; + const Vector3f att_rate_degs { + packet.angular_velocity_x, + packet.angular_velocity_y, + packet.angular_velocity_y + }; + + // Do not allow both quaternion and angular velocity to be specified at the same time: + if (!att_quat.is_nan() && !att_rate_degs.is_nan()) { + return; + } + + if (!att_quat.is_nan()) { + // convert quaternion to euler angles + Vector3f attitude; + att_quat.to_euler(attitude); // attitude is in radians here + attitude *= RAD_TO_DEG; // convert to degrees + + set_angle_target(attitude.x, attitude.y, attitude.z, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); + return; + } + + if (!att_rate_degs.is_nan()) { + const float roll_rate_degs = degrees(packet.angular_velocity_x); + const float pitch_rate_degs = degrees(packet.angular_velocity_y); + const float yaw_rate_degs = degrees(packet.angular_velocity_z); + set_rate_target(roll_rate_degs, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); + } +} + +MAV_RESULT AP_Mount_Backend::handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet) +{ + const uint32_t flags = packet.x; + + if (!handle_gimbal_manager_flags(flags)) { + return MAV_RESULT_FAILED; + } + + // Do not allow both angle and rate to be specified at the same time + if (!isnan(packet.param1) && !isnan(packet.param2) && !isnan(packet.param3) && !isnan(packet.param4)) { + return MAV_RESULT_ACCEPTED; + } + + // param1 : pitch_angle (in degrees) + // param2 : yaw angle (in degrees) + const float pitch_angle_deg = packet.param1; + const float yaw_angle_deg = packet.param2; + if (!isnan(pitch_angle_deg) && !isnan(yaw_angle_deg)) { + set_angle_target(0, pitch_angle_deg, yaw_angle_deg, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); + return MAV_RESULT_ACCEPTED; + } + + // param3 : pitch_rate (in deg/s) + // param4 : yaw rate (in deg/s) + const float pitch_rate_degs = packet.param3; + const float yaw_rate_degs = packet.param4; + if (!isnan(pitch_rate_degs) && !isnan(yaw_rate_degs)) { + set_rate_target(0, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); + return MAV_RESULT_ACCEPTED; + } + + return MAV_RESULT_ACCEPTED; +} +//OWEND // send a GIMBAL_MANAGER_STATUS message to GCS void AP_Mount_Backend::send_gimbal_manager_status(mavlink_channel_t chan) @@ -287,16 +393,15 @@ void AP_Mount_Backend::send_gimbal_manager_status(mavlink_channel_t chan) if (_yaw_lock) { flags |= GIMBAL_MANAGER_FLAGS_YAW_LOCK; } */ - uint32_t flags = get_gimbal_manager_flags(); - uint8_t gimbal_device_id = get_gimbal_device_id(); //OWEND mavlink_msg_gimbal_manager_status_send(chan, AP_HAL::millis(), // autopilot system time - flags, // bitmap of gimbal manager flags //OW +// flags, // bitmap of gimbal manager flags // _instance + 1, // gimbal device id - gimbal_device_id, // gimbal device id + get_gimbal_manager_flags(), // bitmap of gimbal manager flags + get_gimbal_device_id(), // gimbal device id //OWEND mavlink_control_id.sysid, // primary control system id mavlink_control_id.compid, // primary control component id diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 0f95c21c9dc08..8bf2c42693e89 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -224,14 +224,14 @@ class AP_Mount_Backend // send banner virtual void send_banner() {} - // return gimbal manager flags used by GIMBAL_MANAGER_STATUS message - virtual uint32_t get_gimbal_manager_flags() const; - - // return gimbal device id used by GIMBAL_MANAGER_STATUS message + // return gimbal device id used by GIMBAL_MANAGER_STATUS message, and other places virtual uint8_t get_gimbal_device_id() const; // return true if source of gimbal manager message/command is in control - bool is_in_control(uint8_t sysid, uint8_t compid); + bool is_in_control(uint8_t sysid, uint8_t compid, uint8_t gimbal_device_id); + + // return gimbal manager flags used by GIMBAL_MANAGER_STATUS message + virtual uint32_t get_gimbal_manager_flags() const; // handle gimbal manager flags received from gimbal manager messages // GIMBAL_MANAGER_FLAGS_RETRACT, GIMBAL_MANAGER_FLAGS_NEUTRAL are handled in frontend @@ -240,6 +240,10 @@ class AP_Mount_Backend // Return false to abort angle/rate processing. virtual bool handle_gimbal_manager_flags(uint32_t flags); + void handle_gimbal_manager_set_pitchyaw(const mavlink_gimbal_manager_set_pitchyaw_t &packet); + void handle_gimbal_manager_set_attitude(const mavlink_gimbal_manager_set_attitude_t &packet); + MAV_RESULT handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet); + // used for scripting virtual bool take_control() { return false; } virtual bool give_control() { return false; } diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp index 16cc7642a1160..2fc1a96059a2a 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp @@ -1144,13 +1144,6 @@ uint32_t BP_Mount_STorM32_MAVLink::get_gimbal_manager_flags() const } -// return gimbal device id used by GIMBAL_MANAGER_STATUS message -uint8_t BP_Mount_STorM32_MAVLink::get_gimbal_device_id() const -{ - return _compid; -} - - //------------------------------------------------------ // Prearm & healthy functions //------------------------------------------------------ diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h index eb1862ec658a7..072577285347b 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h @@ -104,8 +104,17 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend // => we need to overwrite it uint32_t get_gimbal_manager_flags() const override; - // added: return gimbal device id used by GIMBAL_MANAGER_STATUS message - uint8_t get_gimbal_device_id() const override; + // return gimbal device id + uint8_t get_gimbal_device_id() const override + { + if (_instance == 0) { + return MAV_COMP_ID_GIMBAL; + } else + if (_instance <= 5) { + return MAV_COMP_ID_GIMBAL2 + _instance - 1; + } + return MAV_COMP_ID_GIMBAL; // should not happen + } // added: handle gimbal manager flags received from gimbal manager messages // AP nonsense: does it wrong, just good for its limits. diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 85ed0429b14ad..f5406f98da8a5 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,6 +1,6 @@ #pragma once -#define BETAPILOTVERSION "v059c" +#define BETAPILOTVERSION "v059d" #define DATEOFBASEBRANCH "20231213" /* From 108487838853d13fd3eee32937e061f6ce471644 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 27 Dec 2023 10:08:36 +0100 Subject: [PATCH 090/125] we don't need it but it should be done --- libraries/AP_Mount/AP_Mount_Backend.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index cfef0fbf5f78d..0183b50be611b 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -180,7 +180,10 @@ void AP_Mount_Backend::send_gimbal_device_attitude_status(mavlink_channel_t chan 0, // failure flags (not supported) std::numeric_limits::quiet_NaN(), // delta_yaw (NaN for unknonw) std::numeric_limits::quiet_NaN(), // delta_yaw_velocity (NaN for unknonw) - _instance + 1); // gimbal_device_id +//OW +// _instance + 1); // gimbal_device_id + get_gimbal_device_id()); // gimbal_device_id +//OWEND } #endif From 3039b1847256f14d5b13612d3b16af2c4be1593e Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 27 Dec 2023 11:33:45 +0100 Subject: [PATCH 091/125] update mavlink --- bp_mavlink/common.xml | 23 ++++++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/bp_mavlink/common.xml b/bp_mavlink/common.xml index e2236472d4711..ffb8e4e37575c 100644 --- a/bp_mavlink/common.xml +++ b/bp_mavlink/common.xml @@ -1003,9 +1003,9 @@ Reach a certain target angle. - target angle, 0 is north - angular speed - direction: -1: counter clockwise, 1: clockwise + target angle [0-360]. Absolute angles: 0 is north. Relative angle: 0 is initial yaw. Direction set by param3. + angular speed + direction: -1: counter clockwise, 0: shortest direction, 1: clockwise 0: absolute angle, 1: relative offset Empty Empty @@ -2278,6 +2278,12 @@ Command is valid and is being executed. This will be followed by further progress updates, i.e. the component may send further COMMAND_ACK messages with result MAV_RESULT_IN_PROGRESS (at a rate decided by the implementation), and must terminate by sending a COMMAND_ACK message with final result of the operation. The COMMAND_ACK.progress field can be used to indicate the progress of the operation. There is no need for the sender to retry the command, but if done during execution, the component will return MAV_RESULT_IN_PROGRESS with an updated progress. + + Command is only accepted when sent as a COMMAND_LONG. + + + Command is only accepted when sent as a COMMAND_INT. + Result of mission operation (in a MISSION_ACK message). @@ -4627,6 +4633,17 @@ Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. Positive values are positive thrust, negative values are negative thrust. R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. + + A bitfield corresponding to the joystick buttons' 16-31 current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 16. + Set bits to 1 to indicate which of the following extension fields contain valid data: bit 0: pitch, bit 1: roll, bit 2: aux1, bit 3: aux2, bit 4: aux3, bit 5: aux4, bit 6: aux5, bit 7: aux6 + Pitch-only-axis, normalized to the range [-1000,1000]. Generally corresponds to pitch on vehicles with additional degrees of freedom. Valid if bit 0 of enabled_extensions field is set. Set to 0 if invalid. + Roll-only-axis, normalized to the range [-1000,1000]. Generally corresponds to roll on vehicles with additional degrees of freedom. Valid if bit 1 of enabled_extensions field is set. Set to 0 if invalid. + Aux continuous input field 1. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 2 of enabled_extensions field is set. 0 if bit 2 is unset. + Aux continuous input field 2. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 3 of enabled_extensions field is set. 0 if bit 3 is unset. + Aux continuous input field 3. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 4 of enabled_extensions field is set. 0 if bit 4 is unset. + Aux continuous input field 4. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 5 of enabled_extensions field is set. 0 if bit 5 is unset. + Aux continuous input field 5. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 6 of enabled_extensions field is set. 0 if bit 6 is unset. + Aux continuous input field 6. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 7 of enabled_extensions field is set. 0 if bit 7 is unset. The RAW values of the RC channels sent to the MAV to override info received from the RC radio. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. Note carefully the semantic differences between the first 8 channels and the subsequent channels From 8ea2b109ab9aea29f4a794d5066bdb081811aea3 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 27 Dec 2023 11:58:35 +0100 Subject: [PATCH 092/125] remove 3pos mount switch --- libraries/AP_Mount/AP_Mount.cpp | 14 -------------- libraries/AP_Mount/AP_Mount.h | 7 ------- libraries/AP_Mount/AP_Mount_Backend.cpp | 19 ------------------- libraries/AP_Mount/AP_Mount_Backend.h | 11 ----------- libraries/RC_Channel/RC_Channel.cpp | 18 +----------------- libraries/RC_Channel/RC_Channel.h | 5 ++--- 6 files changed, 3 insertions(+), 71 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index dcdf7ed6aa1c3..331db19a629c8 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -262,20 +262,6 @@ void AP_Mount::set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode) backend->set_mode(mode); } -//OW -// set_mode_3pos - sets the mount's retract or default mode from an aux switch -void AP_Mount::set_mode_3pos(uint8_t instance, uint8_t ch_flag) -{ - auto *backend = get_instance(instance); - if (backend == nullptr) { - return; - } - - // call backend's set_mode_3pos - backend->set_mode_3pos(ch_flag); -} -//OWEND - // set yaw_lock. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North) // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle void AP_Mount::set_yaw_lock(uint8_t instance, bool yaw_lock) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 1a7eb39800377..38244d2458eb7 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -162,13 +162,6 @@ class AP_Mount void set_mode_to_default() { set_mode_to_default(_primary); } void set_mode_to_default(uint8_t instance); -//OW - // set_mode_3pos - sets the mount's retract or default mode from an aux switch - // ch_flag 0 = LOW enters default mode, 1 = MIDDLE return to previous mode, 2 = HIGH enter retract mode - void set_mode_3pos(uint8_t ch_flag) { set_mode_3pos(_primary, ch_flag); } - void set_mode_3pos(uint8_t instance, uint8_t ch_flag); -//OWEND - // set yaw_lock. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North) // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle void set_yaw_lock(bool yaw_lock) { set_yaw_lock(_primary, yaw_lock); } diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 0183b50be611b..5c9edb8d800f4 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -977,23 +977,4 @@ void AP_Mount_Backend::send_warning_to_GCS(const char* warning_str) _last_warning_ms = now_ms; } -//OW -// set_mode_3pos - sets the mount's retract or default mode from an aux switch -void AP_Mount_Backend::set_mode_3pos(uint8_t ch_flag) -{ - switch (ch_flag) { - case 2: // = HIGH - if (get_mode() > MAV_MOUNT_MODE_NEUTRAL) _mode_last = get_mode(); - set_mode(MAV_MOUNT_MODE_RETRACT); - break; - case 1: // = MIDDLE - if ((get_mode() != _mode_last) && (_mode_last > MAV_MOUNT_MODE_NEUTRAL)) set_mode(_mode_last); - break; - case 0: // LOW: - set_mode((MAV_MOUNT_MODE)_params.default_mode.get()); - break; - } -} -//OWEND - #endif // HAL_MOUNT_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 8bf2c42693e89..799da2661f0ee 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -79,12 +79,6 @@ class AP_Mount_Backend // set mount's mode bool set_mode(enum MAV_MOUNT_MODE mode); -//OW - // set_mode_3pos - sets the mount's retract or default mode from an aux switch - // ch_flag 0 = LOW enters default mode, 1 = MIDDLE return to previous mode, 2 = HIGH enter retract mode - void set_mode_3pos(uint8_t ch_flag); -//OWEND - // set yaw_lock. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North) // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle //OW @@ -367,11 +361,6 @@ class AP_Mount_Backend uint8_t sysid; uint8_t compid; } mavlink_control_id; - -//OW - // aux switch handling - MAV_MOUNT_MODE _mode_last; // previous mode, used in set_mode_3pos() -//OWEND }; #endif // HAL_MOUNT_ENABLED diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 697632dba884a..1c5189a554848 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -693,9 +693,6 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const AuxSwitchPo #if HAL_MOUNT_ENABLED case AUX_FUNC::RETRACT_MOUNT1: case AUX_FUNC::MOUNT_LOCK: -//OW - case AUX_FUNC::RETRACT_MOUNT1_3POS: -//OWEND #endif case AUX_FUNC::LOG_PAUSE: case AUX_FUNC::ARM_EMERGENCY_STOP: @@ -1555,7 +1552,6 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos #endif #if HAL_MOUNT_ENABLED -/* case AUX_FUNC::RETRACT_MOUNT1: { AP_Mount *mount = AP::mount(); if (mount == nullptr) { @@ -1573,7 +1569,7 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos break; } break; - } */ + } case AUX_FUNC::MOUNT_LOCK: { AP_Mount *mount = AP::mount(); @@ -1583,18 +1579,6 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos mount->set_yaw_lock(ch_flag == AuxSwitchPos::HIGH); break; } - -//OW - case AUX_FUNC::RETRACT_MOUNT1: - case AUX_FUNC::RETRACT_MOUNT1_3POS: { - AP_Mount *mount = AP::mount(); - if (mount == nullptr) { - break; - } - mount->set_mode_3pos(0, (uint8_t)ch_flag); - break; - } -//OWEND #endif case AUX_FUNC::LOG_PAUSE: { diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 3b4e5194d7357..6e5a95eb8b7af 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -250,9 +250,8 @@ class RC_Channel { CAMERA_LENS = 175, // camera lens selection VFWD_THR_OVERRIDE = 176, // force enabled VTOL forward throttle method //OW - RETRACT_MOUNT1_3POS = 177, // Retract Mount1 3pos - CAMERA_SET_MODE = 178, // set camera mode, high = video mode, low = picture mode - CAMERA_TRIG_MODE = 179, // set camera mode and trigger/start/stop, high = start video, mid = stop video, low = take picture + CAMERA_SET_MODE = 177, // set camera mode, high = video mode, low = picture mode + CAMERA_TRIG_MODE = 178, // set camera mode and trigger/start/stop, high = start video, mid = stop video, low = take picture //OWEND // inputs from 200 will eventually used to replace RCMAP From f18ed5d23b46a1c4be140bc6a210a7bca868fda6 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 27 Dec 2023 11:58:43 +0100 Subject: [PATCH 093/125] tidy --- libraries/AP_Mount/AP_Mount.cpp | 4 ++++ libraries/AP_Mount/AP_Mount.h | 1 - libraries/AP_Mount/AP_Mount_config.h | 1 - libraries/AP_RCProtocol/AP_RCProtocol_Backend.h | 2 +- libraries/bp_version.h | 4 ++-- 5 files changed, 7 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 331db19a629c8..e436a76981773 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -328,6 +328,7 @@ MAV_RESULT AP_Mount::handle_command_do_mount_control(const mavlink_command_int_t MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet) { AP_Mount_Backend *backend; + // check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is // 2nd gimbal, etc const uint8_t instance = packet.z; @@ -378,6 +379,7 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { AP_Mount_Backend *backend; + // check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is 2nd gimbal, etc const uint8_t instance = packet.z; if (instance == 0) { @@ -399,6 +401,7 @@ void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg) mavlink_msg_gimbal_manager_set_attitude_decode(&msg,&packet); AP_Mount_Backend *backend; + // check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is // 2nd gimbal, etc const uint8_t instance = packet.gimbal_device_id; @@ -464,6 +467,7 @@ void AP_Mount::handle_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg) mavlink_msg_gimbal_manager_set_pitchyaw_decode(&msg,&packet); AP_Mount_Backend *backend; + // check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is // 2nd gimbal, etc const uint8_t instance = packet.gimbal_device_id; diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 38244d2458eb7..3b7d142cbf466 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -340,7 +340,6 @@ class AP_Mount MAV_RESULT handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg); void handle_gimbal_manager_set_attitude(const mavlink_message_t &msg); void handle_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg); - void handle_global_position_int(const mavlink_message_t &msg); void handle_gimbal_device_information(const mavlink_message_t &msg); void handle_gimbal_device_attitude_status(const mavlink_message_t &msg); diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index 5b3c536f8cec4..52b940e565712 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/libraries/AP_Mount/AP_Mount_config.h @@ -59,4 +59,3 @@ #ifndef AP_MOUNT_POI_TO_LATLONALT_ENABLED #define AP_MOUNT_POI_TO_LATLONALT_ENABLED HAL_MOUNT_ENABLED && AP_TERRAIN_AVAILABLE && BOARD_FLASH_SIZE > 1024 #endif - diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index 29cd25e75d2c9..d57f6d3e0732f 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -52,7 +52,7 @@ class AP_RCProtocol_Backend { // update from mavlink messages virtual void update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) {} virtual void update_radio_link_stats(const mavlink_radio_link_stats_t* packet) {} -//END +//OWEND // get number of frames, ignoring failsafe uint32_t get_rc_frame_count(void) const { diff --git a/libraries/bp_version.h b/libraries/bp_version.h index f5406f98da8a5..857a684776140 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,7 +1,7 @@ #pragma once -#define BETAPILOTVERSION "v059d" -#define DATEOFBASEBRANCH "20231213" +#define BETAPILOTVERSION "v059e" +#define DATEOFBASEBRANCH "20231227" /* search for //OW to find all changes From 56479b9e3678b97349014af2b90f84dcf970cf9b Mon Sep 17 00:00:00 2001 From: olliw42 Date: Thu, 28 Dec 2023 11:33:30 +0100 Subject: [PATCH 094/125] radiolink, remove routing --- libraries/GCS_MAVLink/MAVLink_routing.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/libraries/GCS_MAVLink/MAVLink_routing.cpp b/libraries/GCS_MAVLink/MAVLink_routing.cpp index 29fa2f41f0043..39d7975062589 100644 --- a/libraries/GCS_MAVLink/MAVLink_routing.cpp +++ b/libraries/GCS_MAVLink/MAVLink_routing.cpp @@ -140,16 +140,6 @@ bool MAVLink_routing::check_and_forward(GCS_MAVLINK &in_link, const mavlink_mess int16_t target_component = -1; get_targets(msg, target_system, target_component); -//OW RADIOLINK - // handle messages from a mavlink receiver as if it had target_system = our system, target_component = 0 - // we currently narrow it down to "our" messages to play it safe - if ((msg.compid == MAV_COMP_ID_TELEMETRY_RADIO) && - (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS || msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_STATS)) { - target_system = mavlink_system.sysid; - target_component = 0; - } -//OWEND - bool broadcast_system = (target_system == 0 || target_system == -1); bool broadcast_component = (target_component == 0 || target_component == -1); bool match_system = broadcast_system || (target_system == mavlink_system.sysid); From 0d7daeb4fcce4dc1c7d2632f4914864ba610e806 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Thu, 28 Dec 2023 11:46:16 +0100 Subject: [PATCH 095/125] radiolink, remove escape from sysid enforce --- libraries/GCS_MAVLink/GCS_Common.cpp | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index d880e2616b6a3..d49cc55379a72 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -6462,17 +6462,6 @@ bool GCS_MAVLINK::accept_packet(const mavlink_status_t &status, return true; } -//OW RADIOLINK -#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED - // handle messages from a mavlink receiver - // we currently narrow it down to "ours" to play it safe - if ((msg.compid == MAV_COMP_ID_TELEMETRY_RADIO) && - (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS || msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_STATS)) { - return true; - } -#endif -//OWEND - if (!sysid_enforce()) { return true; } From a0331b4088938335655f2a5611ebe7533cdad986 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Thu, 28 Dec 2023 16:40:07 +0100 Subject: [PATCH 096/125] tidy --- libraries/AP_Camera/AP_Camera.cpp | 2 +- libraries/AP_Camera/AP_Camera.h | 2 +- libraries/AP_Camera/AP_Camera_Backend.h | 2 +- libraries/AP_Camera/AP_Camera_Mount.cpp | 2 +- libraries/AP_Camera/AP_Camera_Mount.h | 2 +- libraries/AP_Camera/AP_Camera_shareddefs.h | 2 +- libraries/AP_Mount/AP_Mount.cpp | 5 +++-- libraries/AP_Mount/AP_Mount.h | 5 +++-- libraries/AP_Mount/AP_Mount_Backend.h | 5 +++-- libraries/RC_Channel/RC_Channel.cpp | 4 ++-- libraries/RC_Channel/RC_Channel.h | 2 +- libraries/bp_version.h | 4 ++-- 12 files changed, 20 insertions(+), 17 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index b801d25b87ed0..364974154307e 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -803,7 +803,7 @@ AP_Camera *camera() } -//OW +//OW CAMERA bool AP_Camera::cam_set_mode(bool video_mode) { WITH_SEMAPHORE(_rsem); diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index b9adc313b4010..fc04f4ca0a875 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -119,7 +119,7 @@ class AP_Camera { void cam_mode_toggle(); void cam_mode_toggle(uint8_t instance); -//OW +//OW CAMERA // momentary switch to set to photo or video mode (video_mode false: photo mode, true: video mode) bool cam_set_mode(bool video_mode); bool cam_set_mode(uint8_t instance, bool video_mode); diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/libraries/AP_Camera/AP_Camera_Backend.h index 766a8cf448ac5..0010e2be78cfa 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/libraries/AP_Camera/AP_Camera_Backend.h @@ -53,7 +53,7 @@ class AP_Camera_Backend // momentary switch to change camera between picture and video modes virtual void cam_mode_toggle() {} -//OW +//OW CAMERA // momentary switch to set to photo or video mode (video_mode false: photo mode, true: video mode) virtual bool cam_set_mode(bool video_mode) { return false; } diff --git a/libraries/AP_Camera/AP_Camera_Mount.cpp b/libraries/AP_Camera/AP_Camera_Mount.cpp index b2e24c22251f2..b8d69858e2827 100644 --- a/libraries/AP_Camera/AP_Camera_Mount.cpp +++ b/libraries/AP_Camera/AP_Camera_Mount.cpp @@ -5,7 +5,7 @@ extern const AP_HAL::HAL& hal; -//OW +//OW CAMERA // momentary switch to set to photo or video mode (video_mode false: photo mode, true: video mode) bool AP_Camera_Mount::cam_set_mode(bool video_mode) { diff --git a/libraries/AP_Camera/AP_Camera_Mount.h b/libraries/AP_Camera/AP_Camera_Mount.h index 90d7364544050..6e5714de30224 100644 --- a/libraries/AP_Camera/AP_Camera_Mount.h +++ b/libraries/AP_Camera/AP_Camera_Mount.h @@ -32,7 +32,7 @@ class AP_Camera_Mount : public AP_Camera_Backend /* Do not allow copies */ CLASS_NO_COPY(AP_Camera_Mount); -//OW +//OW CAMERA // momentary switch to set to photo or video mode (video_mode false: photo mode, true: video mode) bool cam_set_mode(bool video_mode) override; diff --git a/libraries/AP_Camera/AP_Camera_shareddefs.h b/libraries/AP_Camera/AP_Camera_shareddefs.h index 0f1a7430e6371..7465a8e5fdd5d 100644 --- a/libraries/AP_Camera/AP_Camera_shareddefs.h +++ b/libraries/AP_Camera/AP_Camera_shareddefs.h @@ -36,7 +36,7 @@ enum class TrackingType : uint8_t { TRK_RECTANGLE = 2 // tracking a rectangle }; -//OW +//OW CAMERA // options for set_cam_photo_video_mode() enum class PhotoVideoMode : uint8_t { PHOTO_TAKE_PIC = 0, // set to photo mode and take picture diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index e436a76981773..430ea40566ecd 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -1198,7 +1198,7 @@ AP_Mount *mount() }; -//OW +//OW CAMERA bool AP_Mount::cam_set_mode(uint8_t instance, bool video_mode) { auto *backend = get_instance(instance); @@ -1216,7 +1216,8 @@ bool AP_Mount::cam_do_photo_video_mode(uint8_t instance, PhotoVideoMode photo_vi } return backend->cam_do_photo_video_mode(photo_video_mode); } - +//OWEND +//OW void AP_Mount::handle_message_extra(mavlink_channel_t chan, const mavlink_message_t &msg) { for (uint8_t instance=0; instance Date: Thu, 28 Dec 2023 18:18:11 +0100 Subject: [PATCH 097/125] rebase --- libraries/bp_version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 3b48d2ddd00db..195d9bcc8efc3 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,7 +1,7 @@ #pragma once #define BETAPILOTVERSION "v059e" -#define DATEOFBASEBRANCH "20231227" +#define DATEOFBASEBRANCH "20231228" /* search for //OW to find all changes From 105ba461556ec51426034a6aa8aa60f3a1d8569d Mon Sep 17 00:00:00 2001 From: olliw42 Date: Thu, 28 Dec 2023 23:53:08 +0100 Subject: [PATCH 098/125] streamline --- libraries/AP_Mount/AP_Mount.cpp | 8 +- libraries/AP_Mount/AP_Mount.h | 2 +- libraries/AP_Mount/AP_Mount_Backend.cpp | 2 + ...VLink.cpp => AP_Mount_STorM32_MAVLink.cpp} | 92 +++++++++---------- ...2_MAVLink.h => AP_Mount_STorM32_MAVLink.h} | 6 +- libraries/AP_Mount/AP_Mount_config.h | 4 +- 6 files changed, 58 insertions(+), 56 deletions(-) rename libraries/AP_Mount/{BP_Mount_STorM32_MAVLink.cpp => AP_Mount_STorM32_MAVLink.cpp} (95%) rename libraries/AP_Mount/{BP_Mount_STorM32_MAVLink.h => AP_Mount_STorM32_MAVLink.h} (98%) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 430ea40566ecd..f0aeb03e908b6 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -20,7 +20,7 @@ #include //OW #include -#include "BP_Mount_STorM32_MAVLink.h" +#include "AP_Mount_STorM32_MAVLink.h" //OWEND const AP_Param::GroupInfo AP_Mount::var_info[] = { @@ -155,13 +155,13 @@ void AP_Mount::init() #endif // HAL_MOUNT_VIEWPRO_ENABLED //OW -#if HAL_MOUNT_BP_STORM32_MAVLINK_ENABLED +#if HAL_MOUNT_STORM32_MAVLINK_V2_ENABLED // check for STorM32_MAVLink mounts using MAVLink protocol case Type::STorM32_MAVLink: - _backends[instance] = new BP_Mount_STorM32_MAVLink(*this, _params[instance], instance); + _backends[instance] = new AP_Mount_STorM32_MAVLink(*this, _params[instance], instance); _num_instances++; break; -#endif // HAL_MOUNT_STORM32_MAVLINK_ENABLED +#endif // HAL_MOUNT_STORM32_MAVLINK_V2_ENABLED //OWEND } diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index ebe40947f9eeb..82b3879a2cf58 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -122,7 +122,7 @@ class AP_Mount Viewpro = 11, /// Viewpro gimbal using a custom serial protocol #endif //OW -#if HAL_MOUNT_BP_STORM32_MAVLINK_ENABLED +#if HAL_MOUNT_STORM32_MAVLINK_V2_ENABLED STorM32_MAVLink = 83 #endif //OWEND diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 5c9edb8d800f4..c511999bbc79d 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -276,10 +276,12 @@ bool AP_Mount_Backend::handle_gimbal_manager_flags(uint32_t flags) // check flags for change to RETRACT if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) { set_mode(MAV_MOUNT_MODE_RETRACT); + return false; } else // check flags for change to NEUTRAL if ((flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) { set_mode(MAV_MOUNT_MODE_NEUTRAL); + return false; } return true; } diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp similarity index 95% rename from libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp rename to libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp index 2fc1a96059a2a..cf1c9ac9d5141 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp @@ -4,9 +4,9 @@ // STorM32 mount backend class //***************************************************** -#include "BP_Mount_STorM32_MAVLink.h" +#include "AP_Mount_STorM32_MAVLink.h" -#if HAL_MOUNT_BP_STORM32_MAVLINK_ENABLED +#if HAL_MOUNT_STORM32_MAVLINK_V2_ENABLED #include #include @@ -72,7 +72,7 @@ void GimbalQuaternion::to_gimbal_euler(float &roll, float &pitch, float &yaw) co //****************************************************** -// BP_Mount_STorM32_MAVLink, main class +// AP_Mount_STorM32_MAVLink, main class //****************************************************** // for reasons I really don't understand, calling them as log methods didn't work @@ -127,10 +127,10 @@ void GimbalQuaternion::to_gimbal_euler(float &roll, float &pitch, float &yaw) co //****************************************************** -// BP_Mount_STorM32_MAVLink, main class +// AP_Mount_STorM32_MAVLink, main class //****************************************************** -void BP_Mount_STorM32_MAVLink::init() +void AP_Mount_STorM32_MAVLink::init() { AP_Mount_Backend::init(); @@ -148,7 +148,7 @@ void BP_Mount_STorM32_MAVLink::init() // several vehicles do not support fast_update(), so let's go with this // priority of update() not very high, so no idea how reliable that is, may be not so good // STorM32-Link wants 25 Hz, so we update at 25 Hz and 12.5 Hz respectively -void BP_Mount_STorM32_MAVLink::update() +void AP_Mount_STorM32_MAVLink::update() { update_target_angles(); // update at 50 Hz (RC_TARGETING handling assumes this) @@ -214,7 +214,7 @@ void BP_Mount_STorM32_MAVLink::update() // Return false to skip this angle/rate processing. // AP's gimbal manager should never ask to do things which the gimbal device // can't do. Isn't so but we know well our gimbal device. -bool BP_Mount_STorM32_MAVLink::handle_gimbal_manager_flags(uint32_t flags) +bool AP_Mount_STorM32_MAVLink::handle_gimbal_manager_flags(uint32_t flags) { // check flags for change to RETRACT if (flags & GIMBAL_MANAGER_FLAGS_RETRACT) { @@ -257,7 +257,7 @@ bool BP_Mount_STorM32_MAVLink::handle_gimbal_manager_flags(uint32_t flags) } -void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags() +void AP_Mount_STorM32_MAVLink::update_gimbal_device_flags() { _flags_for_gimbal_device = 0; @@ -302,7 +302,7 @@ void BP_Mount_STorM32_MAVLink::update_gimbal_device_flags() } -void BP_Mount_STorM32_MAVLink::send_target_angles() +void AP_Mount_STorM32_MAVLink::send_target_angles() { // just send stupidly at 12.5 Hz, no check if get_target_angles() made a change @@ -326,7 +326,7 @@ void BP_Mount_STorM32_MAVLink::send_target_angles() // update_angle_target_from_rate() assumes a 50hz update rate! // TODO: one should allow angles outside of +-PI, to go shortest path in case of turn around -void BP_Mount_STorM32_MAVLink::update_target_angles() +void AP_Mount_STorM32_MAVLink::update_target_angles() { // update based on mount mode switch ((uint8_t)get_mode()) { @@ -448,7 +448,7 @@ void BP_Mount_STorM32_MAVLink::update_target_angles() // Gimbal and protocol discovery //------------------------------------------------------ -void BP_Mount_STorM32_MAVLink::find_gimbal() +void AP_Mount_STorM32_MAVLink::find_gimbal() { // search for gimbal only until vehicle is armed if (hal.util->get_soft_armed()) { @@ -490,7 +490,7 @@ void BP_Mount_STorM32_MAVLink::find_gimbal() } -void BP_Mount_STorM32_MAVLink::determine_protocol(const mavlink_message_t &msg) +void AP_Mount_STorM32_MAVLink::determine_protocol(const mavlink_message_t &msg) { if (msg.sysid != _sysid || msg.compid != _compid) { // this msg is not from our gimbal return; @@ -511,7 +511,7 @@ void BP_Mount_STorM32_MAVLink::determine_protocol(const mavlink_message_t &msg) // Gimbal control flags //------------------------------------------------------ -bool BP_Mount_STorM32_MAVLink::has_roll_control() const +bool AP_Mount_STorM32_MAVLink::has_roll_control() const { if (_protocol == Protocol::GIMBAL_DEVICE) { return (_device_info.cap_flags & GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS); @@ -523,7 +523,7 @@ bool BP_Mount_STorM32_MAVLink::has_roll_control() const } -bool BP_Mount_STorM32_MAVLink::has_pitch_control() const +bool AP_Mount_STorM32_MAVLink::has_pitch_control() const { if (_protocol == Protocol::GIMBAL_DEVICE) { return (_device_info.cap_flags & GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS); @@ -535,7 +535,7 @@ bool BP_Mount_STorM32_MAVLink::has_pitch_control() const } -bool BP_Mount_STorM32_MAVLink::has_pan_control() const +bool AP_Mount_STorM32_MAVLink::has_pan_control() const { if (_protocol == Protocol::GIMBAL_DEVICE) { return (_device_info.cap_flags & GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS); @@ -551,7 +551,7 @@ bool BP_Mount_STorM32_MAVLink::has_pan_control() const // Gimbal attitude and rate //------------------------------------------------------ -bool BP_Mount_STorM32_MAVLink::get_attitude_quaternion(Quaternion &att_quat) +bool AP_Mount_STorM32_MAVLink::get_attitude_quaternion(Quaternion &att_quat) { if (!_initialised) { return false; @@ -564,7 +564,7 @@ bool BP_Mount_STorM32_MAVLink::get_attitude_quaternion(Quaternion &att_quat) } -bool BP_Mount_STorM32_MAVLink::get_angular_velocity(Vector3f& rates) +bool AP_Mount_STorM32_MAVLink::get_angular_velocity(Vector3f& rates) { if (!_initialised) { return false; @@ -584,7 +584,7 @@ bool BP_Mount_STorM32_MAVLink::get_angular_velocity(Vector3f& rates) // MAVLink handle functions //------------------------------------------------------ -void BP_Mount_STorM32_MAVLink::handle_gimbal_device_information(const mavlink_message_t &msg) +void AP_Mount_STorM32_MAVLink::handle_gimbal_device_information(const mavlink_message_t &msg) { // this msg is not from our gimbal if (msg.sysid != _sysid || msg.compid != _compid) { @@ -621,7 +621,7 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_information(const mavlink_me } -void BP_Mount_STorM32_MAVLink::handle_gimbal_device_attitude_status(const mavlink_message_t &msg) +void AP_Mount_STorM32_MAVLink::handle_gimbal_device_attitude_status(const mavlink_message_t &msg) { if (_protocol != Protocol::GIMBAL_DEVICE) { return; @@ -670,7 +670,7 @@ void BP_Mount_STorM32_MAVLink::handle_gimbal_device_attitude_status(const mavlin } -void BP_Mount_STorM32_MAVLink::handle_message_extra(const mavlink_message_t &msg) +void AP_Mount_STorM32_MAVLink::handle_message_extra(const mavlink_message_t &msg) { if (_protocol == Protocol::UNDEFINED) { // implies !_initialised && _compid determine_protocol(msg); @@ -720,7 +720,7 @@ void BP_Mount_STorM32_MAVLink::handle_message_extra(const mavlink_message_t &msg // MAVLink send functions //------------------------------------------------------ -void BP_Mount_STorM32_MAVLink::send_cmd_request_gimbal_device_information() +void AP_Mount_STorM32_MAVLink::send_cmd_request_gimbal_device_information() { if (!HAVE_PAYLOAD_SPACE(_chan, COMMAND_LONG)) { return; @@ -738,7 +738,7 @@ void BP_Mount_STorM32_MAVLink::send_cmd_request_gimbal_device_information() // called by send_target_angles() -void BP_Mount_STorM32_MAVLink::send_cmd_do_mount_control() +void AP_Mount_STorM32_MAVLink::send_cmd_do_mount_control() { if (!HAVE_PAYLOAD_SPACE(_chan, COMMAND_LONG)) { return; @@ -763,7 +763,7 @@ void BP_Mount_STorM32_MAVLink::send_cmd_do_mount_control() // called by send_target_angles() // _flags_for_gimbal were just updated, so are correct for sure -void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude() +void AP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude() { if (!HAVE_PAYLOAD_SPACE(_chan, GIMBAL_DEVICE_SET_ATTITUDE)) { return; @@ -805,7 +805,7 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude() } -void BP_Mount_STorM32_MAVLink::send_autopilot_state_for_gimbal_device() +void AP_Mount_STorM32_MAVLink::send_autopilot_state_for_gimbal_device() { if (!HAVE_PAYLOAD_SPACE(_chan, AUTOPILOT_STATE_FOR_GIMBAL_DEVICE)) { return; @@ -986,7 +986,7 @@ landed state: } -void BP_Mount_STorM32_MAVLink::send_system_time() +void AP_Mount_STorM32_MAVLink::send_system_time() { if (!HAVE_PAYLOAD_SPACE(_chan, SYSTEM_TIME)) { return; @@ -1005,7 +1005,7 @@ void BP_Mount_STorM32_MAVLink::send_system_time() } -void BP_Mount_STorM32_MAVLink::send_rc_channels() +void AP_Mount_STorM32_MAVLink::send_rc_channels() { if (!HAVE_PAYLOAD_SPACE(_chan, RC_CHANNELS)) { return; @@ -1029,14 +1029,14 @@ void BP_Mount_STorM32_MAVLink::send_rc_channels() } -void BP_Mount_STorM32_MAVLink::send_banner() +void AP_Mount_STorM32_MAVLink::send_banner() { // postpone sending by few seconds, to avoid multiple sends _request_send_banner_ms = AP_HAL::millis(); } -void BP_Mount_STorM32_MAVLink::update_send_banner() +void AP_Mount_STorM32_MAVLink::update_send_banner() { if (!_request_send_banner_ms) return; // no request @@ -1082,7 +1082,7 @@ void BP_Mount_STorM32_MAVLink::update_send_banner() //------------------------------------------------------ // send a GIMBAL_MANAGER_INFORMATION message to GCS -void BP_Mount_STorM32_MAVLink::send_gimbal_manager_information(mavlink_channel_t chan) +void AP_Mount_STorM32_MAVLink::send_gimbal_manager_information(mavlink_channel_t chan) { // space already checked by streamer @@ -1131,7 +1131,7 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_manager_information(mavlink_channel_t // return gimbal manager flags. Used by GIMBAL_MANAGER_STATUS message. -uint32_t BP_Mount_STorM32_MAVLink::get_gimbal_manager_flags() const +uint32_t AP_Mount_STorM32_MAVLink::get_gimbal_manager_flags() const { // There are currently no specific gimbal manager flags. So one simply // can carry forward the _flags received from the gimbal. @@ -1156,7 +1156,7 @@ const uint32_t FAILURE_FLAGS = GIMBAL_DEVICE_ERROR_FLAGS_COMMS_ERROR; -bool BP_Mount_STorM32_MAVLink::has_failures(char* s) +bool AP_Mount_STorM32_MAVLink::has_failures(char* s) { uint32_t failure_flags = (_protocol == Protocol::GIMBAL_DEVICE) ? _device_status.received_failure_flags : _gimbal_error_flags; @@ -1176,7 +1176,7 @@ bool BP_Mount_STorM32_MAVLink::has_failures(char* s) } -bool BP_Mount_STorM32_MAVLink::is_healthy() +bool AP_Mount_STorM32_MAVLink::is_healthy() { if (_protocol == Protocol::GIMBAL_DEVICE) { // unhealthy if attitude status is not received within the last second @@ -1208,7 +1208,7 @@ bool BP_Mount_STorM32_MAVLink::is_healthy() // is called with 1 Hz from update loop -void BP_Mount_STorM32_MAVLink::update_checks() +void AP_Mount_STorM32_MAVLink::update_checks() { char txt[255]; @@ -1306,7 +1306,7 @@ char txt[255]; // return true if healthy // this is called when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA is set, and if not armed, else not // is called with 1 Hz -bool BP_Mount_STorM32_MAVLink::healthy() const +bool AP_Mount_STorM32_MAVLink::healthy() const { _armingchecks_running = 2; // to signal that ArduPilot arming check mechanism is active @@ -1320,7 +1320,7 @@ bool BP_Mount_STorM32_MAVLink::healthy() const // return target location if available // returns true if a target location is available and fills in target_loc argument -bool BP_Mount_STorM32_MAVLink::get_location_target(Location &_target_loc) +bool AP_Mount_STorM32_MAVLink::get_location_target(Location &_target_loc) { if (mnt_target_loc_valid) { _target_loc = mnt_target_loc; @@ -1331,7 +1331,7 @@ bool BP_Mount_STorM32_MAVLink::get_location_target(Location &_target_loc) // update mount's actual angles (to be called by script communicating with mount) -void BP_Mount_STorM32_MAVLink::set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) +void AP_Mount_STorM32_MAVLink::set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) { _script_angles.roll = radians(roll_deg); _script_angles.pitch = radians(pitch_deg); @@ -1339,14 +1339,14 @@ void BP_Mount_STorM32_MAVLink::set_attitude_euler(float roll_deg, float pitch_de } -bool BP_Mount_STorM32_MAVLink::take_control() +bool AP_Mount_STorM32_MAVLink::take_control() { _script_angles.control = true; return true; //we assume only one script trying this, so KIS } -bool BP_Mount_STorM32_MAVLink::give_control() +bool AP_Mount_STorM32_MAVLink::give_control() { _script_angles.control = false; return true; //we assume only one script trying this, so KIS @@ -1357,7 +1357,7 @@ bool BP_Mount_STorM32_MAVLink::give_control() // Camera //------------------------------------------------------ -bool BP_Mount_STorM32_MAVLink::take_picture() +bool AP_Mount_STorM32_MAVLink::take_picture() { if (_camera_mode == CameraMode::UNDEFINED) { _camera_mode = CameraMode::PHOTO; @@ -1374,7 +1374,7 @@ bool BP_Mount_STorM32_MAVLink::take_picture() } -bool BP_Mount_STorM32_MAVLink::record_video(bool start_recording) +bool AP_Mount_STorM32_MAVLink::record_video(bool start_recording) { if (_camera_mode == CameraMode::UNDEFINED) { _camera_mode = CameraMode::VIDEO; @@ -1391,7 +1391,7 @@ bool BP_Mount_STorM32_MAVLink::record_video(bool start_recording) } -bool BP_Mount_STorM32_MAVLink::cam_set_mode(bool video_mode) +bool AP_Mount_STorM32_MAVLink::cam_set_mode(bool video_mode) { _camera_mode = (video_mode) ? CameraMode::VIDEO : CameraMode::PHOTO; send_cmd_do_digicam_configure(video_mode); @@ -1402,7 +1402,7 @@ bool BP_Mount_STorM32_MAVLink::cam_set_mode(bool video_mode) } -bool BP_Mount_STorM32_MAVLink::cam_do_photo_video_mode(PhotoVideoMode photo_video_mode) +bool AP_Mount_STorM32_MAVLink::cam_do_photo_video_mode(PhotoVideoMode photo_video_mode) { if (photo_video_mode == PhotoVideoMode::VIDEO_START) { @@ -1441,7 +1441,7 @@ bool BP_Mount_STorM32_MAVLink::cam_do_photo_video_mode(PhotoVideoMode photo_vide } -void BP_Mount_STorM32_MAVLink::send_cmd_do_digicam_configure(bool video_mode) +void AP_Mount_STorM32_MAVLink::send_cmd_do_digicam_configure(bool video_mode) { if (!HAVE_PAYLOAD_SPACE(_chan, COMMAND_LONG)) { return; @@ -1461,7 +1461,7 @@ void BP_Mount_STorM32_MAVLink::send_cmd_do_digicam_configure(bool video_mode) } -void BP_Mount_STorM32_MAVLink::send_cmd_do_digicam_control(bool shoot) +void AP_Mount_STorM32_MAVLink::send_cmd_do_digicam_control(bool shoot) { if (!HAVE_PAYLOAD_SPACE(_chan, COMMAND_LONG)) { return; @@ -1486,7 +1486,7 @@ void BP_Mount_STorM32_MAVLink::send_cmd_do_digicam_control(bool shoot) //------------------------------------------------------ // send a MOUNT_STATUS message to GCS, this is only to make MissionPlanner and alike happy -void BP_Mount_STorM32_MAVLink::send_gimbal_device_attitude_status(mavlink_channel_t chan) +void AP_Mount_STorM32_MAVLink::send_gimbal_device_attitude_status(mavlink_channel_t chan) { // space already checked by streamer // has checked for space of GIMBAL_DEVICE_ATTITUDE_STATUS, but MOUNT_STATUS is (much) smaller, so no issue @@ -1507,7 +1507,7 @@ void BP_Mount_STorM32_MAVLink::send_gimbal_device_attitude_status(mavlink_channe } -#endif // HAL_MOUNT_BP_STORM32_MAVLINK_ENABLED +#endif // HAL_MOUNT_STORM32_MAVLINK_V2_ENABLED diff --git a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h similarity index 98% rename from libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h rename to libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h index 072577285347b..8b5403999656e 100644 --- a/libraries/AP_Mount/BP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h @@ -7,10 +7,10 @@ #include "AP_Mount_Backend.h" -#if HAL_MOUNT_BP_STORM32_MAVLINK_ENABLED +#if HAL_MOUNT_STORM32_MAVLINK_V2_ENABLED -class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend +class AP_Mount_STorM32_MAVLink : public AP_Mount_Backend { public: @@ -347,4 +347,4 @@ class BP_Mount_STorM32_MAVLink : public AP_Mount_Backend }; -#endif // HAL_MOUNT_BP_STORM32_MAVLINK_ENABLED +#endif // HAL_MOUNT_STORM32_MAVLINK_V2_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index 52b940e565712..a079971ed04a1 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/libraries/AP_Mount/AP_Mount_config.h @@ -43,8 +43,8 @@ #endif //OW -#ifndef HAL_MOUNT_BP_STORM32_MAVLINK_ENABLED -#define HAL_MOUNT_BP_STORM32_MAVLINK_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED +#ifndef HAL_MOUNT_STORM32_MAVLINK_V2_ENABLED +#define HAL_MOUNT_STORM32_MAVLINK_V2_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED #endif //OWEND From dcbacd896a80a6e2c287e76bea550cb5600abf89 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Fri, 29 Dec 2023 12:47:48 +0100 Subject: [PATCH 099/125] more streamlining --- .../AP_Mount/AP_Mount_STorM32_MAVLink.cpp | 59 +++++++++++-------- libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h | 17 ++---- 2 files changed, 40 insertions(+), 36 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp index cf1c9ac9d5141..4eb33fd2e34fa 100644 --- a/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp @@ -192,7 +192,7 @@ void AP_Mount_STorM32_MAVLink::update() update_checks(); if (_protocol == Protocol::GIMBAL_DEVICE) { - gcs().send_message(MSG_GIMBAL_MANAGER_STATUS); + gcs().send_message(MSG_GIMBAL_MANAGER_STATUS); // send it voluntarily } } @@ -230,20 +230,20 @@ bool AP_Mount_STorM32_MAVLink::handle_gimbal_manager_flags(uint32_t flags) update_gimbal_device_flags(); // if not in mavlink targeting don't accept the angle/rate settings - // this is needed since backend's set_angle_target(), set_rate_target() set mode to mavlink targeting - // dirty, I think one should change backend's functions, but also makes sense somewhat + // this is needed since backend's set_angle_target(), set_rate_target() do set mode to mavlink targeting + // I think one should change backend's functions, but also somewhat makes sense if (get_mode() != MAV_MOUNT_MODE_MAVLINK_TARGETING) { - return false; + return false; // don't accept angle/rate setting } // driver currently does not support yaw LOCK // front-end is digesting GIMBAL_MANAGER_FLAGS_YAW_LOCK to determine yaw_is_earth_frame - // we could make it to modify the flag, but for moment let's be happy. + // we could make it to modify the flag, but for moment let's be happy if (flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK) { return false; // don't accept angle/rate setting } - // STorM32 expects that only one of them is set, otherwise it rejects + // STorM32 expects the "new" format, i.e. that only one of them is set, otherwise it rejects if (!(flags & GIMBAL_MANAGER_FLAGS_YAW_IN_VEHICLE_FRAME) && !(flags & GIMBAL_MANAGER_FLAGS_YAW_IN_EARTH_FRAME)) { return false; // don't accept angle/rate setting } @@ -293,7 +293,7 @@ void AP_Mount_STorM32_MAVLink::update_gimbal_device_flags() _flags_for_gimbal_device |= GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK; // driver currently does not support yaw lock, only yaw follow -// if (_is_yaw_lock) _flags_for_gimbal |= GIMBAL_DEVICE_FLAGS_YAW_LOCK; + // -> _flags_for_gimbal_device &=~ GIMBAL_DEVICE_FLAGS_YAW_LOCK; // frame flags @@ -304,12 +304,12 @@ void AP_Mount_STorM32_MAVLink::update_gimbal_device_flags() void AP_Mount_STorM32_MAVLink::send_target_angles() { - // just send stupidly at 12.5 Hz, no check if get_target_angles() made a change + // just send stubbornly at 12.5 Hz, no check if get_target_angles() made a change update_gimbal_device_flags(); if (mnt_target.target_type == MountTargetType::RATE) { - // we ignore it. We may think to just send angle_rad, but if yaw is earth frame + // we ignore it. One may think to just send angle_rad, but if yaw is earth frame // this could result in pretty strange behavior. So better ignore. // Should happen only in MAV_MOUNT_MODE_RC_TARGETING, so no need to test for this // explicitly. @@ -324,7 +324,7 @@ void AP_Mount_STorM32_MAVLink::send_target_angles() } -// update_angle_target_from_rate() assumes a 50hz update rate! +// update_angle_target_from_rate() assumes a 50 Hz update rate! // TODO: one should allow angles outside of +-PI, to go shortest path in case of turn around void AP_Mount_STorM32_MAVLink::update_target_angles() { @@ -472,7 +472,7 @@ void AP_Mount_STorM32_MAVLink::find_gimbal() } // request GIMBAL_DEVICE_INFORMATION - // STorM32 gives this also for mount mode + // STorM32 provides this also for mount mode if (!_got_device_info) { if (tnow_ms - _request_device_info_tlast_ms > 1000) { _request_device_info_tlast_ms = tnow_ms; @@ -746,7 +746,7 @@ void AP_Mount_STorM32_MAVLink::send_cmd_do_mount_control() // send command_long command containing a do_mount_control command // Note: pitch and yaw are reversed - // ATTENTION: uses get_bf_yaw() to ensure body frame, which uses ahrs.yaw, not delta_yaw!!! + // ATTENTION: uses get_bf_yaw() to ensure body frame, which uses ahrs.yaw, not delta_yaw! mavlink_msg_command_long_send( _chan, _sysid, _compid, @@ -762,7 +762,7 @@ void AP_Mount_STorM32_MAVLink::send_cmd_do_mount_control() // called by send_target_angles() -// _flags_for_gimbal were just updated, so are correct for sure +// _flags_for_gimbal_device were just updated, so are correct for sure void AP_Mount_STorM32_MAVLink::send_gimbal_device_set_attitude() { if (!HAVE_PAYLOAD_SPACE(_chan, GIMBAL_DEVICE_SET_ATTITUDE)) { @@ -918,6 +918,7 @@ estimator status: _tahrs_healthy_ms = AP_HAL::millis(); } + // delay by 3 sec to get past "quaternion flip" if (ahrs_healthy && (nav_estimator_status & ESTIMATOR_ATTITUDE) && ((AP_HAL::millis() - _tahrs_healthy_ms) > 3000)) { estimator_status |= ESTIMATOR_ATTITUDE; // -> QFix if (ahrs.initialised() && (nav_estimator_status & ESTIMATOR_VELOCITY_VERT) && (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D)) { @@ -1089,12 +1090,12 @@ void AP_Mount_STorM32_MAVLink::send_gimbal_manager_information(mavlink_channel_t // The request to send this message should be NACKed for as long as // the gimbal device was not found or its device info was not received. // Not done currently, so as workaround don't send if not yet available. - // Should make third parties to repeat requests. + // Should make third parties to repeat request. if (!_got_device_info) return; // There are few specific gimbal manager capability flags, which are not used. - // So one simply can carry forward the cap_flags received from the gimbal. + // So we simply can carry forward the cap_flags received from the gimbal. // The STorM32 gimbal has these capabilities: // - GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT | GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL // - GIMBAL_DEVICE_CAP_FLAGS_HAS_xx_AXIS | GIMBAL_DEVICE_CAP_FLAGS_HAS_xx_FOLLOW | @@ -1106,9 +1107,9 @@ void AP_Mount_STorM32_MAVLink::send_gimbal_manager_information(mavlink_channel_t uint32_t cap_flags = _device_info.cap_flags; // This driver does not support all capabilities, so we erase them. - // ATTENTION: This can mean that the gimbal device and gimbal manager capability flags + // Note: This can mean that the gimbal device and gimbal manager capability flags // may be different, and any third party which mistakenly thinks it can use those from - // the gimbal device messages may get confused ! + // the gimbal device messages may get confused. Their fault. cap_flags &=~ (GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_FOLLOW | GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_FOLLOW | @@ -1129,6 +1130,17 @@ void AP_Mount_STorM32_MAVLink::send_gimbal_manager_information(mavlink_channel_t ); } +// return gimbal device id +uint8_t AP_Mount_STorM32_MAVLink::get_gimbal_device_id() const +{ + if (_instance == 0) { + return MAV_COMP_ID_GIMBAL; + } else + if (_instance <= 5) { + return MAV_COMP_ID_GIMBAL2 + _instance - 1; + } + return MAV_COMP_ID_GIMBAL; // should not happen +} // return gimbal manager flags. Used by GIMBAL_MANAGER_STATUS message. uint32_t AP_Mount_STorM32_MAVLink::get_gimbal_manager_flags() const @@ -1136,9 +1148,9 @@ uint32_t AP_Mount_STorM32_MAVLink::get_gimbal_manager_flags() const // There are currently no specific gimbal manager flags. So one simply // can carry forward the _flags received from the gimbal. - // ATTENTION: This driver does not support all capabilities, but this + // Note: This driver does not support all capabilities, but this // should never be a problem since any third party should strictly adhere - // to the capability flags obtained from the gimbal manager ! + // to the capability flags obtained from the gimbal manager. return _device_status.received_flags; } @@ -1342,14 +1354,14 @@ void AP_Mount_STorM32_MAVLink::set_attitude_euler(float roll_deg, float pitch_de bool AP_Mount_STorM32_MAVLink::take_control() { _script_angles.control = true; - return true; //we assume only one script trying this, so KIS + return true; // we assume only one script trying this, so KIS } bool AP_Mount_STorM32_MAVLink::give_control() { _script_angles.control = false; - return true; //we assume only one script trying this, so KIS + return true; // we assume only one script trying this, so KIS } @@ -1485,11 +1497,12 @@ void AP_Mount_STorM32_MAVLink::send_cmd_do_digicam_control(bool shoot) // MAVLink mount status forwarding //------------------------------------------------------ -// send a MOUNT_STATUS message to GCS, this is only to make MissionPlanner and alike happy +// send a MOUNT_STATUS message to GCS +// make MissionPlanner and alike happy and gives parties a chance to know the mode void AP_Mount_STorM32_MAVLink::send_gimbal_device_attitude_status(mavlink_channel_t chan) { // space already checked by streamer - // has checked for space of GIMBAL_DEVICE_ATTITUDE_STATUS, but MOUNT_STATUS is (much) smaller, so no issue + // did check for space of GIMBAL_DEVICE_ATTITUDE_STATUS, but MOUNT_STATUS is (much) smaller, so no issue if (_compid != MAV_COMP_ID_GIMBAL) { // do it only for the 1st gimbal return; diff --git a/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h index 8b5403999656e..5766b80fb1f72 100644 --- a/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h @@ -99,23 +99,14 @@ class AP_Mount_STorM32_MAVLink : public AP_Mount_Backend // => we need to overwrite it void send_gimbal_manager_information(mavlink_channel_t chan) override; + // added: return gimbal device id + uint8_t get_gimbal_device_id() const override; + // added: return gimbal manager flags used by GIMBAL_MANAGER_STATUS message // AP nonsense: does it wrong, just good for its own limits. // => we need to overwrite it uint32_t get_gimbal_manager_flags() const override; - // return gimbal device id - uint8_t get_gimbal_device_id() const override - { - if (_instance == 0) { - return MAV_COMP_ID_GIMBAL; - } else - if (_instance <= 5) { - return MAV_COMP_ID_GIMBAL2 + _instance - 1; - } - return MAV_COMP_ID_GIMBAL; // should not happen - } - // added: handle gimbal manager flags received from gimbal manager messages // AP nonsense: does it wrong, just good for its limits. // => we need to overwrite it @@ -267,7 +258,7 @@ class AP_Mount_STorM32_MAVLink : public AP_Mount_Backend struct { uint16_t received_flags; // obtained from GIMBAL_DEVICE_ATTITUDE_STATUS - uint32_t received_failure_flags; // obtained from GIMBAL_DEVICE_ATTITUDE_STATUS + uint32_t received_failure_flags; // obtained from GIMBAL_DEVICE_ATTITUDE_STATUS uint32_t received_tlast_ms; // time last GIMBAL_DEVICE_ATTITUDE_STATUS was received (used for health reporting) } _device_status; From 97c304be798583f3c691ef641131098dfdf157a7 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 30 Dec 2023 08:53:05 +0100 Subject: [PATCH 100/125] fast --- .../AP_Mount/AP_Mount_STorM32_MAVLink.cpp | 49 +++++++++++++++++-- libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h | 9 ++++ 2 files changed, 54 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp index 4eb33fd2e34fa..a9c9f3d1fe824 100644 --- a/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp @@ -185,15 +185,13 @@ void AP_Mount_STorM32_MAVLink::update() return; } + update_manager_status(); + uint32_t tnow_ms = AP_HAL::millis(); if ((tnow_ms - _checks_tlast_ms) >= 1000) { // do every 1 sec _checks_tlast_ms = tnow_ms; update_checks(); - - if (_protocol == Protocol::GIMBAL_DEVICE) { - gcs().send_message(MSG_GIMBAL_MANAGER_STATUS); // send it voluntarily - } } if ((tnow_ms - _send_system_time_tlast_ms) >= 5000) { // every 5 sec is really plenty @@ -1326,6 +1324,49 @@ bool AP_Mount_STorM32_MAVLink::healthy() const } +//------------------------------------------------------ +// Gimbal manager status function +//------------------------------------------------------ + +void AP_Mount_STorM32_MAVLink::update_manager_status() +{ + // check if status has changed + if (_manager_status.flags_last != get_gimbal_manager_flags() || + _manager_status.primary_sysid_last != mavlink_control_id.sysid || + _manager_status.primary_compid_last != mavlink_control_id.compid) { + + _manager_status.flags_last = get_gimbal_manager_flags(); + _manager_status.primary_sysid_last = mavlink_control_id.sysid; + _manager_status.primary_compid_last = mavlink_control_id.compid; + + _manager_status.fast = 3; + } + + uint32_t tnow_ms = AP_HAL::millis(); + + if (!_manager_status.fast) { + if ((tnow_ms - _manager_status.tlast_ms) >= 2000) { // do every 2 sec + _manager_status.tlast_ms = tnow_ms; + gcs().send_message(MSG_GIMBAL_MANAGER_STATUS); + } + return; + } + + // we are in fast response + + if (_manager_status.fast >= 3) { // status has just changed, so react immediately + _manager_status.fast = 2; + _manager_status.tlast_ms = tnow_ms; + gcs().send_message(MSG_GIMBAL_MANAGER_STATUS); + } else + if ((tnow_ms - _manager_status.tlast_ms) >= 250) { // do every 250 ms + _manager_status.fast--; + _manager_status.tlast_ms = tnow_ms; + gcs().send_message(MSG_GIMBAL_MANAGER_STATUS); + } +} + + //------------------------------------------------------ // Scripting accessors & bindings //------------------------------------------------------ diff --git a/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h index 5766b80fb1f72..0593037fa5675 100644 --- a/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h @@ -250,6 +250,15 @@ class AP_Mount_STorM32_MAVLink : public AP_Mount_Backend uint32_t _request_send_banner_ms; void update_send_banner(); + struct { + uint8_t fast; // counter to determine faster responses + uint32_t tlast_ms; // time last GIMBAL_MANAGER_STATUS was send + uint32_t flags_last; // to detect changes + uint8_t primary_sysid_last; + uint8_t primary_compid_last; + } _manager_status; + void update_manager_status(); + // gimbal target & control struct { From 672fbe86e304671498c187dabb9320593d02b136 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 30 Dec 2023 09:36:37 +0100 Subject: [PATCH 101/125] rebase --- libraries/bp_version.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 195d9bcc8efc3..43a0fe2ebf749 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,7 +1,7 @@ #pragma once -#define BETAPILOTVERSION "v059e" -#define DATEOFBASEBRANCH "20231228" +#define BETAPILOTVERSION "v059f" +#define DATEOFBASEBRANCH "20231230" /* search for //OW to find all changes From 1b4a4cbe24a7c0fe608ad4555969afffc5f57d01 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 30 Dec 2023 10:22:21 +0100 Subject: [PATCH 102/125] prearm, tidy --- libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp | 11 +++++++++++ libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h | 4 ++-- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp index a9c9f3d1fe824..9bb4d243401af 100644 --- a/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp @@ -1222,6 +1222,17 @@ void AP_Mount_STorM32_MAVLink::update_checks() { char txt[255]; + if (!(AP::arming().get_enabled_checks() & AP_Arming::ArmingChecks::ARMING_CHECK_ALL || + AP::arming().get_enabled_checks() & AP_Arming::ArmingChecks::ARMING_CHECK_CAMERA)) { + if (_initialised && _gimbal_prearmchecks_ok) { + if (!_prearmchecks_passed && !_request_send_banner_ms) { // state changed and not going to send soon anyways + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MNT%u: prearm checks passed", _instance+1); + } + _prearmchecks_passed = true; + } + return; + } + if (_armingchecks_running) _armingchecks_running--; // count down if (_prearmchecks_passed && AP::notify().flags.armed && diff --git a/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h index 0593037fa5675..dcf1948f7ba21 100644 --- a/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h +++ b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.h @@ -235,7 +235,7 @@ class AP_Mount_STorM32_MAVLink : public AP_Mount_Backend uint32_t _gimbal_error_flags; // error flags in custom_mode field of the HEARTBEAT message (restricted to 16 bits) bool _gimbal_prearmchecks_ok; // true when the gimbal stops reporting prearm fail in the HEARTBEAT message - mutable uint8_t _armingchecks_running; // true when ARMING_CHECK_ALL or ARMING_CHECK_CAMERA set, we know from healty() + mutable uint8_t _armingchecks_running; // true when ARMING_CHECK_ALL,ARMING_CHECK_CAMERA set and running, we know from healthy() bool _healthy; bool _prearmchecks_passed; // becomes true when all checks were passed once at startup @@ -267,7 +267,7 @@ class AP_Mount_STorM32_MAVLink : public AP_Mount_Backend struct { uint16_t received_flags; // obtained from GIMBAL_DEVICE_ATTITUDE_STATUS - uint32_t received_failure_flags; // obtained from GIMBAL_DEVICE_ATTITUDE_STATUS + uint32_t received_failure_flags; // obtained from GIMBAL_DEVICE_ATTITUDE_STATUS uint32_t received_tlast_ms; // time last GIMBAL_DEVICE_ATTITUDE_STATUS was received (used for health reporting) } _device_status; From 0a26282c1452e6650c409ccef05f158f40a86a4e Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sun, 31 Dec 2023 16:20:53 +0100 Subject: [PATCH 103/125] radio link stats: handle RSSI_DBM slightly better --- .../AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp | 15 +++++++++------ libraries/AP_RSSI/AP_RSSI.h | 3 --- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp index 75f02b5076ec5..4a9f10ea77ed8 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp @@ -58,17 +58,20 @@ void AP_RCProtocol_MAVLinkRadio::update_radio_link_stats(const mavlink_radio_lin } if (packet->flags & RADIO_LINK_STATS_FLAGS_RSSI_DBM) { - // rssi is in dBm, convert to AP rssi using the same logic as in CRSF driver - // AP rssi: -1 for unknown, 0 for no link, 255 for maximum link - if (_rssi < 50) { + // rssi is in negative dBm, 255 = unknown, 254 = no link connection, 0...253 = 0...-253 dBm + // convert to AP rssi using the same logic as in CRSF driver + // AP rssi: -1 for unknown, 0 for no link connection, 255 for maximum link + if (_rssi == 254) { + rssi = 0; // no connection + } else if (_rssi < 50) { rssi = 255; } else if (_rssi > 120) { - rssi = 0; + rssi = 1; // connection, but very low rssi } else { - rssi = int16_t(roundf((1.0f - (_rssi - 50.0f) / 70.0f) * 255.0f)); + rssi = int16_t(roundf((1.0f - ((float)_rssi - 50.0f) / 70.0f) * 255.0f)); } } else { - // _rssi is 0..254, scale it to 0..255 with rounding + // _rssi is in mavlink scale 0..254, scale it to 0..255 with rounding rssi = (_rssi * 255 + 127) / 254; } } diff --git a/libraries/AP_RSSI/AP_RSSI.h b/libraries/AP_RSSI/AP_RSSI.h index fed075612f3cd..20f0db1b39ffb 100644 --- a/libraries/AP_RSSI/AP_RSSI.h +++ b/libraries/AP_RSSI/AP_RSSI.h @@ -49,9 +49,6 @@ class AP_RSSI // return true if rssi reading is enabled bool enabled() const { return RssiType(rssi_type.get()) != RssiType::TYPE_DISABLED; } -//OW RADIOLINK - bool enabled(RssiType type) const { return RssiType(rssi_type.get()) == type; } -//OWEND // Read the receiver RSSI value as a float 0.0f - 1.0f. // 0.0 represents weakest signal, 1.0 represents maximum signal. From cc5ca02587c9c06da3fd4af31f98d074783a86ef Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 3 Jan 2024 14:27:13 +0100 Subject: [PATCH 104/125] mavlink, use dev versions of radio rc, radio link messages --- ArduCopter/GCS_Mavlink.cpp | 2 +- ArduPlane/GCS_Mavlink.cpp | 2 +- bp_mavlink/olliw_dev.xml | 88 +++++++++++++------ libraries/AP_Logger/AP_Logger.h | 2 +- libraries/AP_Logger/LogFile.cpp | 26 ++++-- libraries/AP_Logger/LogStructure.h | 35 ++++++-- .../AP_Mount/AP_Mount_STorM32_MAVLink.cpp | 5 ++ libraries/AP_RCProtocol/AP_RCProtocol.cpp | 4 +- libraries/AP_RCProtocol/AP_RCProtocol.h | 4 +- .../AP_RCProtocol/AP_RCProtocol_Backend.h | 4 +- .../AP_RCProtocol_MavlinkRadio.cpp | 24 +++-- .../AP_RCProtocol_MavlinkRadio.h | 4 +- libraries/GCS_MAVLink/GCS_Common.cpp | 10 +-- 13 files changed, 147 insertions(+), 63 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 2e5c131b94dbb..b515625d214e0 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1492,7 +1492,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) break; } //OW RADIOLINK - case MAVLINK_MSG_ID_RADIO_LINK_STATS: + case MAVLINK_MSG_ID_RADIO_LINK_STATS_DEV: handle_radio_link_stats(msg, copter.should_log(MASK_LOG_PM)); break; //OWEND diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 404b4524719fe..6ebed404ab6ae 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1234,7 +1234,7 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg) break; } //OW RADIOLINK - case MAVLINK_MSG_ID_RADIO_LINK_STATS: + case MAVLINK_MSG_ID_RADIO_LINK_STATS_DEV: handle_radio_link_stats(msg, plane.should_log(MASK_LOG_PM)); break; //OWEND diff --git a/bp_mavlink/olliw_dev.xml b/bp_mavlink/olliw_dev.xml index 140fc79a76b1d..627080726aeb2 100644 --- a/bp_mavlink/olliw_dev.xml +++ b/bp_mavlink/olliw_dev.xml @@ -1,16 +1,4 @@ - @@ -18,13 +6,22 @@ Documentation: Unknown radio link type. - + + Radio link is HereLink. + + + Radio link is Dragon Link. + + + Radio link is RFD900. + + Radio link is Crossfire. - + Radio link is ExpressLRS. - + Radio link is mLRS. @@ -34,7 +31,7 @@ Documentation: Addition to message AUTOPILOT_STATE_FOR_GIMBAL_DEVICE. System ID. - Component ID. + Component ID. Timestamp (time since system boot). Wind X speed in NED (North,Est, Down). NAN if unknown. Wind Y speed in NED (North, East, Down). NAN if unknown. @@ -50,15 +47,56 @@ Documentation: Receive bandwidth consumption. Values: 0..100, UINT8_MAX: invalid/unknown. For compatibility with legacy method. UINT8_MAX: unknown. - - - - Radio link information. Should be emitted only by components with component id MAV_COMP_ID_TELEMETRY_RADIO. - Radio link type. 0: unknown type. - Operation mode. Radio link dependent. - Rate in Hz. 0: unknown. - Transmit power of transmitter in dBm. INT8_MAX: unknown. - Transmit power of receiver in dBm. INT8_MAX: unknown. + + Radio channels. Supports up to 32 channels. Channel values are in centered 13 bit format. Range is [-4096,4096], center is 0. Conversion to PWM is x * 5/32 + 1500. + The target_system field should normally be set to the system id of the system the radio receiver is connected to, the target_component field can normally be set to 0. + + System ID (can be 0 for broadcast, but this is discouraged). + Component ID (normally 0 for broadcast). + Total number of RC channels being received. This can be larger than 32, indicating that more channels are available but not given in this message. + Radio RC channels status flags. + + RC channels. Channels above count should be set to 0, to benefit from MAVLink's zero padding. + + + Radio link statistics. Tx: ground-side device, Rx: vehicle-side device. + Per default, rssi values are in MAVLink units: 0 represents weakest signal, 254 represents maximum signal; can be changed to inverted dBm with the flag + RADIO_LINK_STATS_FLAGS_RSSI_DBM: 0..253 correspond to 0..-253 dBm, 254 represents no link connection. + The target_system field should normally be set to the system id of the system the radio receiver is connected to, the target_component field can normally be set to 0. + + System ID (can be 0 for broadcast, but this is discouraged). + Component ID (normally 0 for broadcast). + Radio link statistics flags. + Link quality of RC data stream from Tx to Rx. Values: 1..100, 0: no link connection, UINT8_MAX: unknown. + Link quality of serial MAVLink data stream from Tx to Rx. Values: 1..100, 0: no link connection, UINT8_MAX: unknown. + Rssi of antenna1. 254: no link connection, UINT8_MAX: unknown. + Noise on antenna1. Radio link dependent. INT8_MAX: unknown. + Rssi of antenna2. 254: no link connection, UINT8_MAX: ignore/unknown, use rx_rssi1. + Noise on antenna2. Radio link dependent. INT8_MAX: ignore/unknown, use rx_snr1. + 0: antenna1, 1: antenna2, UINT8_MAX: no Rx receive diversity, use rx_rssi1, rx_snr1. + 0: antenna1, 1: antenna2, UINT8_MAX: no Rx transmit diversity. + Rx transmit power in dBm. INT8_MAX: unknown. + Link quality of serial MAVLink data stream from Rx to Tx. Values: 1..100, 0: no link connection, UINT8_MAX: unknown. + Rssi of antenna1. 254: no link connection. UINT8_MAX: unknown. + Noise on antenna1. Radio link dependent. INT8_MAX: unknown. + Rssi of antenna2. 254: no link connection. UINT8_MAX: ignore/unknown, use tx_rssi1. + Noise on antenna2. Radio link dependent. INT8_MAX: ignore/unknown, use tx_snr1. + 0: antenna1, 1: antenna2, UINT8_MAX: no Tx receive diversity, use tx_rssi1, tx_snr1. + 0: antenna1, 1: antenna2, UINT8_MAX: no Tx transmit diversity. + Tx transmit power in dBm. INT8_MAX: unknown. + + + Radio link information. Tx: ground-side device, Rx: vehicle-side device. Can normally be send at a low rate, like 0.2 Hz. + The target_system field should normally be set to the system id of the system the radio receiver is connected to, the target_component field can normally be set to 0. + + System ID (can be 0 for broadcast, but this is discouraged). + Component ID (normally 0 for broadcast). + Radio link type. 0: unknown/generic type. + Operation mode. Radio link dependent. UINT8_MAX: unknown. + Packet rate in Hz for Tx to Rx transmission. 0: unknown. + Packet rate in Hz for Rx to Tx transmission. Normally equal to tx_rate. 0: unknown. + Receive sensitivity of Tx in inverted dBm. 1..255 represents -1..-255 dBm, 0: unknown. + Receive sensitivity of Rx in inverted dBm. 1..255 represents -1..-255 dBm, 0: unknown. diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 5c12a3c5f93dd..a3b86d7a67667 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -252,7 +252,7 @@ class AP_Logger void Write_Power(void); void Write_Radio(const mavlink_radio_t &packet); //OW RADIOLINK - void Write_RadioLinkStats(const mavlink_radio_link_stats_t &packet); + void Write_RadioLinkStats(const mavlink_radio_link_stats_dev_t &packet); //OWEND void Write_Message(const char *message); void Write_MessageF(const char *fmt, ...); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 211dd083b5b9a..85a2f15b14c26 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -398,35 +398,43 @@ void AP_Logger::Write_Radio(const mavlink_radio_t &packet) } //OW RADIOLINK -void AP_Logger::Write_RadioLinkStats(const mavlink_radio_link_stats_t &packet) +void AP_Logger::Write_RadioLinkStats(const mavlink_radio_link_stats_dev_t &packet) { - const struct log_RadioLinkStats pktrx{ + const struct log_RadioLinkStatsRx pktrx{ LOG_PACKET_HEADER_INIT(LOG_RADIO_LINK_STATS_MSG_RX), - time_us : AP_HAL::micros64(), - LQ : packet.rx_LQ, + time_us : AP_HAL::micros64(), + LQrc : packet.rx_LQ_rc, + LQser : packet.rx_LQ_ser, rssi1 : packet.rx_rssi1, snr1 : packet.rx_snr1, rssi2 : packet.rx_rssi2, snr2 : packet.rx_snr2, receive_antenna : packet.rx_receive_antenna, transmit_antenna : packet.rx_transmit_antenna, - flags : packet.flags }; WriteBlock(&pktrx, sizeof(pktrx)); - const struct log_RadioLinkStats pkttx{ + const struct log_RadioLinkStatsTx pkttx{ LOG_PACKET_HEADER_INIT(LOG_RADIO_LINK_STATS_MSG_TX), - time_us : AP_HAL::micros64(), - LQ : packet.tx_LQ, + time_us : AP_HAL::micros64(), + LQser : packet.tx_LQ_ser, rssi1 : packet.tx_rssi1, snr1 : packet.tx_snr1, rssi2 : packet.tx_rssi2, snr2 : packet.tx_snr2, receive_antenna : packet.tx_receive_antenna, transmit_antenna : packet.tx_transmit_antenna, - flags : packet.flags }; WriteBlock(&pkttx, sizeof(pkttx)); + + const struct log_RadioLinkStatsExt pktext{ + LOG_PACKET_HEADER_INIT(LOG_RADIO_LINK_STATS_MSG_EXT), + time_us : AP_HAL::micros64(), + tx_power : packet.tx_power, + rx_power : packet.rx_power, + flags : packet.flags + }; + WriteBlock(&pktext, sizeof(pktext)); } //OWEND diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 08f527e8fecef..e07a1a207e485 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -396,16 +396,36 @@ struct PACKED log_Radio { }; //OW RADIOLINK -struct PACKED log_RadioLinkStats { +struct PACKED log_RadioLinkStatsRx { LOG_PACKET_HEADER; uint64_t time_us; - uint8_t LQ; + uint8_t LQrc; + uint8_t LQser; uint8_t rssi1; int8_t snr1; uint8_t rssi2; int8_t snr2; uint8_t receive_antenna; uint8_t transmit_antenna; +}; + +struct PACKED log_RadioLinkStatsTx { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t LQser; + uint8_t rssi1; + int8_t snr1; + uint8_t rssi2; + int8_t snr2; + uint8_t receive_antenna; + uint8_t transmit_antenna; +}; + +struct PACKED log_RadioLinkStatsExt { + LOG_PACKET_HEADER; + uint64_t time_us; + int8_t tx_power; + int8_t rx_power; uint8_t flags; }; //OWEND @@ -1252,10 +1272,12 @@ LOG_STRUCTURE_FROM_PRECLAND \ "MAVC", "QBBBBBHffffiifBB","TimeUS,TS,TC,SS,SC,Fr,Cmd,P1,P2,P3,P4,X,Y,Z,Res,WL", "s---------------", "F---------------" }, \ { LOG_RADIO_MSG, sizeof(log_Radio), \ "RAD", "QBBBBBHH", "TimeUS,RSSI,RemRSSI,TxBuf,Noise,RemNoise,RxErrors,Fixed", "s-------", "F-------", true }, \ - { LOG_RADIO_LINK_STATS_MSG_RX, sizeof(log_RadioLinkStats), \ - "RDRX", "QBBbBbBBB", "TimeUS,rxLQ,rxRssi1,rxSnr1,rxRssi2,rxSnr2,rxRAnt,rxTAnt,flags", "s--------", "F--------", true }, \ - { LOG_RADIO_LINK_STATS_MSG_TX, sizeof(log_RadioLinkStats), \ - "RDTX", "QBBbBbBBB", "TimeUS,txLQ,txRssi1,txSnr1,txRssi2,txSnr2,txRAnt,txTAnt,flags", "s--------", "F--------", true }, \ + { LOG_RADIO_LINK_STATS_MSG_RX, sizeof(log_RadioLinkStatsRx), \ + "RDRX", "QBBBbBbBB", "TimeUS,rxLQrc,rxLQser,rxRssi1,rxSnr1,rxRssi2,rxSnr2,rxRAnt,rxTAnt", "s%%------", "F--------", true }, \ + { LOG_RADIO_LINK_STATS_MSG_TX, sizeof(log_RadioLinkStatsTx), \ + "RDTX", "QBBbBbBB", "TimeUS,txLQser,txRssi1,txSnr1,txRssi2,txSnr2,txRAnt,txTAnt", "s%------", "F-------", true }, \ + { LOG_RADIO_LINK_STATS_MSG_EXT, sizeof(log_RadioLinkStatsExt), \ + "RDTR", "QbbB", "TimeUS,txPwr,rxPwr,flags", "s---", "F---", true }, \ LOG_STRUCTURE_FROM_CAMERA \ LOG_STRUCTURE_FROM_MOUNT \ { LOG_ARSP_MSG, sizeof(log_ARSP), "ARSP", "QBffcffBBffB", "TimeUS,I,Airspeed,DiffPress,Temp,RawPress,Offset,U,H,Hp,TR,Pri", "s#nPOPP-----", "F-00B00-----", true }, \ @@ -1435,6 +1457,7 @@ enum LogMessages : uint8_t { //OW RADIOLINK LOG_RADIO_LINK_STATS_MSG_RX, LOG_RADIO_LINK_STATS_MSG_TX, + LOG_RADIO_LINK_STATS_MSG_EXT, //OWEND _LOG_LAST_MSG_ diff --git a/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp index 9bb4d243401af..7e4c11aafb93d 100644 --- a/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp @@ -681,6 +681,11 @@ void AP_Mount_STorM32_MAVLink::handle_message_extra(const mavlink_message_t &msg _got_radio_rc_channels = true; } #endif +#ifdef MAVLINK_MSG_ID_RADIO_RC_CHANNELS_DEV + if (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS_DEV) { + _got_radio_rc_channels = true; + } +#endif // this msg is not from our gimbal if (msg.sysid != _sysid || msg.compid != _compid) { diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index be2f61d7d2061..756296491adfd 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -554,7 +554,7 @@ bool AP_RCProtocol::protocol_enabled(rcprotocol_t protocol) const } //OW RADIOLINK -void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) +void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_dev_t* packet) { // receiving this message is also used to check if the receiver is present // so let's first do the receiver detection @@ -582,7 +582,7 @@ void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_t* } }; -void AP_RCProtocol::handle_radio_link_stats(const mavlink_radio_link_stats_t* packet) +void AP_RCProtocol::handle_radio_link_stats(const mavlink_radio_link_stats_dev_t* packet) { // can be handled like CRSF (= receiver) or like RADIO_STATUS (= telemetry) // the user does decide it via RssiType::RECEIVER or RssiType::TELEMETRY_RADIO_RSSI setting diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index cf471f9b5dc23..081de5871c57d 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -208,8 +208,8 @@ class AP_RCProtocol { //OW RADIOLINK // handle mavlink radio - void handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet); - void handle_radio_link_stats(const mavlink_radio_link_stats_t* packet); + void handle_radio_rc_channels(const mavlink_radio_rc_channels_dev_t* packet); + void handle_radio_link_stats(const mavlink_radio_link_stats_dev_t* packet); //OWEND private: diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index d57f6d3e0732f..19496ffa1d2d4 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -50,8 +50,8 @@ class AP_RCProtocol_Backend { //OW RADIOLINK // update from mavlink messages - virtual void update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) {} - virtual void update_radio_link_stats(const mavlink_radio_link_stats_t* packet) {} + virtual void update_radio_rc_channels(const mavlink_radio_rc_channels_dev_t* packet) {} + virtual void update_radio_link_stats(const mavlink_radio_link_stats_dev_t* packet) {} //OWEND // get number of frames, ignoring failsafe diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp index 4a9f10ea77ed8..c7f27d4692110 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp @@ -15,7 +15,7 @@ AP_RCProtocol_MAVLinkRadio::AP_RCProtocol_MAVLinkRadio(AP_RCProtocol &_frontend) AP_RCProtocol_Backend(_frontend) {} -void AP_RCProtocol_MAVLinkRadio::update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) +void AP_RCProtocol_MAVLinkRadio::update_radio_rc_channels(const mavlink_radio_rc_channels_dev_t* packet) { uint8_t count = packet->count; if (count >= MAX_RCIN_CHANNELS) count = MAX_RCIN_CHANNELS; @@ -32,23 +32,33 @@ void AP_RCProtocol_MAVLinkRadio::update_radio_rc_channels(const mavlink_radio_rc add_input(count, rc_chan, failsafe); } -void AP_RCProtocol_MAVLinkRadio::update_radio_link_stats(const mavlink_radio_link_stats_t* packet) +void AP_RCProtocol_MAVLinkRadio::update_radio_link_stats(const mavlink_radio_link_stats_dev_t* packet) { // update the backend's fields - rx_link_quality = (packet->rx_LQ != UINT8_MAX) ? packet->rx_LQ : -1; + if (packet->rx_LQ_rc != UINT8_MAX) { + rx_link_quality = packet->rx_LQ_rc; + } else + if (packet->rx_LQ_ser != UINT8_MAX) { + rx_link_quality = packet->rx_LQ_ser; + } else { + rx_link_quality = -1; + } int32_t _rssi = -1; - if (packet->rx_receive_antenna == UINT8_MAX || packet->rx_rssi2 == UINT8_MAX) { - // no diversity + // comment: according to standard, receive_antenna = UINT8_MAX indicates no diversity + // but the code also catches the case that it's indicated by receive_antenna = 0 & rssi2 = UINT8_MAX + + if (packet->rx_receive_antenna == UINT8_MAX) { + // no receive diversity, receiving on antenna 0 if (packet->rx_rssi1 != UINT8_MAX) _rssi = packet->rx_rssi1; } else if (packet->rx_receive_antenna == 1) { - // diversity, receiving on antenna 1 + // receive diversity, receiving on antenna 1 if (packet->rx_rssi2 != UINT8_MAX) _rssi = packet->rx_rssi2; // UINT8_MAX should not happen, but play it safe } else { - // diversity, receiving on antenna 0 + // receive diversity, receiving on antenna 0 if (packet->rx_rssi1 != UINT8_MAX) _rssi = packet->rx_rssi1; // UINT8_MAX should not happen, but play it safe } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h index 0f3e9734b99c5..954fc1cc294c1 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h @@ -18,8 +18,8 @@ class AP_RCProtocol_MAVLinkRadio : public AP_RCProtocol_Backend { AP_RCProtocol_MAVLinkRadio(AP_RCProtocol &_frontend); // update from mavlink messages - void update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) override; - void update_radio_link_stats(const mavlink_radio_link_stats_t* packet) override; + void update_radio_rc_channels(const mavlink_radio_rc_channels_dev_t* packet) override; + void update_radio_link_stats(const mavlink_radio_link_stats_dev_t* packet) override; }; #endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index d49cc55379a72..11af0861bf39c 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4140,7 +4140,7 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) break; //OW RADIOLINK #if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED - case MAVLINK_MSG_ID_RADIO_RC_CHANNELS: + case MAVLINK_MSG_ID_RADIO_RC_CHANNELS_DEV: handle_radio_rc_channels(msg); #if HAL_MOUNT_ENABLED handle_mount_message(msg); @@ -6842,8 +6842,8 @@ MAV_RESULT GCS_MAVLINK::handle_control_high_latency(const mavlink_command_int_t void GCS_MAVLINK::handle_radio_rc_channels(const mavlink_message_t &msg) { - mavlink_radio_rc_channels_t packet; - mavlink_msg_radio_rc_channels_decode(&msg, &packet); + mavlink_radio_rc_channels_dev_t packet; + mavlink_msg_radio_rc_channels_dev_decode(&msg, &packet); #if AP_RCPROTOCOL_ENABLED AP::RC().handle_radio_rc_channels(&packet); @@ -6854,8 +6854,8 @@ void GCS_MAVLINK::handle_radio_rc_channels(const mavlink_message_t &msg) // AP_RSSI::RssiType::RECEIVER -> rssi is taken from RADIO_LINK_STATS void GCS_MAVLINK::handle_radio_link_stats(const mavlink_message_t &msg, bool log_radio) { - mavlink_radio_link_stats_t packet; - mavlink_msg_radio_link_stats_decode(&msg, &packet); + mavlink_radio_link_stats_dev_t packet; + mavlink_msg_radio_link_stats_dev_decode(&msg, &packet); #if AP_RCPROTOCOL_ENABLED AP::RC().handle_radio_link_stats(&packet); From cb0e62492bc42914d6cb0ac413501450607520db Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 3 Jan 2024 21:14:24 +0100 Subject: [PATCH 105/125] v bump --- libraries/bp_version.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 43a0fe2ebf749..5e8867e62ea23 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,6 +1,6 @@ #pragma once -#define BETAPILOTVERSION "v059f" +#define BETAPILOTVERSION "v060a" #define DATEOFBASEBRANCH "20231230" /* @@ -65,7 +65,6 @@ ArduPlane specific - AP_Mount.h: 7x - AP_RCProtocol.cpp: 4x RADIOLINK - AP_RCProtocol.h: 4x RADIOLINK -- AP_RSSI.h: 1x RADIOLINK - GCS_Common.cpp: 2x 3x RADIOLINK - GCS_MAVLink.h: 2x - GCS.h: 1x 1x RADIOLINK From 820a32b8af3f7c05f324b85df2fa4a5c46659624 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Thu, 25 Jan 2024 08:13:02 +0100 Subject: [PATCH 106/125] THR_SUPP --- ArduPlane/Parameters.cpp | 4 ++++ ArduPlane/Parameters.h | 6 ++++++ ArduPlane/servos.cpp | 7 +++++++ 3 files changed, 17 insertions(+) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index fd5288223d8f8..08996ce3bbb77 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -737,6 +737,10 @@ const AP_Param::Info Plane::var_info[] = { // @User: Advanced ASCALAR(crash_detection_enable, "CRASH_DETECT", 0), +//OW THR_SUPP + GSCALAR(takeoff_throttle_suppress, "TKOFF_THR_SUPP", 0), +//OWEND + // @Group: BARO // @Path: ../libraries/AP_Baro/AP_Baro.cpp GOBJECT(barometer, "BARO", AP_Baro), diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index ef126900dfccf..df2e4242b6a97 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -357,6 +357,9 @@ class Parameters { k_param_acro_yaw_rate, k_param_takeoff_throttle_max_t, k_param_autotune_options, +//OW THR_SUPP + k_param_takeoff_throttle_suppress, +//OWEND }; AP_Int16 format_version; @@ -467,6 +470,9 @@ class Parameters { AP_Int8 override_channel; #endif AP_Int16 gcs_pid_mask; +//OW THR_SUPP + AP_Int8 takeoff_throttle_suppress; +//OWEND }; /* diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index c8f03bb896848..e1ecadb18e736 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -552,6 +552,13 @@ void Plane::set_throttle(void) SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM); } } else if (suppress_throttle()) { +//OW THR_SUPP + if (control_mode == &mode_takeoff && g.takeoff_throttle_suppress) { + // in takeoff mode set throttle to specified value + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_float(g.takeoff_throttle_suppress, 0.0f, 100.0f)); + + } else +//OWEND if (g.throttle_suppress_manual) { // manual pass through of throttle while throttle is suppressed SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); From 70cf18ff045c1ebb329558bb8a4f63b12d4dabcc Mon Sep 17 00:00:00 2001 From: olliw42 Date: Thu, 25 Jan 2024 08:13:28 +0100 Subject: [PATCH 107/125] revised RADIO_RC_CHANNELS --- bp_mavlink/olliw_dev.xml | 21 +++++++++++++++------ libraries/AP_Logger/LogFile.cpp | 12 ++---------- libraries/AP_Logger/LogStructure.h | 13 ++----------- libraries/bp_version.h | 4 ++-- 4 files changed, 21 insertions(+), 29 deletions(-) diff --git a/bp_mavlink/olliw_dev.xml b/bp_mavlink/olliw_dev.xml index 627080726aeb2..45bd1f87969e7 100644 --- a/bp_mavlink/olliw_dev.xml +++ b/bp_mavlink/olliw_dev.xml @@ -48,15 +48,24 @@ For compatibility with legacy method. UINT8_MAX: unknown. - Radio channels. Supports up to 32 channels. Channel values are in centered 13 bit format. Range is [-4096,4096], center is 0. Conversion to PWM is x * 5/32 + 1500. - The target_system field should normally be set to the system id of the system the radio receiver is connected to, the target_component field can normally be set to 0. + RC channel inputs for a flight controller and associated components; the message does not replace the RC_CHANNELS and similar output messages. + Supports up to 32 channels. Channel values are in centered 13 bit format. Range is [-4096,4096], center is 0. Conversion to PWM is x * 5/32 + 1500. + The target_system field should normally be set to the system id of the recipient, typically the flight controller. + The target_component field can normally be set to 0, so that all components of the system can receive the message. + The time_last_update_ms field indicates the time when the RC channels data were last updated. If this time is significantly shorter than the + expected update time for this message, then the RC channels data can be considered invalid. + In this case, or the case of a failsafe, the RC channels data can be froozen and carry, e.g., the last valid data, or failsafe values + configured in the sender of the message; the exact behavior is not defined by the protocol but up to the implementation of the sender. + The default is to carry the last valid data. + Note: The RC channels fields are extensions to ensure that they are located at the end of the serialized payload and subject to MAVLink's trailing-zero trimming. System ID (can be 0 for broadcast, but this is discouraged). Component ID (normally 0 for broadcast). - Total number of RC channels being received. This can be larger than 32, indicating that more channels are available but not given in this message. + Time when RC channels were last updated (time since boot in the sender's time domain). Radio RC channels status flags. + Total number of RC channels being received. This can be larger than 32, indicating that more channels are available but not given in this message. - RC channels. Channels above count should be set to 0, to benefit from MAVLink's zero padding. + RC channels. Channels above count should be set to 0, to benefit from MAVLink's trailing-zero trimming. Radio link statistics. Tx: ground-side device, Rx: vehicle-side device. @@ -75,7 +84,6 @@ Noise on antenna2. Radio link dependent. INT8_MAX: ignore/unknown, use rx_snr1. 0: antenna1, 1: antenna2, UINT8_MAX: no Rx receive diversity, use rx_rssi1, rx_snr1. 0: antenna1, 1: antenna2, UINT8_MAX: no Rx transmit diversity. - Rx transmit power in dBm. INT8_MAX: unknown. Link quality of serial MAVLink data stream from Rx to Tx. Values: 1..100, 0: no link connection, UINT8_MAX: unknown. Rssi of antenna1. 254: no link connection. UINT8_MAX: unknown. Noise on antenna1. Radio link dependent. INT8_MAX: unknown. @@ -83,7 +91,6 @@ Noise on antenna2. Radio link dependent. INT8_MAX: ignore/unknown, use tx_snr1. 0: antenna1, 1: antenna2, UINT8_MAX: no Tx receive diversity, use tx_rssi1, tx_snr1. 0: antenna1, 1: antenna2, UINT8_MAX: no Tx transmit diversity. - Tx transmit power in dBm. INT8_MAX: unknown. Radio link information. Tx: ground-side device, Rx: vehicle-side device. Can normally be send at a low rate, like 0.2 Hz. @@ -95,6 +102,8 @@ Operation mode. Radio link dependent. UINT8_MAX: unknown. Packet rate in Hz for Tx to Rx transmission. 0: unknown. Packet rate in Hz for Rx to Tx transmission. Normally equal to tx_rate. 0: unknown. + Tx transmit power in dBm. INT8_MAX: unknown. + Rx transmit power in dBm. INT8_MAX: unknown. Receive sensitivity of Tx in inverted dBm. 1..255 represents -1..-255 dBm, 0: unknown. Receive sensitivity of Rx in inverted dBm. 1..255 represents -1..-255 dBm, 0: unknown. diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 85a2f15b14c26..279b165d31f96 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -410,7 +410,7 @@ void AP_Logger::Write_RadioLinkStats(const mavlink_radio_link_stats_dev_t &packe rssi2 : packet.rx_rssi2, snr2 : packet.rx_snr2, receive_antenna : packet.rx_receive_antenna, - transmit_antenna : packet.rx_transmit_antenna, + transmit_antenna : packet.rx_transmit_antenna }; WriteBlock(&pktrx, sizeof(pktrx)); @@ -424,17 +424,9 @@ void AP_Logger::Write_RadioLinkStats(const mavlink_radio_link_stats_dev_t &packe snr2 : packet.tx_snr2, receive_antenna : packet.tx_receive_antenna, transmit_antenna : packet.tx_transmit_antenna, + flags : packet.flags }; WriteBlock(&pkttx, sizeof(pkttx)); - - const struct log_RadioLinkStatsExt pktext{ - LOG_PACKET_HEADER_INIT(LOG_RADIO_LINK_STATS_MSG_EXT), - time_us : AP_HAL::micros64(), - tx_power : packet.tx_power, - rx_power : packet.rx_power, - flags : packet.flags - }; - WriteBlock(&pktext, sizeof(pktext)); } //OWEND diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index e07a1a207e485..58f02558587ba 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -419,13 +419,6 @@ struct PACKED log_RadioLinkStatsTx { int8_t snr2; uint8_t receive_antenna; uint8_t transmit_antenna; -}; - -struct PACKED log_RadioLinkStatsExt { - LOG_PACKET_HEADER; - uint64_t time_us; - int8_t tx_power; - int8_t rx_power; uint8_t flags; }; //OWEND @@ -1273,11 +1266,9 @@ LOG_STRUCTURE_FROM_PRECLAND \ { LOG_RADIO_MSG, sizeof(log_Radio), \ "RAD", "QBBBBBHH", "TimeUS,RSSI,RemRSSI,TxBuf,Noise,RemNoise,RxErrors,Fixed", "s-------", "F-------", true }, \ { LOG_RADIO_LINK_STATS_MSG_RX, sizeof(log_RadioLinkStatsRx), \ - "RDRX", "QBBBbBbBB", "TimeUS,rxLQrc,rxLQser,rxRssi1,rxSnr1,rxRssi2,rxSnr2,rxRAnt,rxTAnt", "s%%------", "F--------", true }, \ + "RDRX", "QBBBbBbBB", "TimeUS,rxLQrc,rxLQser,rxRssi1,rxSnr1,rxRssi2,rxSnr2,rxRAn,rxTAn", "s%%------", "F--------", true }, \ { LOG_RADIO_LINK_STATS_MSG_TX, sizeof(log_RadioLinkStatsTx), \ - "RDTX", "QBBbBbBB", "TimeUS,txLQser,txRssi1,txSnr1,txRssi2,txSnr2,txRAnt,txTAnt", "s%------", "F-------", true }, \ - { LOG_RADIO_LINK_STATS_MSG_EXT, sizeof(log_RadioLinkStatsExt), \ - "RDTR", "QbbB", "TimeUS,txPwr,rxPwr,flags", "s---", "F---", true }, \ + "RDTX", "QBBbBbBBB", "TimeUS,txLQser,txRssi1,txSnr1,txRssi2,txSnr2,txRAn,txTAn,flags", "s%-------", "F--------", true }, \ LOG_STRUCTURE_FROM_CAMERA \ LOG_STRUCTURE_FROM_MOUNT \ { LOG_ARSP_MSG, sizeof(log_ARSP), "ARSP", "QBffcffBBffB", "TimeUS,I,Airspeed,DiffPress,Temp,RawPress,Offset,U,H,Hp,TR,Pri", "s#nPOPP-----", "F-00B00-----", true }, \ diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 5e8867e62ea23..d1606a567e4ba 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,10 +1,10 @@ #pragma once -#define BETAPILOTVERSION "v060a" +#define BETAPILOTVERSION "v060c" #define DATEOFBASEBRANCH "20231230" /* -search for //OW to find all changes +search for //OW to find all changes THR_MINSPD THR_SLEW SUPP_MAN on-top features: - RADIO_LINK (OW RADIOLINK) From 6021c3458b4cfd8fb515571bf9764dc8dabe619f Mon Sep 17 00:00:00 2001 From: olliw42 Date: Fri, 16 Feb 2024 09:30:29 +0100 Subject: [PATCH 108/125] cc --- bp_mavlink/olliw_dev.xml | 16 ++++++++-------- libraries/bp_version.h | 4 ++-- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/bp_mavlink/olliw_dev.xml b/bp_mavlink/olliw_dev.xml index 45bd1f87969e7..5566d2a884a26 100644 --- a/bp_mavlink/olliw_dev.xml +++ b/bp_mavlink/olliw_dev.xml @@ -49,14 +49,14 @@ RC channel inputs for a flight controller and associated components; the message does not replace the RC_CHANNELS and similar output messages. - Supports up to 32 channels. Channel values are in centered 13 bit format. Range is [-4096,4096], center is 0. Conversion to PWM is x * 5/32 + 1500. - The target_system field should normally be set to the system id of the recipient, typically the flight controller. + Supports up to 32 channels. Channel values are in centered 13 bit format. Range is -4096 to 4096, center is 0. Conversion to PWM is x * 5/32 + 1500. + The target_system field should normally be set to the system id of the system to control, typically the flight controller. The target_component field can normally be set to 0, so that all components of the system can receive the message. - The time_last_update_ms field indicates the time when the RC channels data were last updated. If this time is significantly shorter than the - expected update time for this message, then the RC channels data can be considered invalid. - In this case, or the case of a failsafe, the RC channels data can be froozen and carry, e.g., the last valid data, or failsafe values - configured in the sender of the message; the exact behavior is not defined by the protocol but up to the implementation of the sender. - The default is to carry the last valid data. + The time_last_update_ms field indicates the time, in the sender's time domain, when the RC channels data were last updated. If this time is significantly shorter + than the expected update time for this message, then the RC channels data can be considered invalid, and the RC channels data should carry the last valid data. + In the case of a failsafe (when the RADIO_RC_CHANNELS_FLAGS_FAILSAFE bit set in the flags field), the RC channels data can be frozen and carried forward, e.g., + the last valid data or failsafe values configured in the sender of the message. The exact behavior is not defined by the protocol but up to the implementation + of the sender; the default is to carry the last valid data. Note: The RC channels fields are extensions to ensure that they are located at the end of the serialized payload and subject to MAVLink's trailing-zero trimming. System ID (can be 0 for broadcast, but this is discouraged). @@ -65,7 +65,7 @@ Radio RC channels status flags. Total number of RC channels being received. This can be larger than 32, indicating that more channels are available but not given in this message. - RC channels. Channels above count should be set to 0, to benefit from MAVLink's trailing-zero trimming. + RC channels. Channels that are above the field count should be set to 0, to benefit from MAVLink's trailing-zero trimming. Radio link statistics. Tx: ground-side device, Rx: vehicle-side device. diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 717d12c37c4a4..8ce96a642c7f1 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,10 +1,10 @@ #pragma once -#define BETAPILOTVERSION "v060d" +#define BETAPILOTVERSION "v060e" #define DATEOFBASEBRANCH "20240125" /* -search for //OW to find all changes THR_MINSPD THR_SLEW SUPP_MAN +search for //OW to find all changes THR_MINSPD THR_SLEW SUPP_MAN waiting for rudder release on-top features: - RADIO_LINK (OW RADIOLINK) From 0522573cc82d658fe925eb7f7c0d467cb7d7921e Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 17 Feb 2024 10:27:23 +0100 Subject: [PATCH 109/125] date update --- libraries/bp_version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 8ce96a642c7f1..5cf71514e3881 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,7 +1,7 @@ #pragma once #define BETAPILOTVERSION "v060e" -#define DATEOFBASEBRANCH "20240125" +#define DATEOFBASEBRANCH "20240216" /* search for //OW to find all changes THR_MINSPD THR_SLEW SUPP_MAN waiting for rudder release From 1468035c6a73d495cacfb14440353b2e5afb6ccf Mon Sep 17 00:00:00 2001 From: olliw42 Date: Fri, 23 Feb 2024 09:52:53 +0100 Subject: [PATCH 110/125] cc vbump --- libraries/bp_version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 35e3e639ef7bb..365866f0de178 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,7 +1,7 @@ #pragma once #define BETAPILOTVERSION "v060e" -#define DATEOFBASEBRANCH "20240221" +#define DATEOFBASEBRANCH "20240223" /* search for //OW to find all changes THR_MINSPD THR_SLEW SUPP_MAN waiting for rudder release From 8895a99cbdda2919df0c4598e085fd0b47a25ccd Mon Sep 17 00:00:00 2001 From: olliw42 Date: Mon, 26 Feb 2024 10:34:08 +0100 Subject: [PATCH 111/125] update rc channels protocol to latest --- bp_mavlink/olliw_dev.xml | 90 +++++++++++++++-------- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 32 +++----- 2 files changed, 70 insertions(+), 52 deletions(-) diff --git a/bp_mavlink/olliw_dev.xml b/bp_mavlink/olliw_dev.xml index 5566d2a884a26..621582454ed05 100644 --- a/bp_mavlink/olliw_dev.xml +++ b/bp_mavlink/olliw_dev.xml @@ -1,27 +1,42 @@ - + + RADIO_RC_CHANNELS flags (bitmask). + + Failsafe is active. The content of the RC channels data in the RADIO_RC_CHANNELS message is implementation dependent. + + + Channel data may be out of date. This is set when the receiver is unable to validate incoming data from the transmitter and has therefore resent the last valid data it received. + + + + RADIO_LINK_STATS flags (bitmask). + + Rssi values are in negative dBm. Values 0..253 corresponds to 0..-253 dBm. 254 represents no link connection. + + + RADIO_LINK_TYPE enum. - + Unknown radio link type. - + Radio link is HereLink. - + Radio link is Dragon Link. - + Radio link is RFD900. - + Radio link is Crossfire. - + Radio link is ExpressLRS. - + Radio link is mLRS. @@ -48,34 +63,40 @@ For compatibility with legacy method. UINT8_MAX: unknown. - RC channel inputs for a flight controller and associated components; the message does not replace the RC_CHANNELS and similar output messages. - Supports up to 32 channels. Channel values are in centered 13 bit format. Range is -4096 to 4096, center is 0. Conversion to PWM is x * 5/32 + 1500. + RC channel outputs from a MAVLink RC receiver for input to a flight controller or other components (allows an RC receiver to connect via MAVLink instead of some other protocol such as PPM-Sum or S.BUS). + Note that this is not intended to be an over-the-air format, and does not replace RC_CHANNELS and similar messages reported by the flight controller. The target_system field should normally be set to the system id of the system to control, typically the flight controller. The target_component field can normally be set to 0, so that all components of the system can receive the message. - The time_last_update_ms field indicates the time, in the sender's time domain, when the RC channels data were last updated. If this time is significantly shorter - than the expected update time for this message, then the RC channels data can be considered invalid, and the RC channels data should carry the last valid data. - In the case of a failsafe (when the RADIO_RC_CHANNELS_FLAGS_FAILSAFE bit set in the flags field), the RC channels data can be frozen and carried forward, e.g., - the last valid data or failsafe values configured in the sender of the message. The exact behavior is not defined by the protocol but up to the implementation - of the sender; the default is to carry the last valid data. + The channels array field can publish up to 32 channels; the number of channel items used in the array is specified in the count field. + The time_last_update_ms field contains the timestamp of the last received valid channels data in the receiver's time domain. + The count field indicates the first index of the channel array that is not used for channel data (this and later indexes are zero-filled). + The RADIO_RC_CHANNELS_FLAGS_OUTDATED flag is set by the receiver if the channels data is not up-to-date (for example, if new data from the transmitter could not be validated so the last valid data is resent). + The RADIO_RC_CHANNELS_FLAGS_FAILSAFE failsafe flag is set by the receiver if the receiver's failsafe condition is met (implementation dependent, e.g., connection to the RC radio is lost). + In this case time_last_update_ms still contains the timestamp of the last valid channels data, but the content of the channels data is not defined by the protocol (it is up to the implementation of the receiver). + For instance, the channels data could contain failsafe values configured in the receiver; the default is to carry the last valid data. Note: The RC channels fields are extensions to ensure that they are located at the end of the serialized payload and subject to MAVLink's trailing-zero trimming. - - System ID (can be 0 for broadcast, but this is discouraged). + + System ID (ID of target system, normally flight controller). Component ID (normally 0 for broadcast). - Time when RC channels were last updated (time since boot in the sender's time domain). - Radio RC channels status flags. + Time when the data in the channels field were last updated (time since boot in the receiver's time domain). + Radio RC channels status flags. Total number of RC channels being received. This can be larger than 32, indicating that more channels are available but not given in this message. - RC channels. Channels that are above the field count should be set to 0, to benefit from MAVLink's trailing-zero trimming. + RC channels. + Channel values are in centered 13 bit format. Range is -4096 to 4096, center is 0. Conversion to PWM is x * 5/32 + 1500. + Channels with indexes equal or above count should be set to 0, to benefit from MAVLink's trailing-zero trimming. - Radio link statistics. Tx: ground-side device, Rx: vehicle-side device. - Per default, rssi values are in MAVLink units: 0 represents weakest signal, 254 represents maximum signal; can be changed to inverted dBm with the flag - RADIO_LINK_STATS_FLAGS_RSSI_DBM: 0..253 correspond to 0..-253 dBm, 254 represents no link connection. - The target_system field should normally be set to the system id of the system the radio receiver is connected to, the target_component field can normally be set to 0. + Radio link statistics. Tx: ground-side device, Rx: vehicle-side device. + The message is normally emitted upon each receiption of a data packet on the link. + Per default, rssi values are in MAVLink units: 0 represents weakest signal, 254 represents maximum signal. + The units are inverted dBm if the flag RADIO_LINK_STATS_FLAGS_RSSI_DBM is set: 0..253 correspond to 0..-253 dBm, 254 represents no link connection. + The target_system field should normally be set to the system id of the system the link is connected to, typically the flight controller. + The target_component field can normally be set to 0, so that all components of the system can receive the message. - System ID (can be 0 for broadcast, but this is discouraged). + System ID (ID of target system, normally flight controller). Component ID (normally 0 for broadcast). - Radio link statistics flags. + Radio link statistics flags. Link quality of RC data stream from Tx to Rx. Values: 1..100, 0: no link connection, UINT8_MAX: unknown. Link quality of serial MAVLink data stream from Tx to Rx. Values: 1..100, 0: no link connection, UINT8_MAX: unknown. Rssi of antenna1. 254: no link connection, UINT8_MAX: unknown. @@ -93,15 +114,20 @@ 0: antenna1, 1: antenna2, UINT8_MAX: no Tx transmit diversity. - Radio link information. Tx: ground-side device, Rx: vehicle-side device. Can normally be send at a low rate, like 0.2 Hz. - The target_system field should normally be set to the system id of the system the radio receiver is connected to, the target_component field can normally be set to 0. + Radio link information. Tx: ground-side device, Rx: vehicle-side device. + The values of the fields in this message do normally not or only slowly change with time, and for most times the message can be send at a low rate, like 0.2 Hz. + If values change then the message should temporarily be send more often to inform the system about the changes. + The target_system field should normally be set to the system id of the system the link is connected to, typically the flight controller. + The target_component field can normally be set to 0, so that all components of the system can receive the message. - System ID (can be 0 for broadcast, but this is discouraged). + System ID (ID of target system, normally flight controller). Component ID (normally 0 for broadcast). - Radio link type. 0: unknown/generic type. + Radio link type. 0: unknown/generic type. Operation mode. Radio link dependent. UINT8_MAX: unknown. - Packet rate in Hz for Tx to Rx transmission. 0: unknown. - Packet rate in Hz for Rx to Tx transmission. Normally equal to tx_rate. 0: unknown. + Packet rate in Hz for Tx to Rx transmission. 0: unknown. + Packet rate in Hz for Rx to Tx transmission. Normally equal to tx_packet_rate. 0: unknown. + Data rate of serial stream in Bytes/s for Tx to Rx transmission. 0: unknown. UINT16_MAX: data rate is 64 KBytes/s or larger. + Data rate of serial stream in Bytes/s for Rx to Tx transmission. 0: unknown. UINT16_MAX: data rate is 64 KBytes/s or larger. Tx transmit power in dBm. INT8_MAX: unknown. Rx transmit power in dBm. INT8_MAX: unknown. Receive sensitivity of Tx in inverted dBm. 1..255 represents -1..-255 dBm, 0: unknown. diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index ab849ed3dcab7..bb9b3f7de7745 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -431,6 +431,11 @@ bool AP_RCProtocol::new_input() #if AP_RCPROTOCOL_DRONECAN_ENABLED AP_RCProtocol::DRONECAN, #endif +//OW RADIOLINK +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + AP_RCProtocol::MAVLINK_RADIO, +#endif +//OWEND }; for (const auto protocol : pollable) { if (!detect_async_protocol(protocol)) { @@ -606,29 +611,16 @@ bool AP_RCProtocol::protocol_enabled(rcprotocol_t protocol) const //OW RADIOLINK void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_dev_t* packet) { - // receiving this message is also used to check if the receiver is present - // so let's first do the receiver detection - if (_detected_protocol == AP_RCProtocol::NONE) { // still searching -#if AP_RC_CHANNEL_ENABLED - rc_protocols_mask = rc().enabled_protocols(); -#endif - if (!protocol_enabled(MAVLINK_RADIO)) return; // not our turn - _detected_protocol = AP_RCProtocol::MAVLINK_RADIO; - } - - // here now comes the message handling itself - - if (_detected_protocol != AP_RCProtocol::MAVLINK_RADIO) { + // take a shortcut if protocol is known to be MAVLINK_RADIO + if (_detected_protocol == AP_RCProtocol::MAVLINK_RADIO) { + backend[_detected_protocol]->update_radio_rc_channels(packet); return; } - // now update the backend - backend[_detected_protocol]->update_radio_rc_channels(packet); - - // now we can ask the backend if it got a new input - if (backend[_detected_protocol]->new_input()) { - _new_input = true; - _last_input_ms = AP_HAL::millis(); + for (uint8_t i = 0; i < ARRAY_SIZE(backend); i++) { + if (backend[i] != nullptr) { + backend[i]->update_radio_rc_channels(packet); + } } }; From 3eafd26db91d6b679ffce25976031a7ef2df622e Mon Sep 17 00:00:00 2001 From: olliw42 Date: Fri, 1 Mar 2024 14:53:02 +0100 Subject: [PATCH 112/125] update & sync to main --- bp_mavlink/common.xml | 25 +++++++++- bp_mavlink/development.xml | 33 +++++++++++++ bp_mavlink/olliw_dev.xml | 33 ------------- bp_mavlink/storm32.xml | 48 +------------------ libraries/AP_RCProtocol/AP_RCProtocol.cpp | 4 +- libraries/AP_RCProtocol/AP_RCProtocol.h | 4 +- .../AP_RCProtocol/AP_RCProtocol_Backend.h | 2 +- .../AP_RCProtocol_MavlinkRadio.cpp | 12 ++--- .../AP_RCProtocol_MavlinkRadio.h | 4 +- libraries/GCS_MAVLink/GCS_Common.cpp | 12 ++--- libraries/bp_version.h | 2 +- 11 files changed, 73 insertions(+), 106 deletions(-) diff --git a/bp_mavlink/common.xml b/bp_mavlink/common.xml index ffb8e4e37575c..c781d1369280f 100644 --- a/bp_mavlink/common.xml +++ b/bp_mavlink/common.xml @@ -1919,6 +1919,16 @@ Reserved (set to 0) + + Change state of safety switch. + New safety switch state. + Empty. + Empty. + Empty + Empty. + Empty. + Empty. + Trigger the start of an ADSB-out IDENT. This should only be used when requested to do so by an Air Traffic Controller in controlled airspace. This starts the IDENT which is then typically held for 18 seconds by the hardware per the Mode A, C, and S transponder spec. Reserved (set to 0) @@ -4123,6 +4133,17 @@ Mission has executed all mission items. + + + Possible safety switch states. + + + Safety switch is engaged and vehicle should be safe to approach. + + + Safety switch is NOT engaged and motors, propellers and other actuators should be considered active. + + @@ -4914,8 +4935,8 @@ Optical flow from a flow sensor (e.g. optical mouse sensor) Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. Sensor ID - Flow in x-sensor direction - Flow in y-sensor direction + Flow rate around X-axis (deprecated; use flow_rate_x) + Flow rate around Y-axis (deprecated; use flow_rate_y) Flow in x-sensor direction, angular-speed compensated Flow in y-sensor direction, angular-speed compensated Optical flow quality / confidence. 0: bad, 255: maximum quality diff --git a/bp_mavlink/development.xml b/bp_mavlink/development.xml index dbe60ab8b3600..aeceaff67eede 100644 --- a/bp_mavlink/development.xml +++ b/bp_mavlink/development.xml @@ -14,6 +14,15 @@ True if the data from this sensor is being actively used by the flight controller for guidance, navigation or control. + + RADIO_RC_CHANNELS flags (bitmask). + + Failsafe is active. The content of the RC channels data in the RADIO_RC_CHANNELS message is implementation dependent. + + + Channel data may be out of date. This is set when the receiver is unable to validate incoming data from the transmitter and has therefore resent the last valid data it received. + + @@ -38,5 +47,29 @@ Raw differential pressure. NaN for value unknown/not supplied. Airspeed sensor flags. + + RC channel outputs from a MAVLink RC receiver for input to a flight controller or other components (allows an RC receiver to connect via MAVLink instead of some other protocol such as PPM-Sum or S.BUS). + Note that this is not intended to be an over-the-air format, and does not replace RC_CHANNELS and similar messages reported by the flight controller. + The target_system field should normally be set to the system id of the system to control, typically the flight controller. + The target_component field can normally be set to 0, so that all components of the system can receive the message. + The channels array field can publish up to 32 channels; the number of channel items used in the array is specified in the count field. + The time_last_update_ms field contains the timestamp of the last received valid channels data in the receiver's time domain. + The count field indicates the first index of the channel array that is not used for channel data (this and later indexes are zero-filled). + The RADIO_RC_CHANNELS_FLAGS_OUTDATED flag is set by the receiver if the channels data is not up-to-date (for example, if new data from the transmitter could not be validated so the last valid data is resent). + The RADIO_RC_CHANNELS_FLAGS_FAILSAFE failsafe flag is set by the receiver if the receiver's failsafe condition is met (implementation dependent, e.g., connection to the RC radio is lost). + In this case time_last_update_ms still contains the timestamp of the last valid channels data, but the content of the channels data is not defined by the protocol (it is up to the implementation of the receiver). + For instance, the channels data could contain failsafe values configured in the receiver; the default is to carry the last valid data. + Note: The RC channels fields are extensions to ensure that they are located at the end of the serialized payload and subject to MAVLink's trailing-zero trimming. + + System ID (ID of target system, normally flight controller). + Component ID (normally 0 for broadcast). + Time when the data in the channels field were last updated (time since boot in the receiver's time domain). + Radio RC channels status flags. + Total number of RC channels being received. This can be larger than 32, indicating that more channels are available but not given in this message. + + RC channels. + Channel values are in centered 13 bit format. Range is -4096 to 4096, center is 0. Conversion to PWM is x * 5/32 + 1500. + Channels with indexes equal or above count should be set to 0, to benefit from MAVLink's trailing-zero trimming. + diff --git a/bp_mavlink/olliw_dev.xml b/bp_mavlink/olliw_dev.xml index 621582454ed05..bbad52067f364 100644 --- a/bp_mavlink/olliw_dev.xml +++ b/bp_mavlink/olliw_dev.xml @@ -1,15 +1,6 @@ - - RADIO_RC_CHANNELS flags (bitmask). - - Failsafe is active. The content of the RC channels data in the RADIO_RC_CHANNELS message is implementation dependent. - - - Channel data may be out of date. This is set when the receiver is unable to validate incoming data from the transmitter and has therefore resent the last valid data it received. - - RADIO_LINK_STATS flags (bitmask). @@ -62,30 +53,6 @@ Receive bandwidth consumption. Values: 0..100, UINT8_MAX: invalid/unknown. For compatibility with legacy method. UINT8_MAX: unknown. - - RC channel outputs from a MAVLink RC receiver for input to a flight controller or other components (allows an RC receiver to connect via MAVLink instead of some other protocol such as PPM-Sum or S.BUS). - Note that this is not intended to be an over-the-air format, and does not replace RC_CHANNELS and similar messages reported by the flight controller. - The target_system field should normally be set to the system id of the system to control, typically the flight controller. - The target_component field can normally be set to 0, so that all components of the system can receive the message. - The channels array field can publish up to 32 channels; the number of channel items used in the array is specified in the count field. - The time_last_update_ms field contains the timestamp of the last received valid channels data in the receiver's time domain. - The count field indicates the first index of the channel array that is not used for channel data (this and later indexes are zero-filled). - The RADIO_RC_CHANNELS_FLAGS_OUTDATED flag is set by the receiver if the channels data is not up-to-date (for example, if new data from the transmitter could not be validated so the last valid data is resent). - The RADIO_RC_CHANNELS_FLAGS_FAILSAFE failsafe flag is set by the receiver if the receiver's failsafe condition is met (implementation dependent, e.g., connection to the RC radio is lost). - In this case time_last_update_ms still contains the timestamp of the last valid channels data, but the content of the channels data is not defined by the protocol (it is up to the implementation of the receiver). - For instance, the channels data could contain failsafe values configured in the receiver; the default is to carry the last valid data. - Note: The RC channels fields are extensions to ensure that they are located at the end of the serialized payload and subject to MAVLink's trailing-zero trimming. - - System ID (ID of target system, normally flight controller). - Component ID (normally 0 for broadcast). - Time when the data in the channels field were last updated (time since boot in the receiver's time domain). - Radio RC channels status flags. - Total number of RC channels being received. This can be larger than 32, indicating that more channels are available but not given in this message. - - RC channels. - Channel values are in centered 13 bit format. Range is -4096 to 4096, center is 0. Conversion to PWM is x * 5/32 + 1500. - Channels with indexes equal or above count should be set to 0, to benefit from MAVLink's trailing-zero trimming. - Radio link statistics. Tx: ground-side device, Rx: vehicle-side device. The message is normally emitted upon each receiption of a data packet on the link. diff --git a/bp_mavlink/storm32.xml b/bp_mavlink/storm32.xml index e385eb48895d3..7a0bf2ecd1e98 100644 --- a/bp_mavlink/storm32.xml +++ b/bp_mavlink/storm32.xml @@ -10,7 +10,7 @@ Range of IDs: Documentation: STORM32 and QSHOT additions with mLRS additions merged - 3. Feb. 2023 + 29. Feb. 2024 All messages are technically WIP, but some are quite stable now. Quite stable means that it is in practical use, but may see extension. A more detailed description of the concept underlying the STORM32 and QSHOT messages can be found here: @@ -278,24 +278,6 @@ Documentation: Set shot state or command. The allowed values are specific to the selected shot mode. - - - RADIO_RC_CHANNELS flags (bitmask). - - Failsafe is active. - - - Indicates that the current frame has not been received. Channel values are frozen. - - - - RADIO_LINK_STATS flags (bitmask). - - Rssi are in negative dBm. Values 0..254 corresponds to 0..-254 dBm. - - - - - Radio channels. Supports up to 24 channels. Channel values are in centerd 13 bit format. Range is [-4096,4096], center is 0. Conversion to PWM is x * 5/32 + 1500. Should be emitted only by components with component id MAV_COMP_ID_TELEMETRY_RADIO. - Total number of RC channels being received. This can be larger than 24, indicating that more channels are available but not given in this message. - Radio channels status flags. - - RC channels. Channels above count should be set to 0, to benefit from MAVLink's zero padding. - - - Radio link statistics. Should be emitted only by components with component id MAV_COMP_ID_TELEMETRY_RADIO. Per default, rssi values are in MAVLink units: 0 represents weakest signal, 254 represents maximum signal; can be changed to dBm with the flag RADIO_LINK_STATS_FLAGS_RSSI_DBM. - Radio link statistics flags. - Values: 0..100. UINT8_MAX: invalid/unknown. - Rssi of antenna1. UINT8_MAX: invalid/unknown. - Noise on antenna1. Radio dependent. INT8_MAX: invalid/unknown. - Rssi of antenna2. UINT8_MAX: ignore/unknown, use rx_rssi1. - Noise on antenna2. Radio dependent. INT8_MAX: ignore/unknown, use rx_snr1. - 0: antenna1, 1: antenna2, UINT8_MAX: ignore, no Rx receive diversity, use rx_rssi1, rx_snr1. - 0: antenna1, 1: antenna2, UINT8_MAX: ignore, no Rx transmit diversity. - Values: 0..100. UINT8_MAX: invalid/unknown. - Rssi of antenna1. UINT8_MAX: invalid/unknown. - Noise on antenna1. Radio dependent. INT8_MAX: invalid/unknown. - Rssi of antenna2. UINT8_MAX: ignore/unknown, use tx_rssi1. - Noise on antenna2. Radio dependent. INT8_MAX: ignore/unknown, use tx_snr1. - 0: antenna1, 1: antenna2, UINT8_MAX: ignore, no Tx receive diversity, use tx_rssi1, tx_snr1. - 0: antenna1, 1: antenna2, UINT8_MAX: ignore, no Tx transmit diversity. - Frsky SPort passthrough multi packet container. diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index bb9b3f7de7745..4143909547759 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -609,7 +609,8 @@ bool AP_RCProtocol::protocol_enabled(rcprotocol_t protocol) const } //OW RADIOLINK -void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_dev_t* packet) +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED +void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) { // take a shortcut if protocol is known to be MAVLINK_RADIO if (_detected_protocol == AP_RCProtocol::MAVLINK_RADIO) { @@ -638,6 +639,7 @@ void AP_RCProtocol::handle_radio_link_stats(const mavlink_radio_link_stats_dev_t // now update the backend backend[_detected_protocol]->update_radio_link_stats(packet); } +#endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED //OWEND namespace AP { diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index 0891ae85f9468..ecf33cd8481e1 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -218,8 +218,10 @@ class AP_RCProtocol { //OW RADIOLINK // handle mavlink radio - void handle_radio_rc_channels(const mavlink_radio_rc_channels_dev_t* packet); +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + void handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet); void handle_radio_link_stats(const mavlink_radio_link_stats_dev_t* packet); +#endif //OWEND private: diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index 0d17f28ded50f..b44494e3dbf73 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -46,7 +46,7 @@ class AP_RCProtocol_Backend { //OW RADIOLINK // update from mavlink messages - virtual void update_radio_rc_channels(const mavlink_radio_rc_channels_dev_t* packet) {} + virtual void update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) {} virtual void update_radio_link_stats(const mavlink_radio_link_stats_dev_t* packet) {} //OWEND diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp index c7f27d4692110..58b37502aa39d 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp @@ -10,15 +10,9 @@ #include "AP_RCProtocol_MAVLinkRadio.h" -// constructor -AP_RCProtocol_MAVLinkRadio::AP_RCProtocol_MAVLinkRadio(AP_RCProtocol &_frontend) : - AP_RCProtocol_Backend(_frontend) -{} - -void AP_RCProtocol_MAVLinkRadio::update_radio_rc_channels(const mavlink_radio_rc_channels_dev_t* packet) +void AP_RCProtocol_MAVLinkRadio::update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) { - uint8_t count = packet->count; - if (count >= MAX_RCIN_CHANNELS) count = MAX_RCIN_CHANNELS; + const uint8_t count = MIN(packet->count, MAX_RCIN_CHANNELS); uint16_t rc_chan[MAX_RCIN_CHANNELS]; for (uint8_t i = 0; i < count; i++) { @@ -67,7 +61,7 @@ void AP_RCProtocol_MAVLinkRadio::update_radio_link_stats(const mavlink_radio_lin return; } - if (packet->flags & RADIO_LINK_STATS_FLAGS_RSSI_DBM) { + if (packet->flags & RADIO_LINK_STATS_FLAGS_RSSI_DBM_DEV) { // rssi is in negative dBm, 255 = unknown, 254 = no link connection, 0...253 = 0...-253 dBm // convert to AP rssi using the same logic as in CRSF driver // AP rssi: -1 for unknown, 0 for no link connection, 255 for maximum link diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h index 954fc1cc294c1..b65dc1cc0fcc6 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.h @@ -15,10 +15,10 @@ class AP_RCProtocol_MAVLinkRadio : public AP_RCProtocol_Backend { public: - AP_RCProtocol_MAVLinkRadio(AP_RCProtocol &_frontend); + using AP_RCProtocol_Backend::AP_RCProtocol_Backend; // update from mavlink messages - void update_radio_rc_channels(const mavlink_radio_rc_channels_dev_t* packet) override; + void update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) override; void update_radio_link_stats(const mavlink_radio_link_stats_dev_t* packet) override; }; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index fbf6bd95563f0..6dc7e63e56c06 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4204,7 +4204,7 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) break; //OW RADIOLINK #if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED - case MAVLINK_MSG_ID_RADIO_RC_CHANNELS_DEV: + case MAVLINK_MSG_ID_RADIO_RC_CHANNELS: handle_radio_rc_channels(msg); #if HAL_MOUNT_ENABLED handle_mount_message(msg); @@ -7002,15 +7002,12 @@ MAV_RESULT GCS_MAVLINK::handle_control_high_latency(const mavlink_command_int_t //OW RADIOLINK #if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED - void GCS_MAVLINK::handle_radio_rc_channels(const mavlink_message_t &msg) { - mavlink_radio_rc_channels_dev_t packet; - mavlink_msg_radio_rc_channels_dev_decode(&msg, &packet); + mavlink_radio_rc_channels_t packet; + mavlink_msg_radio_rc_channels_decode(&msg, &packet); -#if AP_RCPROTOCOL_ENABLED AP::RC().handle_radio_rc_channels(&packet); -#endif } // AP_RSSI::RssiType::TELEMETRY_RADIO_RSSI -> rssi is taken from RADIO_STATUS @@ -7020,9 +7017,7 @@ void GCS_MAVLINK::handle_radio_link_stats(const mavlink_message_t &msg) mavlink_radio_link_stats_dev_t packet; mavlink_msg_radio_link_stats_dev_decode(&msg, &packet); -#if AP_RCPROTOCOL_ENABLED AP::RC().handle_radio_link_stats(&packet); -#endif #if HAL_LOGGING_ENABLED // log link stats if logging Performance monitoring data @@ -7031,7 +7026,6 @@ void GCS_MAVLINK::handle_radio_link_stats(const mavlink_message_t &msg) } #endif } - #endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED //OWEND diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 365866f0de178..9dbc5735cd514 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,7 +1,7 @@ #pragma once #define BETAPILOTVERSION "v060e" -#define DATEOFBASEBRANCH "20240223" +#define DATEOFBASEBRANCH "20240301" /* search for //OW to find all changes THR_MINSPD THR_SLEW SUPP_MAN waiting for rudder release From 8c30083c247209a9ffccdaf5da07fd81fa1090f6 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 2 Mar 2024 11:39:31 +0100 Subject: [PATCH 113/125] revise RADIO stuff --- bp_mavlink/olliw_dev.xml | 36 +++++++++---------- libraries/AP_Logger/LogStructure.h | 1 - .../AP_Mount/AP_Mount_STorM32_MAVLink.cpp | 9 +---- libraries/AP_Mount/AP_Mount_config.h | 2 +- libraries/AP_RCProtocol/AP_RCProtocol.h | 2 ++ .../AP_RCProtocol/AP_RCProtocol_Backend.h | 2 ++ .../AP_RCProtocol_MavlinkRadio.cpp | 24 ++++++------- .../AP_RCProtocol/AP_RCProtocol_config.h | 3 +- libraries/GCS_MAVLink/GCS_Common.cpp | 8 ++--- libraries/bp_version.h | 2 +- 10 files changed, 42 insertions(+), 47 deletions(-) diff --git a/bp_mavlink/olliw_dev.xml b/bp_mavlink/olliw_dev.xml index bbad52067f364..e205bee2dcac0 100644 --- a/bp_mavlink/olliw_dev.xml +++ b/bp_mavlink/olliw_dev.xml @@ -57,10 +57,10 @@ Radio link statistics. Tx: ground-side device, Rx: vehicle-side device. The message is normally emitted upon each receiption of a data packet on the link. Per default, rssi values are in MAVLink units: 0 represents weakest signal, 254 represents maximum signal. - The units are inverted dBm if the flag RADIO_LINK_STATS_FLAGS_RSSI_DBM is set: 0..253 correspond to 0..-253 dBm, 254 represents no link connection. + The RADIO_LINK_STATS_FLAGS_RSSI_DBM flag is set if the units are inverted dBm: 0..253 correspond to 0..-253 dBm, 254 represents no link connection. The target_system field should normally be set to the system id of the system the link is connected to, typically the flight controller. The target_component field can normally be set to 0, so that all components of the system can receive the message. - + System ID (ID of target system, normally flight controller). Component ID (normally 0 for broadcast). Radio link statistics flags. @@ -68,17 +68,17 @@ Link quality of serial MAVLink data stream from Tx to Rx. Values: 1..100, 0: no link connection, UINT8_MAX: unknown. Rssi of antenna1. 254: no link connection, UINT8_MAX: unknown. Noise on antenna1. Radio link dependent. INT8_MAX: unknown. - Rssi of antenna2. 254: no link connection, UINT8_MAX: ignore/unknown, use rx_rssi1. - Noise on antenna2. Radio link dependent. INT8_MAX: ignore/unknown, use rx_snr1. - 0: antenna1, 1: antenna2, UINT8_MAX: no Rx receive diversity, use rx_rssi1, rx_snr1. - 0: antenna1, 1: antenna2, UINT8_MAX: no Rx transmit diversity. + Rssi of antenna2. 254: no link connection, UINT8_MAX: use rx_rssi1 if known else unknown. + Noise on antenna2. Radio link dependent. INT8_MAX: use rx_snr1 if known else unknown. Link quality of serial MAVLink data stream from Rx to Tx. Values: 1..100, 0: no link connection, UINT8_MAX: unknown. Rssi of antenna1. 254: no link connection. UINT8_MAX: unknown. Noise on antenna1. Radio link dependent. INT8_MAX: unknown. - Rssi of antenna2. 254: no link connection. UINT8_MAX: ignore/unknown, use tx_rssi1. - Noise on antenna2. Radio link dependent. INT8_MAX: ignore/unknown, use tx_snr1. - 0: antenna1, 1: antenna2, UINT8_MAX: no Tx receive diversity, use tx_rssi1, tx_snr1. - 0: antenna1, 1: antenna2, UINT8_MAX: no Tx transmit diversity. + Rssi of antenna2. 254: no link connection. UINT8_MAX: use tx_rssi1 if known else unknown. + Noise on antenna2. Radio link dependent. INT8_MAX: use tx_snr1 if known else unknown. + 0: antenna1, 1: antenna2. If the antenna is not known, assume antenna1. + 0: antenna1, 1: antenna2. If the antenna is not known, assume antenna1. + 0: antenna1, 1: antenna2. If the antenna is not known, assume antenna1. + 0: antenna1, 1: antenna2. If the antenna is not known, assume antenna1. Radio link information. Tx: ground-side device, Rx: vehicle-side device. @@ -86,19 +86,19 @@ If values change then the message should temporarily be send more often to inform the system about the changes. The target_system field should normally be set to the system id of the system the link is connected to, typically the flight controller. The target_component field can normally be set to 0, so that all components of the system can receive the message. - + System ID (ID of target system, normally flight controller). Component ID (normally 0 for broadcast). Radio link type. 0: unknown/generic type. - Operation mode. Radio link dependent. UINT8_MAX: unknown. - Packet rate in Hz for Tx to Rx transmission. 0: unknown. - Packet rate in Hz for Rx to Tx transmission. Normally equal to tx_packet_rate. 0: unknown. - Data rate of serial stream in Bytes/s for Tx to Rx transmission. 0: unknown. UINT16_MAX: data rate is 64 KBytes/s or larger. - Data rate of serial stream in Bytes/s for Rx to Tx transmission. 0: unknown. UINT16_MAX: data rate is 64 KBytes/s or larger. + Operation mode. Radio link dependent. UINT8_MAX: ignore/unknown. Tx transmit power in dBm. INT8_MAX: unknown. Rx transmit power in dBm. INT8_MAX: unknown. - Receive sensitivity of Tx in inverted dBm. 1..255 represents -1..-255 dBm, 0: unknown. - Receive sensitivity of Rx in inverted dBm. 1..255 represents -1..-255 dBm, 0: unknown. + Packet rate in Hz for Tx to Rx transmission. 0: ignore. + Packet rate in Hz for Rx to Tx transmission. Normally equal to tx_packet_rate. 0: ignore. + Data rate of serial stream in Bytes/s for Tx to Rx transmission. 0: ignore. UINT16_MAX: data rate is 64 KBytes/s or larger. + Data rate of serial stream in Bytes/s for Rx to Tx transmission. 0: ignore. UINT16_MAX: data rate is 64 KBytes/s or larger. + Receive sensitivity of Tx in inverted dBm. 1..255 represents -1..-255 dBm, 0: ignore. + Receive sensitivity of Rx in inverted dBm. 1..255 represents -1..-255 dBm, 0: ignore. diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 46480add729de..7dd07b7b2c3ca 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -1484,7 +1484,6 @@ enum LogMessages : uint8_t { //OW RADIOLINK LOG_RADIO_LINK_STATS_MSG_RX, LOG_RADIO_LINK_STATS_MSG_TX, - LOG_RADIO_LINK_STATS_MSG_EXT, //OWEND _LOG_LAST_MSG_ diff --git a/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp index dfda62ca0b7eb..6f135610dc3d8 100644 --- a/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_STorM32_MAVLink.cpp @@ -676,16 +676,9 @@ void AP_Mount_STorM32_MAVLink::handle_message_extra(const mavlink_message_t &msg } // listen to RADIO_RC_CHANNELS messages to stop sending RC_CHANNELS -#ifdef MAVLINK_MSG_ID_RADIO_RC_CHANNELS - if (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS) { // 60045 + if (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS) { _got_radio_rc_channels = true; } -#endif -#ifdef MAVLINK_MSG_ID_RADIO_RC_CHANNELS_DEV - if (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS_DEV) { - _got_radio_rc_channels = true; - } -#endif // this msg is not from our gimbal if (msg.sysid != _sysid || msg.compid != _compid) { diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index a079971ed04a1..ee6914d1939ef 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/libraries/AP_Mount/AP_Mount_config.h @@ -44,7 +44,7 @@ //OW #ifndef HAL_MOUNT_STORM32_MAVLINK_V2_ENABLED -#define HAL_MOUNT_STORM32_MAVLINK_V2_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED +#define HAL_MOUNT_STORM32_MAVLINK_V2_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED && HAL_GCS_ENABLED #endif //OWEND diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index ecf33cd8481e1..edf9b7757eef5 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -20,7 +20,9 @@ #include #include +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED #include +#endif #define MAX_RCIN_CHANNELS 18 #define MIN_RCIN_CHANNELS 5 diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index b44494e3dbf73..a34e78def5267 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -46,8 +46,10 @@ class AP_RCProtocol_Backend { //OW RADIOLINK // update from mavlink messages +#if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED virtual void update_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) {} virtual void update_radio_link_stats(const mavlink_radio_link_stats_dev_t* packet) {} +#endif //OWEND // get number of frames, ignoring failsafe diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp index 58b37502aa39d..db26754e10a1d 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_MavlinkRadio.cpp @@ -41,19 +41,19 @@ void AP_RCProtocol_MAVLinkRadio::update_radio_link_stats(const mavlink_radio_lin int32_t _rssi = -1; - // comment: according to standard, receive_antenna = UINT8_MAX indicates no diversity - // but the code also catches the case that it's indicated by receive_antenna = 0 & rssi2 = UINT8_MAX - - if (packet->rx_receive_antenna == UINT8_MAX) { - // no receive diversity, receiving on antenna 0 - if (packet->rx_rssi1 != UINT8_MAX) _rssi = packet->rx_rssi1; - } else - if (packet->rx_receive_antenna == 1) { - // receive diversity, receiving on antenna 1 - if (packet->rx_rssi2 != UINT8_MAX) _rssi = packet->rx_rssi2; // UINT8_MAX should not happen, but play it safe + if (packet->rx_receive_antenna > 0) { // should be 1, but just always assume antenna1 is meant + // receiving on antenna 1 + if (packet->rx_rssi2 != UINT8_MAX) { + _rssi = packet->rx_rssi2; + } else + if (packet->rx_rssi1 != UINT8_MAX) { // value is stored in rx_rssi1 + _rssi = packet->rx_rssi1; + } } else { - // receive diversity, receiving on antenna 0 - if (packet->rx_rssi1 != UINT8_MAX) _rssi = packet->rx_rssi1; // UINT8_MAX should not happen, but play it safe + // receiving on antenna 0 + if (packet->rx_rssi1 != UINT8_MAX) { + _rssi = packet->rx_rssi1; + } } if (_rssi == -1) { // no rssi value set diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_config.h b/libraries/AP_RCProtocol/AP_RCProtocol_config.h index 190f135373896..6cf15737740a8 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_config.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_config.h @@ -2,6 +2,7 @@ #include #include +#include #ifndef AP_RCPROTOCOL_ENABLED #define AP_RCPROTOCOL_ENABLED 1 @@ -72,7 +73,7 @@ //OW RADIOLINK #ifndef AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED -#define AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024 +#define AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024 && HAL_GCS_ENABLED #endif //OWEND diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 6dc7e63e56c06..27dbb621847bb 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4099,11 +4099,6 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) case MAVLINK_MSG_ID_RADIO_STATUS: handle_radio_status(msg); break; -//OW RADIOLINK - case MAVLINK_MSG_ID_RADIO_LINK_STATS_DEV: - handle_radio_link_stats(msg); - break; -//OWEND #if AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED case MAVLINK_MSG_ID_SERIAL_CONTROL: @@ -4210,6 +4205,9 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) handle_mount_message(msg); #endif break; + case MAVLINK_MSG_ID_RADIO_LINK_STATS_DEV: + handle_radio_link_stats(msg); + break; #endif //OWEND #endif diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 9dbc5735cd514..0b39b16a56c01 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,6 +1,6 @@ #pragma once -#define BETAPILOTVERSION "v060e" +#define BETAPILOTVERSION "v060f" #define DATEOFBASEBRANCH "20240301" /* From 4d085952c2ccf3d83feae10bfdfb70c6775dc8a5 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 6 Mar 2024 02:11:57 +0100 Subject: [PATCH 114/125] revised mavlink radio --- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 4143909547759..12aa7dc8f0e0c 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -386,6 +386,10 @@ bool AP_RCProtocol::detect_async_protocol(rcprotocol_t protocol) return false; } + if (!protocol_enabled(protocol)) { + return false; + } + if (_detected_protocol == protocol) { // we are using this protocol already, see if there is new // data. Caller will handle the case where we stop presenting @@ -612,17 +616,11 @@ bool AP_RCProtocol::protocol_enabled(rcprotocol_t protocol) const #if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet) { - // take a shortcut if protocol is known to be MAVLINK_RADIO - if (_detected_protocol == AP_RCProtocol::MAVLINK_RADIO) { - backend[_detected_protocol]->update_radio_rc_channels(packet); + if (backend[AP_RCProtocol::MAVLINK_RADIO] == nullptr) { return; } - for (uint8_t i = 0; i < ARRAY_SIZE(backend); i++) { - if (backend[i] != nullptr) { - backend[i]->update_radio_rc_channels(packet); - } - } + backend[AP_RCProtocol::MAVLINK_RADIO]->update_radio_rc_channels(packet); }; void AP_RCProtocol::handle_radio_link_stats(const mavlink_radio_link_stats_dev_t* packet) @@ -637,7 +635,7 @@ void AP_RCProtocol::handle_radio_link_stats(const mavlink_radio_link_stats_dev_t } // now update the backend - backend[_detected_protocol]->update_radio_link_stats(packet); + backend[AP_RCProtocol::MAVLINK_RADIO]->update_radio_link_stats(packet); } #endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED //OWEND From 7b5db977877f436e6247609ea7e322d87aee0636 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 6 Mar 2024 11:30:55 +0100 Subject: [PATCH 115/125] cc --- libraries/bp_version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 0b39b16a56c01..c468e60e354fb 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,7 +1,7 @@ #pragma once #define BETAPILOTVERSION "v060f" -#define DATEOFBASEBRANCH "20240301" +#define DATEOFBASEBRANCH "20240306" /* search for //OW to find all changes THR_MINSPD THR_SLEW SUPP_MAN waiting for rudder release From 8089cac7004428707f95911f3f079ed18dd8f473 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 9 Mar 2024 10:47:15 +0100 Subject: [PATCH 116/125] add RADIO_LINK_STATS_MLRS for deving --- bp_mavlink/olliw_dev.xml | 44 ++++++++++++++++++++++------ libraries/GCS_MAVLink/GCS_Common.cpp | 40 +++++++++++++++++++++++++ libraries/bp_version.h | 2 +- 3 files changed, 76 insertions(+), 10 deletions(-) diff --git a/bp_mavlink/olliw_dev.xml b/bp_mavlink/olliw_dev.xml index e205bee2dcac0..92195ba074a39 100644 --- a/bp_mavlink/olliw_dev.xml +++ b/bp_mavlink/olliw_dev.xml @@ -53,9 +53,35 @@ Receive bandwidth consumption. Values: 0..100, UINT8_MAX: invalid/unknown. For compatibility with legacy method. UINT8_MAX: unknown. + + + Radio link statistics. mLRS specific. + + System ID (ID of target system, normally flight controller). + Component ID (normally 0 for broadcast). + Radio link statistics flags. + Link quality of RC data stream from Tx to Rx. Values: 1..100, 0: no link connection, UINT8_MAX: unknown. + Link quality of serial MAVLink data stream from Tx to Rx. Values: 1..100, 0: no link connection, UINT8_MAX: unknown. + Rssi of antenna1. 254: no reception, UINT8_MAX: unknown. + Noise on antenna1. Radio link dependent. INT8_MAX: unknown. + Rssi of antenna2. 254: no reception, UINT8_MAX: use rx_rssi1 if known else unknown. + Noise on antenna2. Radio link dependent. INT8_MAX: use rx_snr1 if known else unknown. + Link quality of serial MAVLink data stream from Rx to Tx. Values: 1..100, 0: no link connection, UINT8_MAX: unknown. + Rssi of antenna1. 254: no reception. UINT8_MAX: unknown. + Noise on antenna1. Radio link dependent. INT8_MAX: unknown. + Rssi of antenna2. 254: no reception. UINT8_MAX: use tx_rssi1 if known else unknown. + Noise on antenna2. Radio link dependent. INT8_MAX: use tx_snr1 if known else unknown. + 0: antenna1, 1: antenna2. If antenna is not known, assume antenna1. + 0: antenna1, 1: antenna2. If antenna is not known, assume antenna1. + 0: antenna1, 1: antenna2. If antenna is not known, assume antenna1. In a dual diversity/band system use 0 to indicate transmission on both antenna. + 0: antenna1, 1: antenna2. If antenna is not known, assume antenna1. In a dual diversity/band system use 0 to indicate transmission on both antenna. + Frequency on antenna1 in Hz. + Frequency on antenna2 in Hz. + + Radio link statistics. Tx: ground-side device, Rx: vehicle-side device. - The message is normally emitted upon each receiption of a data packet on the link. + The message is normally emitted upon each reception of a data packet on the link. Per default, rssi values are in MAVLink units: 0 represents weakest signal, 254 represents maximum signal. The RADIO_LINK_STATS_FLAGS_RSSI_DBM flag is set if the units are inverted dBm: 0..253 correspond to 0..-253 dBm, 254 represents no link connection. The target_system field should normally be set to the system id of the system the link is connected to, typically the flight controller. @@ -66,19 +92,19 @@ Radio link statistics flags. Link quality of RC data stream from Tx to Rx. Values: 1..100, 0: no link connection, UINT8_MAX: unknown. Link quality of serial MAVLink data stream from Tx to Rx. Values: 1..100, 0: no link connection, UINT8_MAX: unknown. - Rssi of antenna1. 254: no link connection, UINT8_MAX: unknown. + Rssi of antenna1. 254: no reception, UINT8_MAX: unknown. Noise on antenna1. Radio link dependent. INT8_MAX: unknown. - Rssi of antenna2. 254: no link connection, UINT8_MAX: use rx_rssi1 if known else unknown. + Rssi of antenna2. 254: no reception, UINT8_MAX: use rx_rssi1 if known else unknown. Noise on antenna2. Radio link dependent. INT8_MAX: use rx_snr1 if known else unknown. Link quality of serial MAVLink data stream from Rx to Tx. Values: 1..100, 0: no link connection, UINT8_MAX: unknown. - Rssi of antenna1. 254: no link connection. UINT8_MAX: unknown. + Rssi of antenna1. 254: no reception. UINT8_MAX: unknown. Noise on antenna1. Radio link dependent. INT8_MAX: unknown. - Rssi of antenna2. 254: no link connection. UINT8_MAX: use tx_rssi1 if known else unknown. + Rssi of antenna2. 254: no reception. UINT8_MAX: use tx_rssi1 if known else unknown. Noise on antenna2. Radio link dependent. INT8_MAX: use tx_snr1 if known else unknown. - 0: antenna1, 1: antenna2. If the antenna is not known, assume antenna1. - 0: antenna1, 1: antenna2. If the antenna is not known, assume antenna1. - 0: antenna1, 1: antenna2. If the antenna is not known, assume antenna1. - 0: antenna1, 1: antenna2. If the antenna is not known, assume antenna1. + 0: antenna1, 1: antenna2. If antenna is not known, assume antenna1. + 0: antenna1, 1: antenna2. If antenna is not known, assume antenna1. + 0: antenna1, 1: antenna2. If antenna is not known, assume antenna1. In a dual diversity/band system use 0 to indicate transmission on both antenna. + 0: antenna1, 1: antenna2. If antenna is not known, assume antenna1. In a dual diversity/band system use 0 to indicate transmission on both antenna. Radio link information. Tx: ground-side device, Rx: vehicle-side device. diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 10c58d4ab60c3..0094b4f1e4a91 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4211,6 +4211,7 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) //OWEND break; //OW RADIOLINK + case MAVLINK_MSG_ID_RADIO_LINK_STATS_MLRS: case MAVLINK_MSG_ID_RADIO_LINK_STATS_DEV: handle_radio_link_stats(msg); break; @@ -7020,6 +7021,45 @@ void GCS_MAVLINK::handle_radio_rc_channels(const mavlink_message_t &msg) // AP_RSSI::RssiType::RECEIVER -> rssi is taken from RADIO_LINK_STATS void GCS_MAVLINK::handle_radio_link_stats(const mavlink_message_t &msg) { +if (msg.msgid == MAVLINK_MSG_ID_RADIO_LINK_STATS_MLRS) { + mavlink_radio_link_stats_mlrs_t packet_mlrs; + mavlink_msg_radio_link_stats_mlrs_decode(&msg, &packet_mlrs); + + mavlink_radio_link_stats_dev_t packet = { + .target_system = packet_mlrs.target_system, + .target_component = packet_mlrs.target_component, + .flags = packet_mlrs.flags, + .rx_LQ_rc = packet_mlrs.rx_LQ_rc, + .rx_LQ_ser = packet_mlrs.rx_LQ_ser, + .rx_rssi1 = packet_mlrs.rx_rssi1, + .rx_snr1 = packet_mlrs.rx_snr1, + .rx_rssi2 = packet_mlrs.rx_rssi2, + .rx_snr2 = packet_mlrs.rx_snr2, + .tx_LQ_ser = packet_mlrs.tx_LQ_ser, + .tx_rssi1 = packet_mlrs.tx_rssi1, + .tx_snr1 = packet_mlrs.tx_snr1, + .tx_rssi2 = packet_mlrs.tx_rssi2, + .tx_snr2 = packet_mlrs.tx_snr2, + .rx_receive_antenna = packet_mlrs.rx_receive_antenna, + .rx_transmit_antenna = packet_mlrs.rx_transmit_antenna, + .tx_receive_antenna = packet_mlrs.tx_receive_antenna, + .tx_transmit_antenna = packet_mlrs.tx_transmit_antenna, + }; + AP::RC().handle_radio_link_stats(&packet); + + if (AP::logger().should_log(log_radio_bit())) { + AP::logger().Write_RadioLinkStats(packet); + + AP::logger().Write( + "RDFR", "TimeUS,f1,f2", "szz", "F--", "Qff", + AP_HAL::micros64(), + packet_mlrs.frequency1, + packet_mlrs.frequency2 + ); + } + return; +} + mavlink_radio_link_stats_dev_t packet; mavlink_msg_radio_link_stats_dev_decode(&msg, &packet); diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 3b05b5ce81860..cfa861ee45261 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,6 +1,6 @@ #pragma once -#define BETAPILOTVERSION "v060f" +#define BETAPILOTVERSION "v060g" #define DATEOFBASEBRANCH "20240309" /* From 04f7ab0a639f67020145dd0e9ac1369ee7232070 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 May 2024 10:09:21 +0200 Subject: [PATCH 117/125] v bump --- libraries/bp_version.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/bp_version.h b/libraries/bp_version.h index cfa861ee45261..5fb99e09ee485 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,7 +1,7 @@ #pragma once -#define BETAPILOTVERSION "v060g" -#define DATEOFBASEBRANCH "20240309" +#define BETAPILOTVERSION "v060h" +#define DATEOFBASEBRANCH "20240525" /* search for //OW to find all changes THR_MINSPD THR_SLEW SUPP_MAN waiting for rudder release @@ -12,9 +12,10 @@ on-top features: - RC_Channel, AUX_FUNC: CAMERA_SET_MODE, CAMERA_TRIG_MODE (eq CAM_MODE_TOGGLE) (OW CAMERA) - Plane THR (OW THR_SUPP) +2024.05.25: v060 + upgraded to ArduPilot master 4.6.0-dev 2023.11.06: v059 upgraded to ArduPilot master 4.5.0-dev, mount revised significantly - 2023.03.10: v058 upgraded to Copter4.3.6 stable 2023.03.10: v057.3 From 08337cfba3a2cf98a01dc042722cb2f52f197108 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 May 2024 10:19:34 +0200 Subject: [PATCH 118/125] remove zflags --- libraries/AP_Mount/AP_Mount_Params.cpp | 4 ---- libraries/AP_Mount/AP_Mount_Params.h | 4 ---- libraries/bp_version.h | 1 + 3 files changed, 1 insertion(+), 8 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Params.cpp b/libraries/AP_Mount/AP_Mount_Params.cpp index 3c3ab3bad7c51..57257eb5e4e2b 100644 --- a/libraries/AP_Mount/AP_Mount_Params.cpp +++ b/libraries/AP_Mount/AP_Mount_Params.cpp @@ -172,10 +172,6 @@ const AP_Param::GroupInfo AP_Mount_Params::var_info[] = { // @User: Standard AP_GROUPINFO("_OPTIONS", 16, AP_Mount_Params, options, 0), -//OW - AP_GROUPINFO("_ZFLAGS", 20, AP_Mount_Params, zflags, 0), -//OWEND - AP_GROUPEND }; diff --git a/libraries/AP_Mount/AP_Mount_Params.h b/libraries/AP_Mount/AP_Mount_Params.h index 234137e28534f..ccf42c1fe050c 100644 --- a/libraries/AP_Mount/AP_Mount_Params.h +++ b/libraries/AP_Mount/AP_Mount_Params.h @@ -32,8 +32,4 @@ class AP_Mount_Params { AP_Int8 sysid_default; // target sysid for mount to follow AP_Int32 dev_id; // Device id taking into account bus AP_Int8 options; // mount options bitmask - -//OW - AP_Int8 zflags; -//OWEND }; diff --git a/libraries/bp_version.h b/libraries/bp_version.h index 5fb99e09ee485..e6b07d841550b 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -12,6 +12,7 @@ on-top features: - RC_Channel, AUX_FUNC: CAMERA_SET_MODE, CAMERA_TRIG_MODE (eq CAM_MODE_TOGGLE) (OW CAMERA) - Plane THR (OW THR_SUPP) + remove zflags, not used, could use now options 2024.05.25: v060 upgraded to ArduPilot master 4.6.0-dev 2023.11.06: v059 From 60c539bbdf277bc3e8cfa674dca34ce3742afaee Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 May 2024 10:23:57 +0200 Subject: [PATCH 119/125] ups --- ArduCopter/version.h | 2 +- ArduPlane/version.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 082c88a3ef58c..965f6e1938a7f 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -11,7 +11,7 @@ //OW #undef THISFIRMWARE #include "../libraries/bp_version.h" -#define THISFIRMWARE "BetaCopter V4.5.0-dev " BETAPILOTVERSION " " DATEOFBASEBRANCH +#define THISFIRMWARE "BetaCopter V4.6.0-dev " BETAPILOTVERSION " " DATEOFBASEBRANCH //OWEND // the following line is parsed by the autotest scripts diff --git a/ArduPlane/version.h b/ArduPlane/version.h index 406987563c915..ca28fb0cc3dcf 100644 --- a/ArduPlane/version.h +++ b/ArduPlane/version.h @@ -11,7 +11,7 @@ //OW #undef THISFIRMWARE #include "../libraries/bp_version.h" -#define THISFIRMWARE "BetaPlane V4.5.0-dev " BETAPILOTVERSION " " DATEOFBASEBRANCH +#define THISFIRMWARE "BetaPlane V4.6.0-dev " BETAPILOTVERSION " " DATEOFBASEBRANCH //OWEND // the following line is parsed by the autotest scripts From cf228530f3a4c8163d26d7476b7f7dbd17992950 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 May 2024 11:48:05 +0200 Subject: [PATCH 120/125] brad's passthrough pr 27152 --- libraries/AP_HAL/UARTDriver.h | 8 ++- libraries/AP_HAL_ChibiOS/UARTDriver.cpp | 22 ++++++++ libraries/AP_HAL_ChibiOS/UARTDriver.h | 9 +++ .../AP_HAL_ChibiOS/hwdef/common/usbcfg.c | 14 +++++ .../AP_HAL_ChibiOS/hwdef/common/usbcfg.h | 3 + .../hwdef/common/usbcfg_dualcdc.c | 15 +++++ libraries/GCS_MAVLink/GCS.h | 4 ++ libraries/GCS_MAVLink/GCS_Common.cpp | 55 +++++++++++++++++++ 8 files changed, 129 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL/UARTDriver.h b/libraries/AP_HAL/UARTDriver.h index 829d7907b315f..f7639bce74978 100644 --- a/libraries/AP_HAL/UARTDriver.h +++ b/libraries/AP_HAL/UARTDriver.h @@ -108,6 +108,9 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { virtual enum flow_control get_flow_control(void) { return FLOW_CONTROL_DISABLE; } virtual void configure_parity(uint8_t v){}; +//BRAD + virtual uint8_t get_parity(void){ return 0; } +//BRADEND virtual void set_stop_bits(int n){}; /* unbuffered writes bypass the ringbuffer and go straight to the @@ -184,7 +187,10 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { // return true requested baud on USB port virtual uint32_t get_usb_baud(void) const { return 0; } - +//BRAD + // return requested parity on USB port + virtual uint8_t get_usb_parity(void) const { return 0; } +//BRADend // disable TX/RX pins for unusued uart virtual void disable_rxtx(void) const {} diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index 069f64085f98d..b2060947245fd 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -672,6 +672,21 @@ uint32_t UARTDriver::get_usb_baud() const return 0; } +//BRAD +/* + get the requested usb parity. Valid if get_usb_baud() returned non-zero. +*/ +uint8_t UARTDriver::get_usb_parity() const +{ +#if HAL_USE_SERIAL_USB + if (sdef.is_usb) { + return ::get_usb_parity(sdef.endpoint_id); + } +#endif + return 0; +} +//BRADEND + uint32_t UARTDriver::_available() { if (!_rx_initialised || _uart_owner_thd != chThdGetSelfX()) { @@ -1457,6 +1472,13 @@ void UARTDriver::configure_parity(uint8_t v) #endif // HAL_USE_SERIAL } +//BRAD +uint8_t UARTDriver::get_parity(void) +{ + return UARTDriver::parity; +} +//BRADEND + /* set stop bits */ diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index b28091e053e90..54046ed9fb44f 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -38,6 +38,9 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { bool is_initialized() override; bool tx_pending() override; uint32_t get_usb_baud() const override; +//BRAD + uint8_t get_usb_parity() const override; +//BRADEND // disable TX/RX pins for unusued uart void disable_rxtx(void) const override; @@ -88,6 +91,9 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { bool set_unbuffered_writes(bool on) override; void configure_parity(uint8_t v) override; +//BRAD + uint8_t get_parity(void) override; +//BRADEND void set_stop_bits(int n) override; /* @@ -215,6 +221,9 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { uint32_t _cr2_options; uint32_t _cr3_options; uint16_t _last_options; +//BRAD + uint8_t parity; +//BRADEND // half duplex control. After writing we throw away bytes for 4 byte widths to // prevent reading our own bytes back diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c index eed51ebae3ea3..c485099682e43 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c @@ -251,6 +251,20 @@ uint32_t get_usb_baud(uint16_t endpoint_id) } return 0; } + +//BRAD +/* + get the requested usb parity. Valid if get_usb_baud() returned non-zero +*/ +uint8_t get_usb_parity(uint16_t endpoint_id) +{ + if (endpoint_id == 0) { + return linecoding.bParityType; + } + + return 0; +} +//BRADEND #endif /** * @brief IN EP1 state. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h index 2add6e6c7f9bb..1cb2161c5e970 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h @@ -46,6 +46,9 @@ extern SerialUSBDriver SDU2; extern const SerialUSBConfig serusbcfg2; #endif //HAL_HAVE_DUAL_USB_CDC uint32_t get_usb_baud(uint16_t endpoint_id); +//BRAD +uint8_t get_usb_parity(uint16_t endpoint_id); +//BRADEND #endif #define USB_DESC_MAX_STRLEN 100 void setup_usb_strings(void); diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c index cb1a127287ca1..cf93382bf57fb 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c @@ -315,6 +315,21 @@ uint32_t get_usb_baud(uint16_t endpoint_id) } return 0; } + +//BRAD +/* + get the requested usb parity. Valid if get_usb_baud() returned non-zero +*/ +uint8_t get_usb_parity(uint16_t endpoint_id) +{ + for (uint8_t i = 0; i < ARRAY_SIZE(linecoding); i++) { + if (endpoint_id == ep_index[i]) { + return linecoding[i].bParityType; + } + } + return 0; +} +//BRADEND #endif /** * @brief IN EP1 state. diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 89bc6635e14f0..5f4964357d693 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -1340,6 +1340,10 @@ class GCS uint32_t last_port1_data_ms; uint32_t baud1; uint32_t baud2; +//BRAD + uint8_t parity1; + uint8_t parity2; +//BRADEND uint8_t timeout_s; HAL_Semaphore sem; } _passthru; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 26d2388205c44..a16b45ad051ec 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -6702,6 +6702,9 @@ void GCS::update_passthru(void) WITH_SEMAPHORE(_passthru.sem); uint32_t now = AP_HAL::millis(); uint32_t baud1, baud2; +//BRAD + uint8_t parity1 = 0, parity2 = 0; +//BRADEND bool enabled = AP::serialmanager().get_passthru(_passthru.port1, _passthru.port2, _passthru.timeout_s, baud1, baud2); if (enabled && !_passthru.enabled) { @@ -6711,6 +6714,10 @@ void GCS::update_passthru(void) _passthru.last_port1_data_ms = now; _passthru.baud1 = baud1; _passthru.baud2 = baud2; +//BRAD + _passthru.parity1 = parity1 = _passthru.port1->get_parity(); + _passthru.parity2 = parity2 = _passthru.port2->get_parity(); +//BRADEND gcs().send_text(MAV_SEVERITY_INFO, "Passthru enabled"); if (!_passthru.timer_installed) { _passthru.timer_installed = true; @@ -6729,6 +6736,15 @@ void GCS::update_passthru(void) _passthru.port2->end(); _passthru.port2->begin(baud2); } +//BRAD + // Restore original parity + if (_passthru.parity1 != parity1) { + _passthru.port1->configure_parity(parity1); + } + if (_passthru.parity2 != parity2) { + _passthru.port2->configure_parity(parity2); + } +//BRADEND gcs().send_text(MAV_SEVERITY_INFO, "Passthru disabled"); } else if (enabled && _passthru.timeout_s && @@ -6747,6 +6763,15 @@ void GCS::update_passthru(void) _passthru.port2->end(); _passthru.port2->begin(baud2); } +//BRAD + // Restore original parity + if (_passthru.parity1 != parity1) { + _passthru.port1->configure_parity(parity1); + } + if (_passthru.parity2 != parity2) { + _passthru.port2->configure_parity(parity2); + } +//BRAD gcs().send_text(MAV_SEVERITY_INFO, "Passthru timed out"); } } @@ -6781,18 +6806,48 @@ void GCS::passthru_timer(void) // Check for requested Baud rates over USB uint32_t baud = _passthru.port1->get_usb_baud(); +//BRAD +/* if (_passthru.baud2 != baud && baud != 0) { _passthru.baud2 = baud; _passthru.port2->end(); _passthru.port2->begin_locked(baud, 0, 0, lock_key); + } */ + uint8_t parity = _passthru.port1->get_usb_parity(); + if (baud != 0) { // port1 is USB + if (_passthru.baud2 != baud) { + _passthru.baud2 = baud; + _passthru.port2->end(); + _passthru.port2->begin_locked(baud, 0, 0, lock_key); + } + if (_passthru.parity2 != parity) { + _passthru.parity2 = parity; + _passthru.port2->configure_parity(parity); + } } +//BRADEND baud = _passthru.port2->get_usb_baud(); +//BRAD +/* if (_passthru.baud1 != baud && baud != 0) { _passthru.baud1 = baud; _passthru.port1->end(); _passthru.port1->begin_locked(baud, 0, 0, lock_key); + } */ + parity = _passthru.port2->get_usb_parity(); + if (baud != 0) { // port2 is USB + if (_passthru.baud1 != baud) { + _passthru.baud1 = baud; + _passthru.port1->end(); + _passthru.port1->begin_locked(baud, 0, 0, lock_key); + } + if (_passthru.parity1 != parity) { + _passthru.parity1 = parity; + _passthru.port1->configure_parity(parity); + } } +//BRADEND uint8_t buf[64]; From c8374c01dc9d3147b74f6c33172c4962ec1066c8 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 3 Sep 2024 09:12:09 +0200 Subject: [PATCH 121/125] remove --- libraries/AP_HAL/UARTDriver.h | 7 -- libraries/AP_HAL_ChibiOS/UARTDriver.cpp | 22 ------- libraries/AP_HAL_ChibiOS/UARTDriver.h | 9 --- .../AP_HAL_ChibiOS/hwdef/common/usbcfg.c | 14 ---- .../AP_HAL_ChibiOS/hwdef/common/usbcfg.h | 3 - .../hwdef/common/usbcfg_dualcdc.c | 15 ----- libraries/GCS_MAVLink/GCS.h | 4 -- libraries/GCS_MAVLink/GCS_Common.cpp | 65 ------------------- 8 files changed, 139 deletions(-) diff --git a/libraries/AP_HAL/UARTDriver.h b/libraries/AP_HAL/UARTDriver.h index f7639bce74978..cf269cce0d5c6 100644 --- a/libraries/AP_HAL/UARTDriver.h +++ b/libraries/AP_HAL/UARTDriver.h @@ -108,9 +108,6 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { virtual enum flow_control get_flow_control(void) { return FLOW_CONTROL_DISABLE; } virtual void configure_parity(uint8_t v){}; -//BRAD - virtual uint8_t get_parity(void){ return 0; } -//BRADEND virtual void set_stop_bits(int n){}; /* unbuffered writes bypass the ringbuffer and go straight to the @@ -187,10 +184,6 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { // return true requested baud on USB port virtual uint32_t get_usb_baud(void) const { return 0; } -//BRAD - // return requested parity on USB port - virtual uint8_t get_usb_parity(void) const { return 0; } -//BRADend // disable TX/RX pins for unusued uart virtual void disable_rxtx(void) const {} diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index b2060947245fd..069f64085f98d 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -672,21 +672,6 @@ uint32_t UARTDriver::get_usb_baud() const return 0; } -//BRAD -/* - get the requested usb parity. Valid if get_usb_baud() returned non-zero. -*/ -uint8_t UARTDriver::get_usb_parity() const -{ -#if HAL_USE_SERIAL_USB - if (sdef.is_usb) { - return ::get_usb_parity(sdef.endpoint_id); - } -#endif - return 0; -} -//BRADEND - uint32_t UARTDriver::_available() { if (!_rx_initialised || _uart_owner_thd != chThdGetSelfX()) { @@ -1472,13 +1457,6 @@ void UARTDriver::configure_parity(uint8_t v) #endif // HAL_USE_SERIAL } -//BRAD -uint8_t UARTDriver::get_parity(void) -{ - return UARTDriver::parity; -} -//BRADEND - /* set stop bits */ diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index 54046ed9fb44f..b28091e053e90 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -38,9 +38,6 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { bool is_initialized() override; bool tx_pending() override; uint32_t get_usb_baud() const override; -//BRAD - uint8_t get_usb_parity() const override; -//BRADEND // disable TX/RX pins for unusued uart void disable_rxtx(void) const override; @@ -91,9 +88,6 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { bool set_unbuffered_writes(bool on) override; void configure_parity(uint8_t v) override; -//BRAD - uint8_t get_parity(void) override; -//BRADEND void set_stop_bits(int n) override; /* @@ -221,9 +215,6 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { uint32_t _cr2_options; uint32_t _cr3_options; uint16_t _last_options; -//BRAD - uint8_t parity; -//BRADEND // half duplex control. After writing we throw away bytes for 4 byte widths to // prevent reading our own bytes back diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c index c485099682e43..eed51ebae3ea3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c @@ -251,20 +251,6 @@ uint32_t get_usb_baud(uint16_t endpoint_id) } return 0; } - -//BRAD -/* - get the requested usb parity. Valid if get_usb_baud() returned non-zero -*/ -uint8_t get_usb_parity(uint16_t endpoint_id) -{ - if (endpoint_id == 0) { - return linecoding.bParityType; - } - - return 0; -} -//BRADEND #endif /** * @brief IN EP1 state. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h index 1cb2161c5e970..2add6e6c7f9bb 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h @@ -46,9 +46,6 @@ extern SerialUSBDriver SDU2; extern const SerialUSBConfig serusbcfg2; #endif //HAL_HAVE_DUAL_USB_CDC uint32_t get_usb_baud(uint16_t endpoint_id); -//BRAD -uint8_t get_usb_parity(uint16_t endpoint_id); -//BRADEND #endif #define USB_DESC_MAX_STRLEN 100 void setup_usb_strings(void); diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c index cf93382bf57fb..cb1a127287ca1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c @@ -315,21 +315,6 @@ uint32_t get_usb_baud(uint16_t endpoint_id) } return 0; } - -//BRAD -/* - get the requested usb parity. Valid if get_usb_baud() returned non-zero -*/ -uint8_t get_usb_parity(uint16_t endpoint_id) -{ - for (uint8_t i = 0; i < ARRAY_SIZE(linecoding); i++) { - if (endpoint_id == ep_index[i]) { - return linecoding[i].bParityType; - } - } - return 0; -} -//BRADEND #endif /** * @brief IN EP1 state. diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 5f4964357d693..89bc6635e14f0 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -1340,10 +1340,6 @@ class GCS uint32_t last_port1_data_ms; uint32_t baud1; uint32_t baud2; -//BRAD - uint8_t parity1; - uint8_t parity2; -//BRADEND uint8_t timeout_s; HAL_Semaphore sem; } _passthru; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index a16b45ad051ec..93e5878e25f44 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -6702,9 +6702,6 @@ void GCS::update_passthru(void) WITH_SEMAPHORE(_passthru.sem); uint32_t now = AP_HAL::millis(); uint32_t baud1, baud2; -//BRAD - uint8_t parity1 = 0, parity2 = 0; -//BRADEND bool enabled = AP::serialmanager().get_passthru(_passthru.port1, _passthru.port2, _passthru.timeout_s, baud1, baud2); if (enabled && !_passthru.enabled) { @@ -6714,10 +6711,6 @@ void GCS::update_passthru(void) _passthru.last_port1_data_ms = now; _passthru.baud1 = baud1; _passthru.baud2 = baud2; -//BRAD - _passthru.parity1 = parity1 = _passthru.port1->get_parity(); - _passthru.parity2 = parity2 = _passthru.port2->get_parity(); -//BRADEND gcs().send_text(MAV_SEVERITY_INFO, "Passthru enabled"); if (!_passthru.timer_installed) { _passthru.timer_installed = true; @@ -6736,15 +6729,6 @@ void GCS::update_passthru(void) _passthru.port2->end(); _passthru.port2->begin(baud2); } -//BRAD - // Restore original parity - if (_passthru.parity1 != parity1) { - _passthru.port1->configure_parity(parity1); - } - if (_passthru.parity2 != parity2) { - _passthru.port2->configure_parity(parity2); - } -//BRADEND gcs().send_text(MAV_SEVERITY_INFO, "Passthru disabled"); } else if (enabled && _passthru.timeout_s && @@ -6763,15 +6747,6 @@ void GCS::update_passthru(void) _passthru.port2->end(); _passthru.port2->begin(baud2); } -//BRAD - // Restore original parity - if (_passthru.parity1 != parity1) { - _passthru.port1->configure_parity(parity1); - } - if (_passthru.parity2 != parity2) { - _passthru.port2->configure_parity(parity2); - } -//BRAD gcs().send_text(MAV_SEVERITY_INFO, "Passthru timed out"); } } @@ -6806,48 +6781,8 @@ void GCS::passthru_timer(void) // Check for requested Baud rates over USB uint32_t baud = _passthru.port1->get_usb_baud(); -//BRAD -/* - if (_passthru.baud2 != baud && baud != 0) { - _passthru.baud2 = baud; - _passthru.port2->end(); - _passthru.port2->begin_locked(baud, 0, 0, lock_key); - } */ - uint8_t parity = _passthru.port1->get_usb_parity(); - if (baud != 0) { // port1 is USB - if (_passthru.baud2 != baud) { - _passthru.baud2 = baud; - _passthru.port2->end(); - _passthru.port2->begin_locked(baud, 0, 0, lock_key); - } - if (_passthru.parity2 != parity) { - _passthru.parity2 = parity; - _passthru.port2->configure_parity(parity); - } - } -//BRADEND baud = _passthru.port2->get_usb_baud(); -//BRAD -/* - if (_passthru.baud1 != baud && baud != 0) { - _passthru.baud1 = baud; - _passthru.port1->end(); - _passthru.port1->begin_locked(baud, 0, 0, lock_key); - } */ - parity = _passthru.port2->get_usb_parity(); - if (baud != 0) { // port2 is USB - if (_passthru.baud1 != baud) { - _passthru.baud1 = baud; - _passthru.port1->end(); - _passthru.port1->begin_locked(baud, 0, 0, lock_key); - } - if (_passthru.parity1 != parity) { - _passthru.parity1 = parity; - _passthru.port1->configure_parity(parity); - } - } -//BRADEND uint8_t buf[64]; From 3f3a0026e4ed6b823c4cfcad05d62b3f7db12e67 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 3 Sep 2024 09:34:19 +0200 Subject: [PATCH 122/125] update xml --- bp_mavlink/ardupilotmega.xml | 10 +++ bp_mavlink/common.xml | 20 +++++- bp_mavlink/development.xml | 114 +++++++++++++++++++++++++++++++++++ 3 files changed, 141 insertions(+), 3 deletions(-) diff --git a/bp_mavlink/ardupilotmega.xml b/bp_mavlink/ardupilotmega.xml index c58cc0f6275aa..6501a7d0df76d 100644 --- a/bp_mavlink/ardupilotmega.xml +++ b/bp_mavlink/ardupilotmega.xml @@ -340,6 +340,16 @@ Longitude Altitude, not used. Should be sent as NaN. May be supported in a future version of this message. + + Provide a value for height above ground level. This can be used for things like fixed wing and VTOL landing. + Height above ground level. + estimated one standard deviation accuracy of the measurement. Set to NaN if not known. + Timeout for this data. The flight controller should only consider this data valid within the timeout window. + Empty + Empty + Empty + Empty + diff --git a/bp_mavlink/common.xml b/bp_mavlink/common.xml index ff44c7ea44da2..3ad86bafb0530 100644 --- a/bp_mavlink/common.xml +++ b/bp_mavlink/common.xml @@ -3331,8 +3331,20 @@ Stream is MPEG on TCP - - Stream is h.264 on MPEG TS (URI gives the port number) + + Stream is MPEG TS (URI gives the port number) + + + + Video stream encodings + + Stream encoding is unknown + + + Stream encoding is H.264 + + + Stream encoding is H.265 @@ -4437,7 +4449,7 @@ Latitude, expressed Longitude, expressed Altitude (MSL). Note that virtually all GPS modules provide both WGS84 and MSL. - Altitude above ground + Altitude above home Ground X Speed (Latitude, positive north) Ground Y Speed (Longitude, positive east) Ground Z Speed (Altitude, positive down) @@ -6037,6 +6049,8 @@ Horizontal Field of view. Stream name. Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to). + + Encoding of stream. Information about the status of a video stream. It may be requested using MAV_CMD_REQUEST_MESSAGE. diff --git a/bp_mavlink/development.xml b/bp_mavlink/development.xml index 36deb98fc77e5..6292658e43a62 100644 --- a/bp_mavlink/development.xml +++ b/bp_mavlink/development.xml @@ -35,6 +35,105 @@ Reboot components after ID change. Any non-zero value triggers the reboot. + + Set an external estimate of wind direction and speed. + This might be used to provide an initial wind estimate to the estimator (EKF) in the case where the vehicle is wind dead-reckoning, extending the time when operating without GPS before before position drift builds to an unsafe level. For this use case the command might reasonably be sent every few minutes when operating at altitude, and the value is cleared if the estimator resets itself. + + Horizontal wind speed. + Estimated 1 sigma accuracy of wind speed. Set to NaN if unknown. + Azimuth (relative to true north) from where the wind is blowing. + Estimated 1 sigma accuracy of wind direction. Set to NaN if unknown. + Empty + Empty + Empty + + + + Flags indicating errors in a GPS receiver. + + There are problems with incoming correction streams. + + + There are problems with the configuration. + + + There are problems with the software on the GPS receiver. + + + There are problems with an antenna connected to the GPS receiver. + + + There are problems handling all incoming events. + + + The GPS receiver CPU is overloaded. + + + The GPS receiver is experiencing output congestion. + + + + Signal authentication state in a GPS receiver. + + The GPS receiver does not provide GPS signal authentication info. + + + The GPS receiver is initializing signal authentication. + + + The GPS receiver encountered an error while initializing signal authentication. + + + The GPS receiver has correctly authenticated all signals. + + + GPS signal authentication is disabled on the receiver. + + + + Signal jamming state in a GPS receiver. + + The GPS receiver does not provide GPS signal jamming info. + + + The GPS receiver detected no signal jamming. + + + The GPS receiver detected and mitigated signal jamming. + + + The GPS receiver detected signal jamming. + + + + Signal spoofing state in a GPS receiver. + + The GPS receiver does not provide GPS signal spoofing info. + + + The GPS receiver detected no signal spoofing. + + + The GPS receiver detected and mitigated signal spoofing. + + + The GPS receiver detected signal spoofing but still has a fix. + + + + State of RAIM processing. + + RAIM capability is unknown. + + + RAIM is disabled. + + + RAIM integrity check was successful. + + + RAIM integrity check failed. + @@ -84,5 +183,20 @@ Channel values are in centered 13 bit format. Range is -4096 to 4096, center is 0. Conversion to PWM is x * 5/32 + 1500. Channels with indexes equal or above count should be set to 0, to benefit from MAVLink's trailing-zero trimming. + + Information about key components of GNSS receivers, like signal authentication, interference and system errors. + GNSS receiver id. Must match instance ids of other messages from same receiver. + Errors in the GPS system. + Signal authentication state of the GPS system. + Signal jamming state of the GPS system. + Signal spoofing state of the GPS system. + The state of the RAIM processing. + Horizontal expected accuracy using satellites successfully validated using RAIM. + Vertical expected accuracy using satellites successfully validated using RAIM. + An abstract value representing the estimated quality of incoming corrections, or 255 if not available. + An abstract value representing the overall status of the receiver, or 255 if not available. + An abstract value representing the quality of incoming GNSS signals, or 255 if not available. + An abstract value representing the estimated PPK quality, or 255 if not available. + From 86ee621fbd0ac809b68c2daba4799e6c72580cdd Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 3 Sep 2024 09:34:34 +0200 Subject: [PATCH 123/125] cygwin fix --- libraries/AP_Common/c++.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Common/c++.cpp b/libraries/AP_Common/c++.cpp index eab43682fb2aa..86019c12bb27a 100644 --- a/libraries/AP_Common/c++.cpp +++ b/libraries/AP_Common/c++.cpp @@ -84,7 +84,10 @@ void operator delete[](void * ptr) if (ptr) free(ptr); } -#ifdef CYGWIN_BUILD +//OW +//#ifdef CYGWIN_BUILD +#if defined(CYGWIN_BUILD) && CONFIG_HAL_BOARD == HAL_BOARD_SITL +//OWEND /* wrapper around malloc to ensure all memory is initialised as zero cygwin needs to wrap _malloc_r From 4a6c6671fad6eeeb1d669961e8584cedbc8f4653 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Tue, 3 Sep 2024 09:50:32 +0200 Subject: [PATCH 124/125] update bp_version, remove serial control --- libraries/GCS_MAVLink/GCS_serial_control.cpp | 16 ---------------- libraries/bp_version.h | 8 ++++---- 2 files changed, 4 insertions(+), 20 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_serial_control.cpp b/libraries/GCS_MAVLink/GCS_serial_control.cpp index a3f9e9b9e91ec..e0a2b3fa09366 100644 --- a/libraries/GCS_MAVLink/GCS_serial_control.cpp +++ b/libraries/GCS_MAVLink/GCS_serial_control.cpp @@ -114,22 +114,6 @@ void GCS_MAVLINK::handle_serial_control(const mavlink_message_t &msg) port->begin(packet.baudrate); } -//OW SERIALCONTROL - #define SERIAL_CONTROL_FLAG_8N1 0x40 - #define SERIAL_CONTROL_FLAG_8E1 0x80 - - if (exclusive && port != nullptr) { - if (packet.flags & SERIAL_CONTROL_FLAG_8N1) { - port->configure_parity(0); - port->set_stop_bits(1); - } - if (packet.flags & SERIAL_CONTROL_FLAG_8E1) { - port->configure_parity(2); // enable even parity - port->set_stop_bits(1); - } - } -//OWEND - // write the data if (packet.count != 0) { if ((packet.flags & SERIAL_CONTROL_FLAG_BLOCKING) == 0) { diff --git a/libraries/bp_version.h b/libraries/bp_version.h index e6b07d841550b..aa13fa7047c57 100644 --- a/libraries/bp_version.h +++ b/libraries/bp_version.h @@ -1,19 +1,19 @@ #pragma once -#define BETAPILOTVERSION "v060h" -#define DATEOFBASEBRANCH "20240525" +#define BETAPILOTVERSION "v060i" +#define DATEOFBASEBRANCH "20240903" /* search for //OW to find all changes THR_MINSPD THR_SLEW SUPP_MAN waiting for rudder release on-top features: - RADIO_LINK (OW RADIOLINK) -- GCS_serial_control: SERIAL_CONTROL 8E1 (OW SERIALCONTROL) - RC_Channel, AUX_FUNC: CAMERA_SET_MODE, CAMERA_TRIG_MODE (eq CAM_MODE_TOGGLE) (OW CAMERA) - Plane THR (OW THR_SUPP) remove zflags, not used, could use now options -2024.05.25: v060 + remove GCS_serial_control: SERIAL_CONTROL 8E1 +2024.09.03: v060 upgraded to ArduPilot master 4.6.0-dev 2023.11.06: v059 upgraded to ArduPilot master 4.5.0-dev, mount revised significantly From e6d811a5496bda3bd5ff71a28fbf10f69c1cc4c9 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Wed, 4 Sep 2024 04:38:56 +0200 Subject: [PATCH 125/125] tidy --- ArduPlane/servos.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 89a2bce2b737c..835eb6a7edf85 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -615,9 +615,9 @@ void Plane::set_throttle(void) if (suppress_throttle()) { //OW THR_SUPP - if (control_mode == &mode_takeoff && g.takeoff_throttle_suppress) { - // in takeoff mode set throttle to specified value - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_float(g.takeoff_throttle_suppress, 0.0f, 100.0f)); + if (control_mode == &mode_takeoff && g.takeoff_throttle_suppress) { + // in takeoff mode set throttle to specified value + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_float(g.takeoff_throttle_suppress, 0.0f, 100.0f)); } else //OWEND