diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 31c1148db9ea8..c9ec2050c746f 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -276,10 +276,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_NAV_CONTROLLER_OUTPUT, +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif }; @@ -315,7 +321,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_BATTERY_STATUS, }; static const ap_message STREAM_PARAMS_msgs[] = { - MSG_NEXT_PARAM + MSG_NEXT_PARAM, + MSG_AVAILABLE_MODES }; const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { @@ -643,3 +650,43 @@ void GCS_MAVLINK_Tracker::send_global_position_int() 0, // Z speed cm/s (+ve Down) tracker.ahrs.yaw_sensor); // compass heading in 1/100 degree } + +// Send the mode with the given index (not mode number!) return the total number of modes +// Index starts at 1 +uint8_t GCS_MAVLINK_Tracker::send_available_mode(uint8_t index) const +{ + const Mode* modes[] { + &tracker.mode_manual, + &tracker.mode_stop, + &tracker.mode_scan, + &tracker.mode_guided, + &tracker.mode_servotest, + &tracker.mode_auto, + &tracker.mode_initialising, + }; + + const uint8_t mode_count = ARRAY_SIZE(modes); + + // Convert to zero indexed + const uint8_t index_zero = index - 1; + if (index_zero >= mode_count) { + // Mode does not exist!? + return mode_count; + } + + // Ask the mode for its name and number + const char* name = modes[index_zero]->name(); + const uint8_t mode_number = (uint8_t)modes[index_zero]->number(); + + mavlink_msg_available_modes_send( + chan, + mode_count, + index, + MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD, + mode_number, + 0, // MAV_MODE_PROPERTY bitmask + name + ); + + return mode_count; +} diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h index a431f5217e230..af98ee5a39fd7 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -30,6 +30,10 @@ class GCS_MAVLINK_Tracker : public GCS_MAVLINK void send_nav_controller_output() const override; void send_pid_tuning() override; + // Send the mode with the given index (not mode number!) return the total number of modes + // Index starts at 1 + uint8_t send_available_mode(uint8_t index) const override; + private: void packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) override; diff --git a/AntennaTracker/ReleaseNotes.txt b/AntennaTracker/ReleaseNotes.txt index 972964d76c3e7..c03c34c3ab4d8 100644 --- a/AntennaTracker/ReleaseNotes.txt +++ b/AntennaTracker/ReleaseNotes.txt @@ -1,5 +1,429 @@ Antenna Tracker Release Notes: ------------------------------------------------------------------ +Release 4.6.0-beta 13 Nov 2024 + +Changes from 4.5.7 + +1) Board specific changes + +- AnyLeaf H7 supports compass and onboard logging +- Blitz743Pro supports CAN +- BlueRobotics Navigator supports BMP390 baro +- Bootloader ECC failure check fixed on boards with >128k bootloader space (e.g CubeRed) +- CB Unmanned Stamp H743 support +- ClearSky CSKY405 support +- CUAV-7-Nano default batt monitor fixed +- CubeRed bootloader fixes including disabling 2nd core by default +- CubeRed supports PPP networking between primary and secondary MCU +- CubeRedPrimary supports external compasses +- ESP32 main loop rate improvements +- ESP32 RC input fixes and wifi connection reliability improved +- ESP32 safety switch and GPIO pin support +- FlyingMoon no longer support MAX7456 +- Flywoo F405HD-AIOv2 ELRS RX pin pulled high during boot +- Flywoo H743 Pro support +- Flywoo/Goku F405 HD 1-2S ELRS AIO v2 +- FlywooF745 supports DPS310 baro +- FPV boards lose SMBus battery support (to save flash) +- GEPRC F745BTHD support +- GEPRCF745BTHD loses parachute support, non-BMP280 baros (to save flash) +- Here4FC bootloader fix for mismatch between RAM0 and periph that could prevent firmware updates +- Holybro Kakute F4 Wing support +- iFlight 2RAW H743 supports onboard logging +- JFB110 supports measuring servo rail voltage +- JFB110 supports safety switch LED +- JHEM-JHEF405 bootloader supports firmware updates via serial +- JHEMCU GF30H743 HD support +- JHEMCU-GF16-F405 autopilot support +- JHEMCU-GSF405A becomes FPV board (to save flash) +- KakuteF7 only supports BMP280 baro (to save flash) +- KakuteH7Mini supports ICM42688 IMU +- Linux auto detection of GPS baud rate fixed +- Linux board scheduler jitter reduced +- Linux board shutdown fixes +- MakeFlyEasy PixPilot-V6Pro support +- MatekF405, Pixhawk1-1M-bdshot, revo-mini loses blended GPS (to save flash) +- MatekH7A3 support Bi-directional DShot +- MicoAir405v2 and MicoAir405Mini support optical flow and OSD +- MicoAir743 internal compass orientation fixed +- MicroAir405Mini, MicroAir743, NxtPX4v2 support +- MicroAir405v2 Bi-directional DShot and LED DMA fixes +- MicroAir405v2 defined as FPV board with reduced features (to save flash) +- ModalAI VOXL2 support including Starling 2 and Starling 2 max +- mRo Control Zero Classic supports servo rail analog input +- mRo KitCAN revC fixed +- Mugin MUPilot support +- OmnibusF7v2 loses quadplane support (to save flash) +- Pixhack-v3 board added (same as fmuv3) +- Pixhawk6C bootloader supports flashing firmware from SD card +- RadiolinkPIX6 imu orientation fixed +- RadiolinkPIX6 supports SPA06 baro +- ReaperF745 V4 FC supports MPU6000 IMU +- RPI5 support +- SDModelH7v2 SERIAL3/7/9_PROTOCOL param defaults changed +- Solo serial ports default to MAVLink1 +- SpeedyBeeF405Wing gets Bi-directional DShot +- SpeedyBeeF405WING loses landing gear support, some camera gimbals (to save flash) +- Spektreworks boom board support +- TrueNavPro-G4 SPI does not share DMA +- X-MAV AP-H743v2 support + +2) AHRS/EKF enhancements and fixes + +- AHRS_OPTION to disable fallback to DCM (affects Plane and Rover, Copter never falls back) +- AHRS_OPTION to disable innovation check for airspeed sensor +- Airspeed sensor health check fixed when using multiple sensors and AHRS affinity +- DCM support for MAV_CMD_EXTERNAL_WIND_ESTIMATE (Plane only) +- EK2 supports disabling external nav (see EK2_OPTIONS) +- EK3 External Nav position jump after switch from Optical flow removed (see EK3_SRC_OPTION=1) +- EK3 uses filtered velocity corrections for IMU position +- EKF2, EKF3, ExternalAHRS all use common origin +- EKF3 accepts set origin even when using GPS +- EKF3 allows earth-frame fields to be estimated with an origin but no GPS +- EKF3 copes better with GPS jamming +- EKF3 logs mag fusion selection to XKFS +- EKF3 wind estimation when using GPS-for-yaw fixed +- External AHRS improvements including handling variances, pre-arm origin check +- Inertial Labs External AHRS fixes +- VectorNav driver fix for handling of error from sensor +- VectorNav External AHRS enhancements including validation of config commands and logging improvements +- VectorNav support for sensors outside VN-100 and VN-300 + +3) Driver enhancements and bug fixes + +- ADSB fix to display last character in status text sent to GCS +- Ainstein LR-D1 radar support +- Airspeed ASP5033 whoami check fixed when autopilot rebooted independently of the sensor +- AIRSPEED message sent to GCS +- Analog temperature sensor extended to 5th order polynomial (see TEMP_A5) +- ARSPD_OPTIONS to report calibration offset to GCS +- Baro EAS2TAS conversions continuously calculated reducing shocks to TECS (Plane only) +- Baro provides improved atmospheric model for high altitude flight +- BARO_ALT_OFFSET slew slowed to keep EKF happy +- BATTx_ESC_MASK param supports flexible mapping of ESCs to batteries +- BATTx_OPTION to not send battery voltage, current, etc to GCS +- Benewake RDS02U radar support +- Bi-directional DShot on IOMCU supports reversible mask +- Bi-directional DShot telemetry support on F103 8Mhz IOMCUs +- BMM350 compass support +- CAN rangefinders and proximity sensors may share a CAN bus (allows NRA24 and MR72 on a single CAN bus) +- Compass calibration world magnetic model checks can use any position source (e.g. not just GPS) +- CRSF baro and vertical speeed fixed +- CRSF RX bind command support +- DroneCAN battery monitor check to avoid memory corruption when type changed +- DroneCAN DNA server fixes including removing use of invalid node IDs, faster ID allocation, elimination of rare inability to save info +- DroneCAN EFI health check fix +- DroneCAN ESC battery monitors calculate consumed mah +- DroneCAN ESCs forced to zero when disarmed +- DroneCAN RPM message support +- DroneCAN timeout fix for auxiliary frames +- DroneCAN to serial tunneling params accepts short-hand baud rates (e.g. '57' for '57600') +- F9P, F10-N and Zed-F9P support for GPSx_GNSS_MODE to turn on/off SBAS, Galileo, Beidou and Glonass +- FuelLevel battery monitor fix to report capacity +- GPS_xxx params renamed to GPS1_xxx, GPS_xxx2 renamed to GPS2_xxx +- Hirth EFI logging includes modified throttle +- Hirth ICEngine supports reversed crank direction (see ICE_OPTIONS parameter) +- Hott and LTM telemetry deprecated (still available through custom build server) +- i-BUS telemetry support +- ICE_PWM_IGN_ON, ICE_PWM_IGN_OFF, ICE_PWM_STRT_ON, ICE_PWM_STRT_OFF replaced with SERVOx_MIN/MAX/REVERSED (Plane only) +- ICE_START_CHAN replaced with RC aux function (Plane only) +- ICEngine retry max added (see ICE_STRT_MX_RTRY) +- IE 2400 generator error message severity to GCS improved +- INA2xx battery monitor support (reads temp, gets MAX_AMPS and SHUNT params) +- MCP9600 temperature sensor I2C address fixed +- MLX90614 temperature sensor support +- MSP GPS ground course scaling fixed +- MSP RC uses scaled RC inputs (fixes issue with RCx_REVERSED having no effect) +- Networking supports reconnection to TCP server or client +- OSD params for adjusting horizontal spacing and vertical extension (see OSD_SB_H_OFS, OSD_SB_V_EXT) +- Relay inverted output support (see RELAYx_INVERTED parameter) +- ROMFS efficiency improvements +- RS-485 driver enable RTS flow control +- Sagetech MXS ADSP altitude fix (now uses abs alt instead of terrain alt) +- Septentrio GPS sat count correctly drops to zero when 255 received +- Septentrio supports selecting constellations (see GPS_GNSS_MODE) +- Single LED for user notification supported +- SPA06 baro supported +- Sum battery monitor optionally reports minimum voltage instead of average +- Sum battery monitor reports average temp +- Torqeedo dual motor support (see TRQ1, TRQ2 params) +- Ublox GPS driver uses 64 bit time for PPS interrupt (avoids possible dropout at 1hour and 12 minutes) +- UBlox GPS time ignored until at least 2D fix +- VideoTX supports additional freq bands (RushFPV 3.3Ghz) +- Volz logs desired and actual position, voltage, current, motor and PCB temp +- Volz server feedback and logging fixed +- Volz servo output in its own thread resulting in smoother movements +- W25N02KV flash support + +4) Networking enhancements and fixes + +- Allow multiple UDP clients to connect/disconnect/reconnect +- Ethernet supports faster log downloading (raised SDMMC clock limit on H7) + +5) Camera and gimbal enhancements + +- Alexmos precision improved slightly +- CAMERA_CAPTURE_STATUS mavlink msg sent to GCS (reports when images taken or video recorded, used by QGC) +- CAMERA_FOV_STATUS mavlink reports lat,lon of what camera is pointing at +- DO_MOUNT_CONTROL yaw angle interpreted as body-frame (was incorrectly being interpreted as earth-frame) +- Dual serial camera gimbal mounts fixed +- Lua script bindings to send CAMERA_INFORMATION and VIDEO_STREAM_INFORMATION messages to GCS +- Retract Mount2 aux function added (see RCx_OPTION = 113) +- Servo gimbal reported angles respect roll, pitch and yaw limits +- Siyi driver sends autopilot location and speed (recorded in images via EXIF) +- Siyi picture and video download scripts +- Siyi ZT6 and ZT30 support sending min, max temperature (see CAMERA_THERMAL_RANGE msg) +- Siyi ZT6 and ZT30 thermal palette can be changed using camera-change-setting.lua script +- Siyi ZT6 hardware id and set-lens fixed +- Topotek gimbal support +- Trigger distance ignores GPS status and only uses EKF reported location + +6) Harmonic notch enhancements + +- Harmonic notch is active in forward flight on quadplanes +- Harmonic notch filter freq clamping and disabling reworked +- Harmonic notch handles negative ESC RPMs +- Harmonic notch supports per-motor throttle-based harmonic notch + +7) Copter specific enhancements and bug fixes + +- Attitude control fix to dt update order (reduces rate controller noise) +- Auto mode fix to avoid prematurely advancing to next waypoint if given enough time +- Auto mode small target position jump when takeoff completes removed +- Auto mode yaw drift when passing WP removed if CONDITION_YAW command used and WP_YAW_BEHAVIOR = 0/None +- Auto, Guided flight mode pause RC aux switch (see RCx_OPTION = 178) +- AutoRTL (e.g. DO_LAND_START) uses copter stopping point to decide nearest mission command +- AutoRTL mode supports DO_RETURN_PATH_START (Copter, Heli only) +- AutoTune fix to prevent spool up after landing +- AutoTune performance and safety enhancements (less chance of ESC desync, fails when vehicle likely can't be tuned well) +- Autotune test gains RC aux switch function allows testing gains in any mode (see RCx_OPTION = 180) +- Config error avoided if auto mode is paused very soon after poweron +- FLIGHT_OPTIONS bit added to require position estimate before arming +- Follow mode slowdown calcs fixed when target is moving +- Ground oscillation suppressed by reducing gains (see ATC_LAND_R/P/Y_MULT) +- Guided mode internal error fix when taking off using SET_ATTITUDE_CONTROL message +- Guided mode internal error resolved when switching between thrust or climb rate based altitude control +- Guided mode yaw fixed when WP_YAW_BEHAVIOR = 0/None and CONDITION_YAW command received containing relative angle +- Landing detector fixed when in stabilize mode at full throttle but aircraft is upside down +- Landing detector logging added to ease support (see LDET message) +- Loiter unlimited command accepts NaNs (QGC sends these) +- Mavlink SYSTEM_STATUS set to BOOT during initialisation +- MOT_PWM_TYPE of 9 (PWMAngle) respects SERVOx_MIN/MAX/TRIM/REVERSED param values +- Payload place bug fix when aborted because gripper is already released +- RC based tuning (aka CH6 tuning) can use any RC aux function channel (see RCx_OPTION = 219) +- RTL_ALT minimum reduced to 30cm +- SystemID position controller support (Copter and Heli) +- TriCopter motor test and slew-up fixed +- WPNAV_SPEED min reduced to 10 cm/s (Copter only) +- Loiter mode zeros desired accel when re-entering from Auto during RC failsafe + +8) TradHeli specific enhancements + +- Autorotation yaw behaviour fix +- Autotune improvements including using low frequency dwell for feedforward gain tuning and conducting sweep in attitude instead of rate +- Blade pitch angle logging added (see SWSH log message) +- Constrain cyclic roll for intermeshing +- ICE / turbine cool down fix +- Inverted flight extended to non manual throttle modes +- Inverted flight transitions smoothed and restricted to only Stabilize mode +- SWSH logging fix for reversed collectives + +9) Plane specific enhancements and bug fixes + +- AIRSPEED_STALL holds vehicle stall speed and is used for minimum speed scaling +- Allow for different orientations of landing rangefinder +- Assistance requirements evaluted on mode change +- FBWB/CRUISE climb/sink rate limited by TECS limits +- FLIGHT_OPTION to immediately climb in AUTO mode (not doing glide slope) +- Glider pullup support (available only through custom build server) +- Loiter breakout improved to better handle destinations inside loiter circle +- Manual mode throttle made consistent with other modes (e.g battery comp and watt limit is done if enabled) +- Mavlink GUIDED_CHANGE_ALTITUDE supports terrain altitudes +- Minimum throttle not applied during SLT VTOL airbrake (reduces increase in airspeed and alt during back transition) +- Q_APPROACH_DIST set minimum distance to use the fixed wing approach logic +- Quadplane assistance check enhancements +- Quadplane Deca frame support +- Quadplane gets smoother takeoff by input shaping target accel and velocity +- Servo wiggles in altitude wait staged to be one after another +- Set_position_target_global_int accepts MAV_FRAME_GLOBAL_RELATIVE_ALT and MAV_FRAME_GLOBAL_TERRAIN_ALT frames +- Takeoff airspeed control improved (see TKOFF_MODE, TKOFF_THR_MIN) +- Takeoff fixes for fence autoenable +- Takeoff improvements including less overshoot of TKOFF_ALT +- TECS reset along with other controllers (important if plane dropped from balloon) +- Tilt quadplane ramp of motors on back transition fixed +- Tiltrotor tilt angles logged +- TKOFF_THR_MIN applied to SLT transitions +- Twin motor planes with DroneCAN ESCs fix to avoid max throttle at arming due to misconfiguration +- VTOLs switch to QLAND if a LONG_FAILSAFE is triggered during takeoff + +10) Rover specific enhancements and bug fixes + +- Auto mode reversed state maintained if momentarily switched to Hold mode +- Circle mode tracks better and avoids getting stuck at circle edge +- Flight time stats fixed +- MAV_CMD_NAV_SET_YAW_SPEED deprecated +- Omni3Mecanum frame support +- Stopping point uses max deceleration (was incorrectly using acceleration) +- Wheel rate controller slew rate fix + +11) Antenna Tracker specific enhancements and bug fixes + +- Never track lat,lon of 0,0 + +12) Scripting enhancements + +- advance-wp.lua applet supports advancing Auto mode WP via RC switch +- AHRS_switch.lua supports switching between EKF2 and EKF3 via RC switch +- battery_internal_resistance_check.lua monitors battery resistance +- CAN:get_device returns nil for unconfigured CAN device +- copter_terrain_brake.lua script added to prevent impact with terrain in Loiter mode (Copter only) +- Copter:get_target_location, update_target_location support +- crosstrack_restore.lua example allows returning to previous track in Auto (Plane only) +- Display text on OLED display supported +- Docs improved for many bindings +- EFI get_last_update_ms binding +- EFI_SkyPower.lua driver accepts 2nd supply voltage +- ESC_slew_rate.lua example script supports testing ESCs +- Filesystem CRC32 check to allow scripts to check module versions +- forced arming support +- GPIO get/set mode bindings (see gpio:set_mode, get_mode) +- GPS-for-yaw angle binding (see gps:gps_yaw_deg) +- Halo6000 EFI driver can log all CAN packets for easier debugging +- handle_external_position_estimate binding allows sending position estimate from lua to EKF +- I2C:transfer support +- IMU gyros_consistent and accels_consistent bindings added +- INF_Inject.lua driver more robust to serial errors, improved logging, throttle and ignition control +- INS bindings for is calibrating, gyro and accel sensor values +- IPV4 address bindings (see SocketAPM_ipv4_addr_to_string) to allow UDP server that responds to individual clients +- Logging of booleans supported +- Lua language checks improved (finds more errors) +- MAVLink commands can be called from scripting +- MCU voltage binding (see analog:mcu_voltage) +- NMEA 2000 EFI driver (see EFI_NMEA2k.lua) +- "open directory failed" false warning when scripts in ROMFS fixed +- Param_Controller.lua supports quickly switching between 3 parameter sets via RC switch +- Pass by reference values are always initialized +- pelco_d_antennatracker.lua applet supports sending Pelco-D via RS-485 to PTZ cameras +- plane_aerobatics.lua minor enhancements +- REPL applet (read-evaluate-print-loop, see repl.lua) for interactive testing and experimentation +- "require" function failures in rare circumstances fixed +- "require" function works for modules in ROMFS (e.g. not on SD card) +- revert_param.lua supports more params (e.g ATC_RATE_R/P/Y, PTCH2SRV_TCONST, RLL2SRV_TCONST, TECS_) +- Scripts may receive mavlink messages which fail CRC (e.g messages which FC does not understand) +- SD card formatting supported +- Serial device simulation support (allows Lua to feed data to any supported serial protocol for e.g. sensor simulation) +- set_target_throttle_rate_rpy allows rate control from scripts (new for Copter) +- sitl_standby_sim.lua example shows how to switch between redundant flight controllers using an RC switch +- Slung payload oscillation suppression applet added (see copter-slung-payload.lua) +- Temperature sensor bindings added +- uint64 support +- Various performance and memory usage optimizations +- VTOL-quicktune.lua minor enhancements including increasing YAW_FLTE_MAX to 8 +- x-quad-cg-allocation.lua applet corrects for the CoM discrepancy in a quad-X frame + +13) GCS / mavlink related changes and fixes + +- BATTERY2 message deprecated (replaced by BATTERY_STATUS) +- CMD_MISSION_START/STOP rejected if first-item/last-item args provided +- Deny attempts to upload two missions simultaneously +- Fence and Rally points may be uploaded using FTP +- GPS_INPUT and HIL_GPS handles multiple GPSs +- HIGHRES_IMU mavlink message support (used by companion computers to receive IMU data from FC) +- MAV_CMD_COMPONENT_ARM_DISARM accepts force arm magic value of 21196 +- MAV_CMD_DO_SET_SAFETY_SWITCH_STATE support +- MAV_CMD_SET_HAGL support (Plane only) +- MAVFTP respects TX buffer flow control to improve FTP on low bandwidth links +- MAVLink receiver support (RADIO_RC_CHANNELS mavlink message) +- Message interval supports TERRAIN_REPORT msg +- Mission upload may be cancelled using MISSION_CLEAR_ALL +- MOUNT_CONFIGURE, MOUNT_CONTROL messages deprecated +- RC_CHANNELS_RAW deprecated +- Serial passthrough supports parity allowing STM32CubeProgrammer to be used to program FrSky R9 receivers +- SET_ATTITUDE_TARGET angular rate input frame fixed (Copter only) +- TIMESYNC and NAMED_VALUE_FLOAT messages not sent on high latency MAVLink ports + +14) Logging enhancements and fixes + +- AC_PID resets and I-term sets logged +- ANG provides attitude at high rate (equivalent to ATT) +- ATT logs angles as floats (better resolution than ints) +- CAND message gets driver index +- DCM log message includes roll/pitch and yaw error +- EDT2 log msg includes stress and status received via extended DShot Telemetry v2 +- EFI ECYL cylinder head and exhaust temp logs in degC (was Kelvin) +- ESCX log msg includes DroneCAN ESCs status, temp, duty cycle and power pct +- FMT messages written as required instead of all at beginning +- Logging restarted after download completes when LOG_DISARMED = 1 +- MISE msg logs active mission command (CMD logged when commands are uploaded) +- ORGN message logging fixed when set using SET_GPS_GLOBAL_ORIGIN +- RPM sensor logging gets instance field, quality and health fields +- Short filename support removed (e.g log1.BIN instead of 00000001.BIN) +- Temperature sensor logging option for only sensors with no source (see TEMP_LOG) +- UART data rates logged at 1hz (see UART message) + +15) ROS2 / DDS support + +- Airspeed published +- Battery topic reports all available batteries +- Compile-time configurable rates for each publisher +- DDS_TIMEOUT_MS and DDS_MAX_RETRY set timeout and num retries when client pings XRCE agent +- GPS global origin published at 1 Hz +- High frequency raw imu data transmission +- Joystick support +- Support sending waypoints to Copter and Rover +- Remove the XML refs file in favor of binary entity creation + +16) Safety related enhancements and fixes + +- Accel/Gyro inconsistent message fixed for using with only single IMU +- Battery monitor failure reported to GCS, triggers battery failsafe but does not take action +- Far from EKF origin pre-arm check removed (Copter only) +- Fence breach warning message slightly improved +- Fence enhancements incluiding alt min fence (Copter only, see FENCE_AUTOENABLE, FENCE_OPTIONS) +- Fences can be stored to SD Card (see BRD_SD_FENCE param) +- ICEngine stopped when in E-Stop or safety engaged (Plane only) +- LEDs flash green lights based on EKF location not GPS +- Parachute option to skip disarm before parachute release (see CHUTE_OPTIONS) +- Plane FENCE_AUTOENABLE of 1 or 2 deprecation warning added +- Pre-arm check if OpenDroneID is compiled in but disabled +- Pre-arm check of duplicate RC aux functions fixed (was skipping recently added aux functions) +- Pre-arm checks alert user more quickly on failure +- Prearm check for misconfigured EAHRS_SENSORS and GPS_TYPE +- Proximity sensor based avoidance keeps working even if one proximity sensor fails (Copter, Rover) +- RC aux functions for Arm, Disarm, E-Stop and Parachute ignored when RC is first turned on +- Warning of duplicate SERIALx_PROTOCOL = RCIN + +17) Other bug fixes and minor enhancements + +- Accel cal fixed for auxiliary IMUs (e.g. IMU4 and higher) +- Bootloader fix to reduce unnecessary mass erasing of flash when using STLink or Jlink tools +- Bootloader rejects allocation of broadcast node ID +- CAN forward registering/de-registering fix (affected Mission Planner DroneCAN UI) +- Dijkstras fix to correct use of uninitialised variable +- DShot rates are not limited by NeoPixel rates +- Ethernet and CAN bootloader fix to prevent getting stuck in bootloader mode +- Filesystem does not show entries for empty @ files +- Filesystem efficiency improvements when reading files +- Flight statistics only reset if user sets STAT_RESET to zero (avoids accidental reset) +- Flight time statistics updated on disarm (avoids issue if vehicle powered-down soon after disarm) +- Internal error thrown if we lose parameters due to saving queue being too small +- MAVLink via DroneCAN baud rate fix +- SPI pins may also be used as GPIOs +- Terrain cache size configurable (see TERRAIN_CACHE_SZ) + +18) Developer focused fixes and enhancements + +- AP_Camera gets example python code showing how to use GStreamer with UDP and RTSP video streams +- Cygwin build fix for non-SITL builds +- Cygwin build fixed with malloc wrap +- DroneCAN and scripting support FlexDebug messages (see CAN_Dn_UC_OPTION, FlexDebug.lua) +- EKF3 code generator documentation and cleanup +- GPS jamming simulator added +- MacOS compiler warnings reduced +- SFML joystick support +- SITL support for OpenBSD +- Text warning if older Fence or Rally point protocols are used +------------------------------------------------------------------ Release 4.5.7 08 Oct 2024 Changes from 4.5.7-beta1 diff --git a/AntennaTracker/Tracker.cpp b/AntennaTracker/Tracker.cpp index 435134d6189e1..a35a6f3251ad5 100644 --- a/AntennaTracker/Tracker.cpp +++ b/AntennaTracker/Tracker.cpp @@ -82,7 +82,7 @@ void Tracker::one_second_loop() mavlink_system.sysid = g.sysid_this_mav; // update assigned functions and enable auxiliary servos - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); // updated armed/disarmed status LEDs update_armed_disarmed(); diff --git a/AntennaTracker/mode.h b/AntennaTracker/mode.h index d6a10022dc8c5..bfdcad9ac7860 100644 --- a/AntennaTracker/mode.h +++ b/AntennaTracker/mode.h @@ -22,6 +22,7 @@ class Mode { // returns a unique number specific to this mode virtual Mode::Number number() const = 0; + virtual const char* name() const = 0; virtual bool requires_armed_servos() const = 0; @@ -41,6 +42,7 @@ class Mode { class ModeAuto : public Mode { public: Mode::Number number() const override { return Mode::Number::AUTO; } + const char* name() const override { return "Auto"; } bool requires_armed_servos() const override { return true; } void update() override; }; @@ -48,6 +50,7 @@ class ModeAuto : public Mode { class ModeGuided : public Mode { public: Mode::Number number() const override { return Mode::Number::GUIDED; } + const char* name() const override { return "Guided"; } bool requires_armed_servos() const override { return true; } void update() override; @@ -66,6 +69,7 @@ class ModeGuided : public Mode { class ModeInitialising : public Mode { public: Mode::Number number() const override { return Mode::Number::INITIALISING; } + const char* name() const override { return "Initialising"; } bool requires_armed_servos() const override { return false; } void update() override {}; }; @@ -73,6 +77,7 @@ class ModeInitialising : public Mode { class ModeManual : public Mode { public: Mode::Number number() const override { return Mode::Number::MANUAL; } + const char* name() const override { return "Manual"; } bool requires_armed_servos() const override { return true; } void update() override; }; @@ -80,6 +85,7 @@ class ModeManual : public Mode { class ModeScan : public Mode { public: Mode::Number number() const override { return Mode::Number::SCAN; } + const char* name() const override { return "Scan"; } bool requires_armed_servos() const override { return true; } void update() override; }; @@ -87,6 +93,7 @@ class ModeScan : public Mode { class ModeServoTest : public Mode { public: Mode::Number number() const override { return Mode::Number::SERVOTEST; } + const char* name() const override { return "ServoTest"; } bool requires_armed_servos() const override { return true; } void update() override {}; @@ -96,6 +103,7 @@ class ModeServoTest : public Mode { class ModeStop : public Mode { public: Mode::Number number() const override { return Mode::Number::STOP; } + const char* name() const override { return "Stop"; } bool requires_armed_servos() const override { return false; } void update() override {}; }; diff --git a/AntennaTracker/servos.cpp b/AntennaTracker/servos.cpp index 23e1e0cab5bfd..5ca8bc0b54656 100644 --- a/AntennaTracker/servos.cpp +++ b/AntennaTracker/servos.cpp @@ -8,7 +8,7 @@ void Tracker::init_servos() { // update assigned functions and enable auxiliary servos - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); SRV_Channels::set_default_function(CH_YAW, SRV_Channel::k_tracker_yaw); SRV_Channels::set_default_function(CH_PITCH, SRV_Channel::k_tracker_pitch); diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 5b86ade1f5069..7f1ecabb7f14c 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -27,7 +27,7 @@ // features below are disabled by default on all boards //#define CAL_ALWAYS_REBOOT // flight controller will reboot after compass or accelerometer calibration completes //#define DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE // disable mode changes from GCS during Radio failsafes. Avoids a race condition for vehicle like Solo in which the RC and telemetry travel along the same link -//#define ADVANCED_FAILSAFE 1 // enabled advanced failsafe which allows running a portion of the mission in failsafe events +//#define AP_COPTER_ADVANCED_FAILSAFE_ENABLED 1 // enabled advanced failsafe which allows running a portion of the mission in failsafe events // other settings //#define THROTTLE_IN_DEADBAND 100 // redefine size of throttle deadband in pwm (0 ~ 1000) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index e33abf569df02..6ef06d421641a 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -1,7 +1,7 @@ #include "Copter.h" #pragma GCC diagnostic push -#if defined(__clang__) +#if defined(__clang_major__) && __clang_major__ >= 14 #pragma GCC diagnostic ignored "-Wbitwise-instead-of-logical" #endif diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 675b3b07eab03..84e31f3ebb326 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -232,7 +232,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if HAL_ADSB_ENABLED SCHED_TASK(avoidance_adsb_update, 10, 100, 138), #endif -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED SCHED_TASK(afs_fs_check, 10, 100, 141), #endif #if AP_TERRAIN_AVAILABLE @@ -727,7 +727,7 @@ void Copter::one_hz_loop() } // update assigned functions and enable auxiliary servos - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); #if HAL_LOGGING_ENABLED // log terrain data diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 60c0818e6339d..48b6141c9a84d 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -137,7 +137,7 @@ #include #endif -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED # include "afs_copter.h" #endif #if TOY_MODE_ENABLED @@ -183,7 +183,7 @@ class Copter : public AP_Vehicle { friend class ParametersG2; friend class AP_Avoidance_Copter; -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED friend class AP_AdvancedFailsafe_Copter; #endif friend class AP_Arming_Copter; @@ -804,7 +804,7 @@ class Copter : public AP_Vehicle { // failsafe.cpp void failsafe_enable(); void failsafe_disable(); -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED void afs_fs_check(void); #endif diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 1e4e6da5984a1..b87375320753a 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -505,10 +505,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, // MISSION_CURRENT +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif MSG_NAV_CONTROLLER_OUTPUT, @@ -583,7 +589,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { #endif }; static const ap_message STREAM_PARAMS_msgs[] = { - MSG_NEXT_PARAM + MSG_NEXT_PARAM, + MSG_AVAILABLE_MODES }; static const ap_message STREAM_ADSB_msgs[] = { MSG_ADSB_VEHICLE, @@ -1519,7 +1526,7 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg) } MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_int_t &packet) { -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED if (GCS_MAVLINK::handle_flight_termination(packet) == MAV_RESULT_ACCEPTED) { return MAV_RESULT_ACCEPTED; } @@ -1657,3 +1664,120 @@ uint8_t GCS_MAVLINK_Copter::high_latency_wind_direction() const return 0; } #endif // HAL_HIGH_LATENCY2_ENABLED + +// Send the mode with the given index (not mode number!) return the total number of modes +// Index starts at 1 +uint8_t GCS_MAVLINK_Copter::send_available_mode(uint8_t index) const +{ + const Mode* modes[] { +#if MODE_AUTO_ENABLED + &copter.mode_auto, // This auto is actually auto RTL! + &copter.mode_auto, // This one is really is auto! +#endif +#if MODE_ACRO_ENABLED + &copter.mode_acro, +#endif + &copter.mode_stabilize, + &copter.mode_althold, +#if MODE_CIRCLE_ENABLED + &copter.mode_circle, +#endif +#if MODE_LOITER_ENABLED + &copter.mode_loiter, +#endif +#if MODE_GUIDED_ENABLED + &copter.mode_guided, +#endif + &copter.mode_land, +#if MODE_RTL_ENABLED + &copter.mode_rtl, +#endif +#if MODE_DRIFT_ENABLED + &copter.mode_drift, +#endif +#if MODE_SPORT_ENABLED + &copter.mode_sport, +#endif +#if MODE_FLIP_ENABLED + &copter.mode_flip, +#endif +#if AUTOTUNE_ENABLED + &copter.mode_autotune, +#endif +#if MODE_POSHOLD_ENABLED + &copter.mode_poshold, +#endif +#if MODE_BRAKE_ENABLED + &copter.mode_brake, +#endif +#if MODE_THROW_ENABLED + &copter.mode_throw, +#endif +#if HAL_ADSB_ENABLED + &copter.mode_avoid_adsb, +#endif +#if MODE_GUIDED_NOGPS_ENABLED + &copter.mode_guided_nogps, +#endif +#if MODE_SMARTRTL_ENABLED + &copter.mode_smartrtl, +#endif +#if MODE_FLOWHOLD_ENABLED + (Mode*)copter.g2.mode_flowhold_ptr, +#endif +#if MODE_FOLLOW_ENABLED + &copter.mode_follow, +#endif +#if MODE_ZIGZAG_ENABLED + &copter.mode_zigzag, +#endif +#if MODE_SYSTEMID_ENABLED + (Mode *)copter.g2.mode_systemid_ptr, +#endif +#if MODE_AUTOROTATE_ENABLED + &copter.mode_autorotate, +#endif +#if MODE_TURTLE_ENABLED + &copter.mode_turtle, +#endif + }; + + const uint8_t mode_count = ARRAY_SIZE(modes); + + // Convert to zero indexed + const uint8_t index_zero = index - 1; + if (index_zero >= mode_count) { + // Mode does not exist!? + return mode_count; + } + + // Ask the mode for its name and number + const char* name = modes[index_zero]->name(); + uint8_t mode_number = (uint8_t)modes[index_zero]->mode_number(); + +#if MODE_AUTO_ENABLED + // Auto RTL is odd + // Have to deal with is separately becase its number and name can change depending on if were in it or not + if (index_zero == 0) { + mode_number = (uint8_t)Mode::Number::AUTO_RTL; + name = "AUTO RTL"; + + } else if (index_zero == 1) { + mode_number = (uint8_t)Mode::Number::AUTO; + name = "AUTO"; + + } +#endif + + mavlink_msg_available_modes_send( + chan, + mode_count, + index, + MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD, + mode_number, + 0, // MAV_MODE_PROPERTY bitmask + name + ); + + return mode_count; +} diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index e38c9b6e7aa02..558804c05e8c3 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -64,6 +64,10 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK uint32_t log_radio_bit() const override { return MASK_LOG_PM; } #endif + // Send the mode with the given index (not mode number!) return the total number of modes + // Index starts at 1 + uint8_t send_available_mode(uint8_t index) const override; + private: // sanity check velocity or acceleration vector components are numbers diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 8f5425482143a..fd5eea3e8626f 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -771,7 +771,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced AP_GROUPINFO("GND_EFFECT_COMP", 5, ParametersG2, gndeffect_comp_enabled, 1), -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED // @Group: AFS_ // @Path: ../libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp AP_SUBGROUPINFO(afs, "AFS_", 6, ParametersG2, AP_AdvancedFailsafe), @@ -1240,8 +1240,11 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { /* constructor for g2 object */ -ParametersG2::ParametersG2(void) - : command_model_pilot(PILOT_Y_RATE_DEFAULT, PILOT_Y_EXPO_DEFAULT, 0.0f) +ParametersG2::ParametersG2(void) : + unused_integer{17} +#if HAL_BUTTON_ENABLED + ,button_ptr(&copter.button) +#endif #if AP_TEMPCALIBRATION_ENABLED , temp_calibration() #endif @@ -1251,21 +1254,21 @@ ParametersG2::ParametersG2(void) #if HAL_PROXIMITY_ENABLED , proximity() #endif -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED ,afs() #endif #if MODE_SMARTRTL_ENABLED ,smart_rtl() #endif +#if USER_PARAMS_ENABLED + ,user_parameters() +#endif #if MODE_FLOWHOLD_ENABLED ,mode_flowhold_ptr(&copter.mode_flowhold) #endif #if MODE_FOLLOW_ENABLED ,follow() #endif -#if USER_PARAMS_ENABLED - ,user_parameters() -#endif #if AUTOTUNE_ENABLED ,autotune_ptr(&copter.mode_autotune.autotune) #endif @@ -1275,13 +1278,9 @@ ParametersG2::ParametersG2(void) #if MODE_AUTOROTATE_ENABLED ,arot() #endif -#if HAL_BUTTON_ENABLED - ,button_ptr(&copter.button) -#endif #if MODE_ZIGZAG_ENABLED ,mode_zigzag_ptr(&copter.mode_zigzag) #endif - #if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED ,command_model_acro_rp(ACRO_RP_RATE_DEFAULT, ACRO_RP_EXPO_DEFAULT, 0.0f) #endif @@ -1290,6 +1289,8 @@ ParametersG2::ParametersG2(void) ,command_model_acro_y(ACRO_Y_RATE_DEFAULT, ACRO_Y_EXPO_DEFAULT, 0.0f) #endif + ,command_model_pilot(PILOT_Y_RATE_DEFAULT, PILOT_Y_EXPO_DEFAULT, 0.0f) + #if WEATHERVANE_ENABLED ,weathervane() #endif diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 0eee4058331d7..0cf0a8cb51959 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -499,6 +499,11 @@ class ParametersG2 { // altitude at which nav control can start in takeoff AP_Float wp_navalt_min; + // unused_integer simply exists so that the constructor for + // ParametersG2 can be created with a relatively easy syntax in + // the face of many #ifs: + uint8_t unused_integer; + // button checking #if HAL_BUTTON_ENABLED AP_Button *button_ptr; @@ -530,8 +535,8 @@ class ParametersG2 { // whether to enforce acceptance of packets only from sysid_my_gcs AP_Int8 sysid_enforce; - -#if ADVANCED_FAILSAFE + +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED // advanced failsafe library AP_AdvancedFailsafe_Copter afs; #endif diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 510ed875e842b..26bda5c232ac0 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,429 @@ ArduPilot Copter Release Notes: ------------------------------------------------------------------ +Release 4.6.0-beta 13 Nov 2024 + +Changes from 4.5.7 + +1) Board specific changes + +- AnyLeaf H7 supports compass and onboard logging +- Blitz743Pro supports CAN +- BlueRobotics Navigator supports BMP390 baro +- Bootloader ECC failure check fixed on boards with >128k bootloader space (e.g CubeRed) +- CB Unmanned Stamp H743 support +- ClearSky CSKY405 support +- CUAV-7-Nano default batt monitor fixed +- CubeRed bootloader fixes including disabling 2nd core by default +- CubeRed supports PPP networking between primary and secondary MCU +- CubeRedPrimary supports external compasses +- ESP32 main loop rate improvements +- ESP32 RC input fixes and wifi connection reliability improved +- ESP32 safety switch and GPIO pin support +- FlyingMoon no longer support MAX7456 +- Flywoo F405HD-AIOv2 ELRS RX pin pulled high during boot +- Flywoo H743 Pro support +- Flywoo/Goku F405 HD 1-2S ELRS AIO v2 +- FlywooF745 supports DPS310 baro +- FPV boards lose SMBus battery support (to save flash) +- GEPRC F745BTHD support +- GEPRCF745BTHD loses parachute support, non-BMP280 baros (to save flash) +- Here4FC bootloader fix for mismatch between RAM0 and periph that could prevent firmware updates +- Holybro Kakute F4 Wing support +- iFlight 2RAW H743 supports onboard logging +- JFB110 supports measuring servo rail voltage +- JFB110 supports safety switch LED +- JHEM-JHEF405 bootloader supports firmware updates via serial +- JHEMCU GF30H743 HD support +- JHEMCU-GF16-F405 autopilot support +- JHEMCU-GSF405A becomes FPV board (to save flash) +- KakuteF7 only supports BMP280 baro (to save flash) +- KakuteH7Mini supports ICM42688 IMU +- Linux auto detection of GPS baud rate fixed +- Linux board scheduler jitter reduced +- Linux board shutdown fixes +- MakeFlyEasy PixPilot-V6Pro support +- MatekF405, Pixhawk1-1M-bdshot, revo-mini loses blended GPS (to save flash) +- MatekH7A3 support Bi-directional DShot +- MicoAir405v2 and MicoAir405Mini support optical flow and OSD +- MicoAir743 internal compass orientation fixed +- MicroAir405Mini, MicroAir743, NxtPX4v2 support +- MicroAir405v2 Bi-directional DShot and LED DMA fixes +- MicroAir405v2 defined as FPV board with reduced features (to save flash) +- ModalAI VOXL2 support including Starling 2 and Starling 2 max +- mRo Control Zero Classic supports servo rail analog input +- mRo KitCAN revC fixed +- Mugin MUPilot support +- OmnibusF7v2 loses quadplane support (to save flash) +- Pixhack-v3 board added (same as fmuv3) +- Pixhawk6C bootloader supports flashing firmware from SD card +- RadiolinkPIX6 imu orientation fixed +- RadiolinkPIX6 supports SPA06 baro +- ReaperF745 V4 FC supports MPU6000 IMU +- RPI5 support +- SDModelH7v2 SERIAL3/7/9_PROTOCOL param defaults changed +- Solo serial ports default to MAVLink1 +- SpeedyBeeF405Wing gets Bi-directional DShot +- SpeedyBeeF405WING loses landing gear support, some camera gimbals (to save flash) +- Spektreworks boom board support +- TrueNavPro-G4 SPI does not share DMA +- X-MAV AP-H743v2 support + +2) AHRS/EKF enhancements and fixes + +- AHRS_OPTION to disable fallback to DCM (affects Plane and Rover, Copter never falls back) +- AHRS_OPTION to disable innovation check for airspeed sensor +- Airspeed sensor health check fixed when using multiple sensors and AHRS affinity +- DCM support for MAV_CMD_EXTERNAL_WIND_ESTIMATE (Plane only) +- EK2 supports disabling external nav (see EK2_OPTIONS) +- EK3 External Nav position jump after switch from Optical flow removed (see EK3_SRC_OPTION=1) +- EK3 uses filtered velocity corrections for IMU position +- EKF2, EKF3, ExternalAHRS all use common origin +- EKF3 accepts set origin even when using GPS +- EKF3 allows earth-frame fields to be estimated with an origin but no GPS +- EKF3 copes better with GPS jamming +- EKF3 logs mag fusion selection to XKFS +- EKF3 wind estimation when using GPS-for-yaw fixed +- External AHRS improvements including handling variances, pre-arm origin check +- Inertial Labs External AHRS fixes +- VectorNav driver fix for handling of error from sensor +- VectorNav External AHRS enhancements including validation of config commands and logging improvements +- VectorNav support for sensors outside VN-100 and VN-300 + +3) Driver enhancements and bug fixes + +- ADSB fix to display last character in status text sent to GCS +- Ainstein LR-D1 radar support +- Airspeed ASP5033 whoami check fixed when autopilot rebooted independently of the sensor +- AIRSPEED message sent to GCS +- Analog temperature sensor extended to 5th order polynomial (see TEMP_A5) +- ARSPD_OPTIONS to report calibration offset to GCS +- Baro EAS2TAS conversions continuously calculated reducing shocks to TECS (Plane only) +- Baro provides improved atmospheric model for high altitude flight +- BARO_ALT_OFFSET slew slowed to keep EKF happy +- BATTx_ESC_MASK param supports flexible mapping of ESCs to batteries +- BATTx_OPTION to not send battery voltage, current, etc to GCS +- Benewake RDS02U radar support +- Bi-directional DShot on IOMCU supports reversible mask +- Bi-directional DShot telemetry support on F103 8Mhz IOMCUs +- BMM350 compass support +- CAN rangefinders and proximity sensors may share a CAN bus (allows NRA24 and MR72 on a single CAN bus) +- Compass calibration world magnetic model checks can use any position source (e.g. not just GPS) +- CRSF baro and vertical speeed fixed +- CRSF RX bind command support +- DroneCAN battery monitor check to avoid memory corruption when type changed +- DroneCAN DNA server fixes including removing use of invalid node IDs, faster ID allocation, elimination of rare inability to save info +- DroneCAN EFI health check fix +- DroneCAN ESC battery monitors calculate consumed mah +- DroneCAN ESCs forced to zero when disarmed +- DroneCAN RPM message support +- DroneCAN timeout fix for auxiliary frames +- DroneCAN to serial tunneling params accepts short-hand baud rates (e.g. '57' for '57600') +- F9P, F10-N and Zed-F9P support for GPSx_GNSS_MODE to turn on/off SBAS, Galileo, Beidou and Glonass +- FuelLevel battery monitor fix to report capacity +- GPS_xxx params renamed to GPS1_xxx, GPS_xxx2 renamed to GPS2_xxx +- Hirth EFI logging includes modified throttle +- Hirth ICEngine supports reversed crank direction (see ICE_OPTIONS parameter) +- Hott and LTM telemetry deprecated (still available through custom build server) +- i-BUS telemetry support +- ICE_PWM_IGN_ON, ICE_PWM_IGN_OFF, ICE_PWM_STRT_ON, ICE_PWM_STRT_OFF replaced with SERVOx_MIN/MAX/REVERSED (Plane only) +- ICE_START_CHAN replaced with RC aux function (Plane only) +- ICEngine retry max added (see ICE_STRT_MX_RTRY) +- IE 2400 generator error message severity to GCS improved +- INA2xx battery monitor support (reads temp, gets MAX_AMPS and SHUNT params) +- MCP9600 temperature sensor I2C address fixed +- MLX90614 temperature sensor support +- MSP GPS ground course scaling fixed +- MSP RC uses scaled RC inputs (fixes issue with RCx_REVERSED having no effect) +- Networking supports reconnection to TCP server or client +- OSD params for adjusting horizontal spacing and vertical extension (see OSD_SB_H_OFS, OSD_SB_V_EXT) +- Relay inverted output support (see RELAYx_INVERTED parameter) +- ROMFS efficiency improvements +- RS-485 driver enable RTS flow control +- Sagetech MXS ADSP altitude fix (now uses abs alt instead of terrain alt) +- Septentrio GPS sat count correctly drops to zero when 255 received +- Septentrio supports selecting constellations (see GPS_GNSS_MODE) +- Single LED for user notification supported +- SPA06 baro supported +- Sum battery monitor optionally reports minimum voltage instead of average +- Sum battery monitor reports average temp +- Torqeedo dual motor support (see TRQ1, TRQ2 params) +- Ublox GPS driver uses 64 bit time for PPS interrupt (avoids possible dropout at 1hour and 12 minutes) +- UBlox GPS time ignored until at least 2D fix +- VideoTX supports additional freq bands (RushFPV 3.3Ghz) +- Volz logs desired and actual position, voltage, current, motor and PCB temp +- Volz server feedback and logging fixed +- Volz servo output in its own thread resulting in smoother movements +- W25N02KV flash support + +4) Networking enhancements and fixes + +- Allow multiple UDP clients to connect/disconnect/reconnect +- Ethernet supports faster log downloading (raised SDMMC clock limit on H7) + +5) Camera and gimbal enhancements + +- Alexmos precision improved slightly +- CAMERA_CAPTURE_STATUS mavlink msg sent to GCS (reports when images taken or video recorded, used by QGC) +- CAMERA_FOV_STATUS mavlink reports lat,lon of what camera is pointing at +- DO_MOUNT_CONTROL yaw angle interpreted as body-frame (was incorrectly being interpreted as earth-frame) +- Dual serial camera gimbal mounts fixed +- Lua script bindings to send CAMERA_INFORMATION and VIDEO_STREAM_INFORMATION messages to GCS +- Retract Mount2 aux function added (see RCx_OPTION = 113) +- Servo gimbal reported angles respect roll, pitch and yaw limits +- Siyi driver sends autopilot location and speed (recorded in images via EXIF) +- Siyi picture and video download scripts +- Siyi ZT6 and ZT30 support sending min, max temperature (see CAMERA_THERMAL_RANGE msg) +- Siyi ZT6 and ZT30 thermal palette can be changed using camera-change-setting.lua script +- Siyi ZT6 hardware id and set-lens fixed +- Topotek gimbal support +- Trigger distance ignores GPS status and only uses EKF reported location + +6) Harmonic notch enhancements + +- Harmonic notch is active in forward flight on quadplanes +- Harmonic notch filter freq clamping and disabling reworked +- Harmonic notch handles negative ESC RPMs +- Harmonic notch supports per-motor throttle-based harmonic notch + +7) Copter specific enhancements and bug fixes + +- Attitude control fix to dt update order (reduces rate controller noise) +- Auto mode fix to avoid prematurely advancing to next waypoint if given enough time +- Auto mode small target position jump when takeoff completes removed +- Auto mode yaw drift when passing WP removed if CONDITION_YAW command used and WP_YAW_BEHAVIOR = 0/None +- Auto, Guided flight mode pause RC aux switch (see RCx_OPTION = 178) +- AutoRTL (e.g. DO_LAND_START) uses copter stopping point to decide nearest mission command +- AutoRTL mode supports DO_RETURN_PATH_START (Copter, Heli only) +- AutoTune fix to prevent spool up after landing +- AutoTune performance and safety enhancements (less chance of ESC desync, fails when vehicle likely can't be tuned well) +- Autotune test gains RC aux switch function allows testing gains in any mode (see RCx_OPTION = 180) +- Config error avoided if auto mode is paused very soon after poweron +- FLIGHT_OPTIONS bit added to require position estimate before arming +- Follow mode slowdown calcs fixed when target is moving +- Ground oscillation suppressed by reducing gains (see ATC_LAND_R/P/Y_MULT) +- Guided mode internal error fix when taking off using SET_ATTITUDE_CONTROL message +- Guided mode internal error resolved when switching between thrust or climb rate based altitude control +- Guided mode yaw fixed when WP_YAW_BEHAVIOR = 0/None and CONDITION_YAW command received containing relative angle +- Landing detector fixed when in stabilize mode at full throttle but aircraft is upside down +- Landing detector logging added to ease support (see LDET message) +- Loiter unlimited command accepts NaNs (QGC sends these) +- Mavlink SYSTEM_STATUS set to BOOT during initialisation +- MOT_PWM_TYPE of 9 (PWMAngle) respects SERVOx_MIN/MAX/TRIM/REVERSED param values +- Payload place bug fix when aborted because gripper is already released +- RC based tuning (aka CH6 tuning) can use any RC aux function channel (see RCx_OPTION = 219) +- RTL_ALT minimum reduced to 30cm +- SystemID position controller support (Copter and Heli) +- TriCopter motor test and slew-up fixed +- WPNAV_SPEED min reduced to 10 cm/s (Copter only) +- Loiter mode zeros desired accel when re-entering from Auto during RC failsafe + +8) TradHeli specific enhancements + +- Autorotation yaw behaviour fix +- Autotune improvements including using low frequency dwell for feedforward gain tuning and conducting sweep in attitude instead of rate +- Blade pitch angle logging added (see SWSH log message) +- Constrain cyclic roll for intermeshing +- ICE / turbine cool down fix +- Inverted flight extended to non manual throttle modes +- Inverted flight transitions smoothed and restricted to only Stabilize mode +- SWSH logging fix for reversed collectives + +9) Plane specific enhancements and bug fixes + +- AIRSPEED_STALL holds vehicle stall speed and is used for minimum speed scaling +- Allow for different orientations of landing rangefinder +- Assistance requirements evaluted on mode change +- FBWB/CRUISE climb/sink rate limited by TECS limits +- FLIGHT_OPTION to immediately climb in AUTO mode (not doing glide slope) +- Glider pullup support (available only through custom build server) +- Loiter breakout improved to better handle destinations inside loiter circle +- Manual mode throttle made consistent with other modes (e.g battery comp and watt limit is done if enabled) +- Mavlink GUIDED_CHANGE_ALTITUDE supports terrain altitudes +- Minimum throttle not applied during SLT VTOL airbrake (reduces increase in airspeed and alt during back transition) +- Q_APPROACH_DIST set minimum distance to use the fixed wing approach logic +- Quadplane assistance check enhancements +- Quadplane Deca frame support +- Quadplane gets smoother takeoff by input shaping target accel and velocity +- Servo wiggles in altitude wait staged to be one after another +- Set_position_target_global_int accepts MAV_FRAME_GLOBAL_RELATIVE_ALT and MAV_FRAME_GLOBAL_TERRAIN_ALT frames +- Takeoff airspeed control improved (see TKOFF_MODE, TKOFF_THR_MIN) +- Takeoff fixes for fence autoenable +- Takeoff improvements including less overshoot of TKOFF_ALT +- TECS reset along with other controllers (important if plane dropped from balloon) +- Tilt quadplane ramp of motors on back transition fixed +- Tiltrotor tilt angles logged +- TKOFF_THR_MIN applied to SLT transitions +- Twin motor planes with DroneCAN ESCs fix to avoid max throttle at arming due to misconfiguration +- VTOLs switch to QLAND if a LONG_FAILSAFE is triggered during takeoff + +10) Rover specific enhancements and bug fixes + +- Auto mode reversed state maintained if momentarily switched to Hold mode +- Circle mode tracks better and avoids getting stuck at circle edge +- Flight time stats fixed +- MAV_CMD_NAV_SET_YAW_SPEED deprecated +- Omni3Mecanum frame support +- Stopping point uses max deceleration (was incorrectly using acceleration) +- Wheel rate controller slew rate fix + +11) Antenna Tracker specific enhancements and bug fixes + +- Never track lat,lon of 0,0 + +12) Scripting enhancements + +- advance-wp.lua applet supports advancing Auto mode WP via RC switch +- AHRS_switch.lua supports switching between EKF2 and EKF3 via RC switch +- battery_internal_resistance_check.lua monitors battery resistance +- CAN:get_device returns nil for unconfigured CAN device +- copter_terrain_brake.lua script added to prevent impact with terrain in Loiter mode (Copter only) +- Copter:get_target_location, update_target_location support +- crosstrack_restore.lua example allows returning to previous track in Auto (Plane only) +- Display text on OLED display supported +- Docs improved for many bindings +- EFI get_last_update_ms binding +- EFI_SkyPower.lua driver accepts 2nd supply voltage +- ESC_slew_rate.lua example script supports testing ESCs +- Filesystem CRC32 check to allow scripts to check module versions +- forced arming support +- GPIO get/set mode bindings (see gpio:set_mode, get_mode) +- GPS-for-yaw angle binding (see gps:gps_yaw_deg) +- Halo6000 EFI driver can log all CAN packets for easier debugging +- handle_external_position_estimate binding allows sending position estimate from lua to EKF +- I2C:transfer support +- IMU gyros_consistent and accels_consistent bindings added +- INF_Inject.lua driver more robust to serial errors, improved logging, throttle and ignition control +- INS bindings for is calibrating, gyro and accel sensor values +- IPV4 address bindings (see SocketAPM_ipv4_addr_to_string) to allow UDP server that responds to individual clients +- Logging of booleans supported +- Lua language checks improved (finds more errors) +- MAVLink commands can be called from scripting +- MCU voltage binding (see analog:mcu_voltage) +- NMEA 2000 EFI driver (see EFI_NMEA2k.lua) +- "open directory failed" false warning when scripts in ROMFS fixed +- Param_Controller.lua supports quickly switching between 3 parameter sets via RC switch +- Pass by reference values are always initialized +- pelco_d_antennatracker.lua applet supports sending Pelco-D via RS-485 to PTZ cameras +- plane_aerobatics.lua minor enhancements +- REPL applet (read-evaluate-print-loop, see repl.lua) for interactive testing and experimentation +- "require" function failures in rare circumstances fixed +- "require" function works for modules in ROMFS (e.g. not on SD card) +- revert_param.lua supports more params (e.g ATC_RATE_R/P/Y, PTCH2SRV_TCONST, RLL2SRV_TCONST, TECS_) +- Scripts may receive mavlink messages which fail CRC (e.g messages which FC does not understand) +- SD card formatting supported +- Serial device simulation support (allows Lua to feed data to any supported serial protocol for e.g. sensor simulation) +- set_target_throttle_rate_rpy allows rate control from scripts (new for Copter) +- sitl_standby_sim.lua example shows how to switch between redundant flight controllers using an RC switch +- Slung payload oscillation suppression applet added (see copter-slung-payload.lua) +- Temperature sensor bindings added +- uint64 support +- Various performance and memory usage optimizations +- VTOL-quicktune.lua minor enhancements including increasing YAW_FLTE_MAX to 8 +- x-quad-cg-allocation.lua applet corrects for the CoM discrepancy in a quad-X frame + +13) GCS / mavlink related changes and fixes + +- BATTERY2 message deprecated (replaced by BATTERY_STATUS) +- CMD_MISSION_START/STOP rejected if first-item/last-item args provided +- Deny attempts to upload two missions simultaneously +- Fence and Rally points may be uploaded using FTP +- GPS_INPUT and HIL_GPS handles multiple GPSs +- HIGHRES_IMU mavlink message support (used by companion computers to receive IMU data from FC) +- MAV_CMD_COMPONENT_ARM_DISARM accepts force arm magic value of 21196 +- MAV_CMD_DO_SET_SAFETY_SWITCH_STATE support +- MAV_CMD_SET_HAGL support (Plane only) +- MAVFTP respects TX buffer flow control to improve FTP on low bandwidth links +- MAVLink receiver support (RADIO_RC_CHANNELS mavlink message) +- Message interval supports TERRAIN_REPORT msg +- Mission upload may be cancelled using MISSION_CLEAR_ALL +- MOUNT_CONFIGURE, MOUNT_CONTROL messages deprecated +- RC_CHANNELS_RAW deprecated +- Serial passthrough supports parity allowing STM32CubeProgrammer to be used to program FrSky R9 receivers +- SET_ATTITUDE_TARGET angular rate input frame fixed (Copter only) +- TIMESYNC and NAMED_VALUE_FLOAT messages not sent on high latency MAVLink ports + +14) Logging enhancements and fixes + +- AC_PID resets and I-term sets logged +- ANG provides attitude at high rate (equivalent to ATT) +- ATT logs angles as floats (better resolution than ints) +- CAND message gets driver index +- DCM log message includes roll/pitch and yaw error +- EDT2 log msg includes stress and status received via extended DShot Telemetry v2 +- EFI ECYL cylinder head and exhaust temp logs in degC (was Kelvin) +- ESCX log msg includes DroneCAN ESCs status, temp, duty cycle and power pct +- FMT messages written as required instead of all at beginning +- Logging restarted after download completes when LOG_DISARMED = 1 +- MISE msg logs active mission command (CMD logged when commands are uploaded) +- ORGN message logging fixed when set using SET_GPS_GLOBAL_ORIGIN +- RPM sensor logging gets instance field, quality and health fields +- Short filename support removed (e.g log1.BIN instead of 00000001.BIN) +- Temperature sensor logging option for only sensors with no source (see TEMP_LOG) +- UART data rates logged at 1hz (see UART message) + +15) ROS2 / DDS support + +- Airspeed published +- Battery topic reports all available batteries +- Compile-time configurable rates for each publisher +- DDS_TIMEOUT_MS and DDS_MAX_RETRY set timeout and num retries when client pings XRCE agent +- GPS global origin published at 1 Hz +- High frequency raw imu data transmission +- Joystick support +- Support sending waypoints to Copter and Rover +- Remove the XML refs file in favor of binary entity creation + +16) Safety related enhancements and fixes + +- Accel/Gyro inconsistent message fixed for using with only single IMU +- Battery monitor failure reported to GCS, triggers battery failsafe but does not take action +- Far from EKF origin pre-arm check removed (Copter only) +- Fence breach warning message slightly improved +- Fence enhancements incluiding alt min fence (Copter only, see FENCE_AUTOENABLE, FENCE_OPTIONS) +- Fences can be stored to SD Card (see BRD_SD_FENCE param) +- ICEngine stopped when in E-Stop or safety engaged (Plane only) +- LEDs flash green lights based on EKF location not GPS +- Parachute option to skip disarm before parachute release (see CHUTE_OPTIONS) +- Plane FENCE_AUTOENABLE of 1 or 2 deprecation warning added +- Pre-arm check if OpenDroneID is compiled in but disabled +- Pre-arm check of duplicate RC aux functions fixed (was skipping recently added aux functions) +- Pre-arm checks alert user more quickly on failure +- Prearm check for misconfigured EAHRS_SENSORS and GPS_TYPE +- Proximity sensor based avoidance keeps working even if one proximity sensor fails (Copter, Rover) +- RC aux functions for Arm, Disarm, E-Stop and Parachute ignored when RC is first turned on +- Warning of duplicate SERIALx_PROTOCOL = RCIN + +17) Other bug fixes and minor enhancements + +- Accel cal fixed for auxiliary IMUs (e.g. IMU4 and higher) +- Bootloader fix to reduce unnecessary mass erasing of flash when using STLink or Jlink tools +- Bootloader rejects allocation of broadcast node ID +- CAN forward registering/de-registering fix (affected Mission Planner DroneCAN UI) +- Dijkstras fix to correct use of uninitialised variable +- DShot rates are not limited by NeoPixel rates +- Ethernet and CAN bootloader fix to prevent getting stuck in bootloader mode +- Filesystem does not show entries for empty @ files +- Filesystem efficiency improvements when reading files +- Flight statistics only reset if user sets STAT_RESET to zero (avoids accidental reset) +- Flight time statistics updated on disarm (avoids issue if vehicle powered-down soon after disarm) +- Internal error thrown if we lose parameters due to saving queue being too small +- MAVLink via DroneCAN baud rate fix +- SPI pins may also be used as GPIOs +- Terrain cache size configurable (see TERRAIN_CACHE_SZ) + +18) Developer focused fixes and enhancements + +- AP_Camera gets example python code showing how to use GStreamer with UDP and RTSP video streams +- Cygwin build fix for non-SITL builds +- Cygwin build fixed with malloc wrap +- DroneCAN and scripting support FlexDebug messages (see CAN_Dn_UC_OPTION, FlexDebug.lua) +- EKF3 code generator documentation and cleanup +- GPS jamming simulator added +- MacOS compiler warnings reduced +- SFML joystick support +- SITL support for OpenBSD +- Text warning if older Fence or Rally point protocols are used +------------------------------------------------------------------ Release 4.5.7 08 Oct 2024 Changes from 4.5.7-beta1 diff --git a/ArduCopter/afs_copter.cpp b/ArduCopter/afs_copter.cpp index a0b83b36cad3a..0d639e1276b12 100644 --- a/ArduCopter/afs_copter.cpp +++ b/ArduCopter/afs_copter.cpp @@ -4,7 +4,7 @@ #include "Copter.h" -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED /* setup radio_out values for all channels to termination values @@ -63,4 +63,4 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Copter::afs_mode(void) { copter.set_mode(Mode::Number::AUTO,ModeReason::GCS_FAILSAFE); } -#endif // ADVANCED_FAILSAFE +#endif // AP_COPTER_ADVANCED_FAILSAFE_ENABLED diff --git a/ArduCopter/afs_copter.h b/ArduCopter/afs_copter.h index 29e2dfa7a347c..4d133f6276a3d 100644 --- a/ArduCopter/afs_copter.h +++ b/ArduCopter/afs_copter.h @@ -18,7 +18,9 @@ advanced failsafe support for copter */ -#if ADVANCED_FAILSAFE +#include "config.h" + +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED #include /* @@ -44,5 +46,5 @@ class AP_AdvancedFailsafe_Copter : public AP_AdvancedFailsafe void set_mode_auto(void) override; }; -#endif // ADVANCED_FAILSAFE +#endif // AP_COPTER_ADVANCED_FAILSAFE_ENABLED diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index 703f5cf722a2e..28282c35399f1 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -146,9 +146,10 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan) read_radio(); // pass through throttle to motors - SRV_Channels::cork(); + auto &srv = AP::srv(); + srv.cork(); motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f); - SRV_Channels::push(); + srv.push(); // read some compass values compass.read(); diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 3faf4609c6672..0da560ea09099 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -574,8 +574,8 @@ // Developer Items // -#ifndef ADVANCED_FAILSAFE -# define ADVANCED_FAILSAFE 0 +#ifndef AP_COPTER_ADVANCED_FAILSAFE_ENABLED +# define AP_COPTER_ADVANCED_FAILSAFE_ENABLED 0 #endif #ifndef CH_MODE_DEFAULT diff --git a/ArduCopter/esc_calibration.cpp b/ArduCopter/esc_calibration.cpp index b4c5c11906399..0c1d7ea70e636 100644 --- a/ArduCopter/esc_calibration.cpp +++ b/ArduCopter/esc_calibration.cpp @@ -95,9 +95,10 @@ void Copter::esc_calibration_passthrough() hal.scheduler->delay(3); // pass through to motors - SRV_Channels::cork(); + auto &srv = AP::srv(); + srv.cork(); motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f); - SRV_Channels::push(); + srv.push(); } #endif // FRAME_CONFIG != HELI_FRAME } @@ -112,25 +113,26 @@ void Copter::esc_calibration_auto() esc_calibration_setup(); // raise throttle to maximum - SRV_Channels::cork(); + auto &srv = AP::srv(); + srv.cork(); motors->set_throttle_passthrough_for_esc_calibration(1.0f); - SRV_Channels::push(); + srv.push(); // delay for 5 seconds while outputting pulses uint32_t tstart = millis(); while (millis() - tstart < 5000) { - SRV_Channels::cork(); + srv.cork(); motors->set_throttle_passthrough_for_esc_calibration(1.0f); - SRV_Channels::push(); + srv.push(); esc_calibration_notify(); hal.scheduler->delay(3); } // block until we restart while(1) { - SRV_Channels::cork(); + srv.cork(); motors->set_throttle_passthrough_for_esc_calibration(0.0f); - SRV_Channels::push(); + srv.push(); esc_calibration_notify(); hal.scheduler->delay(3); } diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index d49ce3d01aaf0..3002e773c02e5 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -492,7 +492,7 @@ void Copter::do_failsafe_action(FailsafeAction action, ModeReason reason){ set_mode_SmartRTL_or_land_with_pause(reason); break; case FailsafeAction::TERMINATE: { -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED g2.afs.gcs_terminate(true, "Failsafe"); #else arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE); diff --git a/ArduCopter/failsafe.cpp b/ArduCopter/failsafe.cpp index 57864b01be009..f143505977923 100644 --- a/ArduCopter/failsafe.cpp +++ b/ArduCopter/failsafe.cpp @@ -72,7 +72,7 @@ void Copter::failsafe_check() } -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED /* check for AFS failsafe check */ diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index faf22b0180f45..5579b51fb786e 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -4,7 +4,7 @@ #include #include // TODO why is this needed if Copter.h includes this -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED #include "afs_copter.h" #endif @@ -134,7 +134,7 @@ class Mode { virtual bool allows_flip() const { return false; } virtual bool crash_check_enabled() const { return true; } -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED // Return the type of this mode for use by advanced failsafe virtual AP_AdvancedFailsafe_Copter::control_mode afs_mode() const { return AP_AdvancedFailsafe_Copter::control_mode::AFS_STABILIZED; } #endif @@ -517,7 +517,7 @@ class ModeAuto : public Mode { bool allows_inverted() const override { return true; }; #endif -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED // Return the type of this mode for use by advanced failsafe AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; } #endif @@ -1068,7 +1068,7 @@ class ModeGuided : public Mode { bool requires_terrain_failsafe() const override { return true; } -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED // Return the type of this mode for use by advanced failsafe AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; } #endif @@ -1243,7 +1243,7 @@ class ModeLand : public Mode { bool is_landing() const override { return true; }; -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED // Return the type of this mode for use by advanced failsafe AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; } #endif @@ -1425,7 +1425,7 @@ class ModeRTL : public Mode { bool requires_terrain_failsafe() const override { return true; } -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED // Return the type of this mode for use by advanced failsafe AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; } #endif diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index c06072c597f90..5474aaa84151f 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -535,11 +535,7 @@ void ModePosHold::update_brake_angle_from_velocity(float &brake_angle, float vel } // calculate velocity-only based lean angle - if (velocity >= 0) { - lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (velocity + 60.0f)); - } else { - lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (-velocity + 60.0f)); - } + lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (fabsf(velocity) + 60.0f)); // do not let lean_angle be too far from brake_angle brake_angle = constrain_float(lean_angle, brake_angle - brake_rate, brake_angle + brake_rate); @@ -592,7 +588,7 @@ void ModePosHold::update_wind_comp_estimate() } // limit acceleration - const float accel_lim_cmss = tanf(radians(POSHOLD_WIND_COMP_LEAN_PCT_MAX * copter.aparm.angle_max * 0.01f)) * 981.0f; + const float accel_lim_cmss = tanf(radians(POSHOLD_WIND_COMP_LEAN_PCT_MAX * copter.aparm.angle_max * 0.01f)) * (GRAVITY_MSS*100); const float wind_comp_ef_len = wind_comp_ef.length(); if (!is_zero(accel_lim_cmss) && (wind_comp_ef_len > accel_lim_cmss)) { wind_comp_ef *= accel_lim_cmss / wind_comp_ef_len; diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 45eccc3e84f99..c37a4d55a2fb3 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -135,7 +135,7 @@ void Copter::auto_disarm_check() // motors_output - send output to motors library which will adjust and send to ESCs and servos void Copter::motors_output() { -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED // this is to allow the failsafe module to deliberately crash // the vehicle. Only used in extreme circumstances to meet the // OBC rules @@ -156,8 +156,10 @@ void Copter::motors_output() // output any servo channels SRV_Channels::calc_pwm(); + auto &srv = AP::srv(); + // cork now, so that all channel outputs happen at once - SRV_Channels::cork(); + srv.cork(); // update output on any aux channels, for manual passthru SRV_Channels::output_ch_all(); @@ -181,7 +183,7 @@ void Copter::motors_output() } // push all channels - SRV_Channels::push(); + srv.push(); } // check for pilot stick input to trigger lost vehicle alarm diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index b92178d0981ea..9873d1a2846f8 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -45,7 +45,7 @@ void Copter::init_rc_out() motors->init((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get()); // enable aux servos to cope with multiple output channels per motor - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); // update rate must be set after motors->init() to allow for motor mapping motors->set_update_rate(g.rc_speed); diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 0181f392d804b..1e10df8c42784 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -329,7 +329,7 @@ void Plane::one_second_loop() set_control_channels(); #if HAL_WITH_IO_MCU - iomcu.setup_mixing(&rcmap, g.override_channel.get(), g.mixing_gain, g2.manual_rc_mask); + iomcu.setup_mixing(g.override_channel.get(), g.mixing_gain, g2.manual_rc_mask); #endif #if HAL_ADSB_ENABLED @@ -346,7 +346,7 @@ void Plane::one_second_loop() // sync MAVLink system ID mavlink_system.sysid = g.sysid_this_mav; - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); // update notify flags AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index d2800c367631f..298b4e47a5195 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -630,10 +630,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif MSG_NAV_CONTROLLER_OUTPUT, @@ -710,7 +716,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_VIBRATION, }; static const ap_message STREAM_PARAMS_msgs[] = { - MSG_NEXT_PARAM + MSG_NEXT_PARAM, + MSG_AVAILABLE_MODES }; static const ap_message STREAM_ADSB_msgs[] = { MSG_ADSB_VEHICLE, @@ -863,6 +870,9 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) || (plane.control_mode == &plane.mode_guided)) { plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND); +#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED + plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; +#endif // add home alt if needed if (requested_position.relative_alt) { @@ -973,14 +983,20 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl float new_target_heading = radians(wrap_180(packet.param2)); - // course over ground - if ( int(packet.param1) == HEADING_TYPE_COURSE_OVER_GROUND) { // compare as nearest int + switch(HEADING_TYPE(packet.param1)) { + case HEADING_TYPE_COURSE_OVER_GROUND: + // course over ground plane.guided_state.target_heading_type = GUIDED_HEADING_COG; plane.prev_WP_loc = plane.current_loc; - // normal vehicle heading - } else if (int(packet.param1) == HEADING_TYPE_HEADING) { // compare as nearest int + break; + case HEADING_TYPE_HEADING: + // normal vehicle heading plane.guided_state.target_heading_type = GUIDED_HEADING_HEADING; - } else { + break; + case HEADING_TYPE_DEFAULT: + plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; + return MAV_RESULT_ACCEPTED; + default: // MAV_RESULT_DENIED means Command is invalid (is supported but has invalid parameters). return MAV_RESULT_DENIED; } @@ -1530,3 +1546,99 @@ MAV_LANDED_STATE GCS_MAVLINK_Plane::landed_state() const return MAV_LANDED_STATE_ON_GROUND; } +// Send the mode with the given index (not mode number!) return the total number of modes +// Index starts at 1 +uint8_t GCS_MAVLINK_Plane::send_available_mode(uint8_t index) const +{ + // Fixed wing modes + const Mode* fw_modes[] { + &plane.mode_manual, + &plane.mode_circle, + &plane.mode_stabilize, + &plane.mode_training, + &plane.mode_acro, + &plane.mode_fbwa, + &plane.mode_fbwb, + &plane.mode_cruise, + &plane.mode_autotune, + &plane.mode_auto, + &plane.mode_rtl, + &plane.mode_loiter, +#if HAL_ADSB_ENABLED + &plane.mode_avoidADSB, +#endif + &plane.mode_guided, + &plane.mode_initializing, + &plane.mode_takeoff, +#if HAL_SOARING_ENABLED + &plane.mode_thermal, +#endif + }; + + const uint8_t fw_mode_count = ARRAY_SIZE(fw_modes); + + // Fixedwing modes are always present + uint8_t mode_count = fw_mode_count; + +#if HAL_QUADPLANE_ENABLED + // Quadplane modes + const Mode* q_modes[] { + &plane.mode_qstabilize, + &plane.mode_qhover, + &plane.mode_qloiter, + &plane.mode_qland, + &plane.mode_qrtl, + &plane.mode_qacro, + &plane.mode_loiter_qland, +#if QAUTOTUNE_ENABLED + &plane.mode_qautotune, +#endif + }; + + // Quadplane modes must be enabled + if (plane.quadplane.available()) { + mode_count += ARRAY_SIZE(q_modes); + } +#endif // HAL_QUADPLANE_ENABLED + + + // Convert to zero indexed + const uint8_t index_zero = index - 1; + if (index_zero >= mode_count) { + // Mode does not exist!? + return mode_count; + } + + // Ask the mode for its name and number + const char* name; + uint8_t mode_number; + + if (index_zero < fw_mode_count) { + // A fixedwing mode + name = fw_modes[index_zero]->name(); + mode_number = (uint8_t)fw_modes[index_zero]->mode_number(); + + } else { +#if HAL_QUADPLANE_ENABLED + // A Quadplane mode + const uint8_t q_index = index_zero - fw_mode_count; + name = q_modes[q_index]->name(); + mode_number = (uint8_t)q_modes[q_index]->mode_number(); +#else + // Should not endup here + return mode_count; +#endif + } + + mavlink_msg_available_modes_send( + chan, + mode_count, + index, + MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD, + mode_number, + 0, // MAV_MODE_PROPERTY bitmask + name + ); + + return mode_count; +} diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 9e03940882713..2fa88274e1655 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -50,6 +50,10 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK void handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow) override; void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override; + // Send the mode with the given index (not mode number!) return the total number of modes + // Index starts at 1 + uint8_t send_available_mode(uint8_t index) const override; + private: void send_pid_info(const struct AP_PIDInfo *pid_info, const uint8_t axis, const float achieved); diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index d643d8b013754..5f746640e16e2 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -60,7 +60,7 @@ const AP_Param::Info Plane::var_info[] = { // @DisplayName: GCS PID tuning mask // @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for // @User: Advanced - // @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:Steering,4:Landing + // @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:Steering,4:Landing,5:AccZ GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0), // @Param: KFF_RDDRMIX @@ -158,6 +158,15 @@ const AP_Param::Info Plane::var_info[] = { // @User: Standard ASCALAR(takeoff_throttle_min, "TKOFF_THR_MIN", 0), + // @Param: TKOFF_THR_IDLE + // @DisplayName: Takeoff idle throttle + // @Description: The idle throttle to hold after arming and before a takeoff. Applicable in TAKEOFF and AUTO modes. + // @Units: % + // @Range: 0 100 + // @Increment: 1 + // @User: Standard + ASCALAR(takeoff_throttle_idle, "TKOFF_THR_IDLE", 0), + // @Param: TKOFF_OPTIONS // @DisplayName: Takeoff options // @Description: This selects the mode of the takeoff in AUTO and TAKEOFF flight modes. diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 377eb9ded865f..24645b3b88471 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -359,6 +359,7 @@ class Parameters { k_param_autotune_options, k_param_takeoff_throttle_min, k_param_takeoff_options, + k_param_takeoff_throttle_idle, k_param_pullup = 270, }; @@ -483,6 +484,9 @@ class ParametersG2 { // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; + // just to make compilation easier when all things are compiled out... + uint8_t unused_integer; + // button reporting library #if HAL_BUTTON_ENABLED AP_Button *button_ptr; @@ -579,9 +583,6 @@ class ParametersG2 { AP_Int8 axis_bitmask; // axes to be autotuned - // just to make compilation easier when all things are compiled out... - uint8_t unused_integer; - #if AP_RANGEFINDER_ENABLED // orientation of rangefinder to use for landing AP_Int8 rangefinder_land_orient; diff --git a/ArduPlane/ReleaseNotes.txt b/ArduPlane/ReleaseNotes.txt index e8699fa5c9320..ed28f55e6d524 100644 --- a/ArduPlane/ReleaseNotes.txt +++ b/ArduPlane/ReleaseNotes.txt @@ -1,5 +1,429 @@ ArduPilot Plane Release Notes: ------------------------------------------------------------------ +Release 4.6.0-beta 13 Nov 2024 + +Changes from 4.5.7 + +1) Board specific changes + +- AnyLeaf H7 supports compass and onboard logging +- Blitz743Pro supports CAN +- BlueRobotics Navigator supports BMP390 baro +- Bootloader ECC failure check fixed on boards with >128k bootloader space (e.g CubeRed) +- CB Unmanned Stamp H743 support +- ClearSky CSKY405 support +- CUAV-7-Nano default batt monitor fixed +- CubeRed bootloader fixes including disabling 2nd core by default +- CubeRed supports PPP networking between primary and secondary MCU +- CubeRedPrimary supports external compasses +- ESP32 main loop rate improvements +- ESP32 RC input fixes and wifi connection reliability improved +- ESP32 safety switch and GPIO pin support +- FlyingMoon no longer support MAX7456 +- Flywoo F405HD-AIOv2 ELRS RX pin pulled high during boot +- Flywoo H743 Pro support +- Flywoo/Goku F405 HD 1-2S ELRS AIO v2 +- FlywooF745 supports DPS310 baro +- FPV boards lose SMBus battery support (to save flash) +- GEPRC F745BTHD support +- GEPRCF745BTHD loses parachute support, non-BMP280 baros (to save flash) +- Here4FC bootloader fix for mismatch between RAM0 and periph that could prevent firmware updates +- Holybro Kakute F4 Wing support +- iFlight 2RAW H743 supports onboard logging +- JFB110 supports measuring servo rail voltage +- JFB110 supports safety switch LED +- JHEM-JHEF405 bootloader supports firmware updates via serial +- JHEMCU GF30H743 HD support +- JHEMCU-GF16-F405 autopilot support +- JHEMCU-GSF405A becomes FPV board (to save flash) +- KakuteF7 only supports BMP280 baro (to save flash) +- KakuteH7Mini supports ICM42688 IMU +- Linux auto detection of GPS baud rate fixed +- Linux board scheduler jitter reduced +- Linux board shutdown fixes +- MakeFlyEasy PixPilot-V6Pro support +- MatekF405, Pixhawk1-1M-bdshot, revo-mini loses blended GPS (to save flash) +- MatekH7A3 support Bi-directional DShot +- MicoAir405v2 and MicoAir405Mini support optical flow and OSD +- MicoAir743 internal compass orientation fixed +- MicroAir405Mini, MicroAir743, NxtPX4v2 support +- MicroAir405v2 Bi-directional DShot and LED DMA fixes +- MicroAir405v2 defined as FPV board with reduced features (to save flash) +- ModalAI VOXL2 support including Starling 2 and Starling 2 max +- mRo Control Zero Classic supports servo rail analog input +- mRo KitCAN revC fixed +- Mugin MUPilot support +- OmnibusF7v2 loses quadplane support (to save flash) +- Pixhack-v3 board added (same as fmuv3) +- Pixhawk6C bootloader supports flashing firmware from SD card +- RadiolinkPIX6 imu orientation fixed +- RadiolinkPIX6 supports SPA06 baro +- ReaperF745 V4 FC supports MPU6000 IMU +- RPI5 support +- SDModelH7v2 SERIAL3/7/9_PROTOCOL param defaults changed +- Solo serial ports default to MAVLink1 +- SpeedyBeeF405Wing gets Bi-directional DShot +- SpeedyBeeF405WING loses landing gear support, some camera gimbals (to save flash) +- Spektreworks boom board support +- TrueNavPro-G4 SPI does not share DMA +- X-MAV AP-H743v2 support + +2) AHRS/EKF enhancements and fixes + +- AHRS_OPTION to disable fallback to DCM (affects Plane and Rover, Copter never falls back) +- AHRS_OPTION to disable innovation check for airspeed sensor +- Airspeed sensor health check fixed when using multiple sensors and AHRS affinity +- DCM support for MAV_CMD_EXTERNAL_WIND_ESTIMATE (Plane only) +- EK2 supports disabling external nav (see EK2_OPTIONS) +- EK3 External Nav position jump after switch from Optical flow removed (see EK3_SRC_OPTION=1) +- EK3 uses filtered velocity corrections for IMU position +- EKF2, EKF3, ExternalAHRS all use common origin +- EKF3 accepts set origin even when using GPS +- EKF3 allows earth-frame fields to be estimated with an origin but no GPS +- EKF3 copes better with GPS jamming +- EKF3 logs mag fusion selection to XKFS +- EKF3 wind estimation when using GPS-for-yaw fixed +- External AHRS improvements including handling variances, pre-arm origin check +- Inertial Labs External AHRS fixes +- VectorNav driver fix for handling of error from sensor +- VectorNav External AHRS enhancements including validation of config commands and logging improvements +- VectorNav support for sensors outside VN-100 and VN-300 + +3) Driver enhancements and bug fixes + +- ADSB fix to display last character in status text sent to GCS +- Ainstein LR-D1 radar support +- Airspeed ASP5033 whoami check fixed when autopilot rebooted independently of the sensor +- AIRSPEED message sent to GCS +- Analog temperature sensor extended to 5th order polynomial (see TEMP_A5) +- ARSPD_OPTIONS to report calibration offset to GCS +- Baro EAS2TAS conversions continuously calculated reducing shocks to TECS (Plane only) +- Baro provides improved atmospheric model for high altitude flight +- BARO_ALT_OFFSET slew slowed to keep EKF happy +- BATTx_ESC_MASK param supports flexible mapping of ESCs to batteries +- BATTx_OPTION to not send battery voltage, current, etc to GCS +- Benewake RDS02U radar support +- Bi-directional DShot on IOMCU supports reversible mask +- Bi-directional DShot telemetry support on F103 8Mhz IOMCUs +- BMM350 compass support +- CAN rangefinders and proximity sensors may share a CAN bus (allows NRA24 and MR72 on a single CAN bus) +- Compass calibration world magnetic model checks can use any position source (e.g. not just GPS) +- CRSF baro and vertical speeed fixed +- CRSF RX bind command support +- DroneCAN battery monitor check to avoid memory corruption when type changed +- DroneCAN DNA server fixes including removing use of invalid node IDs, faster ID allocation, elimination of rare inability to save info +- DroneCAN EFI health check fix +- DroneCAN ESC battery monitors calculate consumed mah +- DroneCAN ESCs forced to zero when disarmed +- DroneCAN RPM message support +- DroneCAN timeout fix for auxiliary frames +- DroneCAN to serial tunneling params accepts short-hand baud rates (e.g. '57' for '57600') +- F9P, F10-N and Zed-F9P support for GPSx_GNSS_MODE to turn on/off SBAS, Galileo, Beidou and Glonass +- FuelLevel battery monitor fix to report capacity +- GPS_xxx params renamed to GPS1_xxx, GPS_xxx2 renamed to GPS2_xxx +- Hirth EFI logging includes modified throttle +- Hirth ICEngine supports reversed crank direction (see ICE_OPTIONS parameter) +- Hott and LTM telemetry deprecated (still available through custom build server) +- i-BUS telemetry support +- ICE_PWM_IGN_ON, ICE_PWM_IGN_OFF, ICE_PWM_STRT_ON, ICE_PWM_STRT_OFF replaced with SERVOx_MIN/MAX/REVERSED (Plane only) +- ICE_START_CHAN replaced with RC aux function (Plane only) +- ICEngine retry max added (see ICE_STRT_MX_RTRY) +- IE 2400 generator error message severity to GCS improved +- INA2xx battery monitor support (reads temp, gets MAX_AMPS and SHUNT params) +- MCP9600 temperature sensor I2C address fixed +- MLX90614 temperature sensor support +- MSP GPS ground course scaling fixed +- MSP RC uses scaled RC inputs (fixes issue with RCx_REVERSED having no effect) +- Networking supports reconnection to TCP server or client +- OSD params for adjusting horizontal spacing and vertical extension (see OSD_SB_H_OFS, OSD_SB_V_EXT) +- Relay inverted output support (see RELAYx_INVERTED parameter) +- ROMFS efficiency improvements +- RS-485 driver enable RTS flow control +- Sagetech MXS ADSP altitude fix (now uses abs alt instead of terrain alt) +- Septentrio GPS sat count correctly drops to zero when 255 received +- Septentrio supports selecting constellations (see GPS_GNSS_MODE) +- Single LED for user notification supported +- SPA06 baro supported +- Sum battery monitor optionally reports minimum voltage instead of average +- Sum battery monitor reports average temp +- Torqeedo dual motor support (see TRQ1, TRQ2 params) +- Ublox GPS driver uses 64 bit time for PPS interrupt (avoids possible dropout at 1hour and 12 minutes) +- UBlox GPS time ignored until at least 2D fix +- VideoTX supports additional freq bands (RushFPV 3.3Ghz) +- Volz logs desired and actual position, voltage, current, motor and PCB temp +- Volz server feedback and logging fixed +- Volz servo output in its own thread resulting in smoother movements +- W25N02KV flash support + +4) Networking enhancements and fixes + +- Allow multiple UDP clients to connect/disconnect/reconnect +- Ethernet supports faster log downloading (raised SDMMC clock limit on H7) + +5) Camera and gimbal enhancements + +- Alexmos precision improved slightly +- CAMERA_CAPTURE_STATUS mavlink msg sent to GCS (reports when images taken or video recorded, used by QGC) +- CAMERA_FOV_STATUS mavlink reports lat,lon of what camera is pointing at +- DO_MOUNT_CONTROL yaw angle interpreted as body-frame (was incorrectly being interpreted as earth-frame) +- Dual serial camera gimbal mounts fixed +- Lua script bindings to send CAMERA_INFORMATION and VIDEO_STREAM_INFORMATION messages to GCS +- Retract Mount2 aux function added (see RCx_OPTION = 113) +- Servo gimbal reported angles respect roll, pitch and yaw limits +- Siyi driver sends autopilot location and speed (recorded in images via EXIF) +- Siyi picture and video download scripts +- Siyi ZT6 and ZT30 support sending min, max temperature (see CAMERA_THERMAL_RANGE msg) +- Siyi ZT6 and ZT30 thermal palette can be changed using camera-change-setting.lua script +- Siyi ZT6 hardware id and set-lens fixed +- Topotek gimbal support +- Trigger distance ignores GPS status and only uses EKF reported location + +6) Harmonic notch enhancements + +- Harmonic notch is active in forward flight on quadplanes +- Harmonic notch filter freq clamping and disabling reworked +- Harmonic notch handles negative ESC RPMs +- Harmonic notch supports per-motor throttle-based harmonic notch + +7) Copter specific enhancements and bug fixes + +- Attitude control fix to dt update order (reduces rate controller noise) +- Auto mode fix to avoid prematurely advancing to next waypoint if given enough time +- Auto mode small target position jump when takeoff completes removed +- Auto mode yaw drift when passing WP removed if CONDITION_YAW command used and WP_YAW_BEHAVIOR = 0/None +- Auto, Guided flight mode pause RC aux switch (see RCx_OPTION = 178) +- AutoRTL (e.g. DO_LAND_START) uses copter stopping point to decide nearest mission command +- AutoRTL mode supports DO_RETURN_PATH_START (Copter, Heli only) +- AutoTune fix to prevent spool up after landing +- AutoTune performance and safety enhancements (less chance of ESC desync, fails when vehicle likely can't be tuned well) +- Autotune test gains RC aux switch function allows testing gains in any mode (see RCx_OPTION = 180) +- Config error avoided if auto mode is paused very soon after poweron +- FLIGHT_OPTIONS bit added to require position estimate before arming +- Follow mode slowdown calcs fixed when target is moving +- Ground oscillation suppressed by reducing gains (see ATC_LAND_R/P/Y_MULT) +- Guided mode internal error fix when taking off using SET_ATTITUDE_CONTROL message +- Guided mode internal error resolved when switching between thrust or climb rate based altitude control +- Guided mode yaw fixed when WP_YAW_BEHAVIOR = 0/None and CONDITION_YAW command received containing relative angle +- Landing detector fixed when in stabilize mode at full throttle but aircraft is upside down +- Landing detector logging added to ease support (see LDET message) +- Loiter unlimited command accepts NaNs (QGC sends these) +- Mavlink SYSTEM_STATUS set to BOOT during initialisation +- MOT_PWM_TYPE of 9 (PWMAngle) respects SERVOx_MIN/MAX/TRIM/REVERSED param values +- Payload place bug fix when aborted because gripper is already released +- RC based tuning (aka CH6 tuning) can use any RC aux function channel (see RCx_OPTION = 219) +- RTL_ALT minimum reduced to 30cm +- SystemID position controller support (Copter and Heli) +- TriCopter motor test and slew-up fixed +- WPNAV_SPEED min reduced to 10 cm/s (Copter only) +- Loiter mode zeros desired accel when re-entering from Auto during RC failsafe + +8) TradHeli specific enhancements + +- Autorotation yaw behaviour fix +- Autotune improvements including using low frequency dwell for feedforward gain tuning and conducting sweep in attitude instead of rate +- Blade pitch angle logging added (see SWSH log message) +- Constrain cyclic roll for intermeshing +- ICE / turbine cool down fix +- Inverted flight extended to non manual throttle modes +- Inverted flight transitions smoothed and restricted to only Stabilize mode +- SWSH logging fix for reversed collectives + +9) Plane specific enhancements and bug fixes + +- AIRSPEED_STALL holds vehicle stall speed and is used for minimum speed scaling +- Allow for different orientations of landing rangefinder +- Assistance requirements evaluted on mode change +- FBWB/CRUISE climb/sink rate limited by TECS limits +- FLIGHT_OPTION to immediately climb in AUTO mode (not doing glide slope) +- Glider pullup support (available only through custom build server) +- Loiter breakout improved to better handle destinations inside loiter circle +- Manual mode throttle made consistent with other modes (e.g battery comp and watt limit is done if enabled) +- Mavlink GUIDED_CHANGE_ALTITUDE supports terrain altitudes +- Minimum throttle not applied during SLT VTOL airbrake (reduces increase in airspeed and alt during back transition) +- Q_APPROACH_DIST set minimum distance to use the fixed wing approach logic +- Quadplane assistance check enhancements +- Quadplane Deca frame support +- Quadplane gets smoother takeoff by input shaping target accel and velocity +- Servo wiggles in altitude wait staged to be one after another +- Set_position_target_global_int accepts MAV_FRAME_GLOBAL_RELATIVE_ALT and MAV_FRAME_GLOBAL_TERRAIN_ALT frames +- Takeoff airspeed control improved (see TKOFF_MODE, TKOFF_THR_MIN) +- Takeoff fixes for fence autoenable +- Takeoff improvements including less overshoot of TKOFF_ALT +- TECS reset along with other controllers (important if plane dropped from balloon) +- Tilt quadplane ramp of motors on back transition fixed +- Tiltrotor tilt angles logged +- TKOFF_THR_MIN applied to SLT transitions +- Twin motor planes with DroneCAN ESCs fix to avoid max throttle at arming due to misconfiguration +- VTOLs switch to QLAND if a LONG_FAILSAFE is triggered during takeoff + +10) Rover specific enhancements and bug fixes + +- Auto mode reversed state maintained if momentarily switched to Hold mode +- Circle mode tracks better and avoids getting stuck at circle edge +- Flight time stats fixed +- MAV_CMD_NAV_SET_YAW_SPEED deprecated +- Omni3Mecanum frame support +- Stopping point uses max deceleration (was incorrectly using acceleration) +- Wheel rate controller slew rate fix + +11) Antenna Tracker specific enhancements and bug fixes + +- Never track lat,lon of 0,0 + +12) Scripting enhancements + +- advance-wp.lua applet supports advancing Auto mode WP via RC switch +- AHRS_switch.lua supports switching between EKF2 and EKF3 via RC switch +- battery_internal_resistance_check.lua monitors battery resistance +- CAN:get_device returns nil for unconfigured CAN device +- copter_terrain_brake.lua script added to prevent impact with terrain in Loiter mode (Copter only) +- Copter:get_target_location, update_target_location support +- crosstrack_restore.lua example allows returning to previous track in Auto (Plane only) +- Display text on OLED display supported +- Docs improved for many bindings +- EFI get_last_update_ms binding +- EFI_SkyPower.lua driver accepts 2nd supply voltage +- ESC_slew_rate.lua example script supports testing ESCs +- Filesystem CRC32 check to allow scripts to check module versions +- forced arming support +- GPIO get/set mode bindings (see gpio:set_mode, get_mode) +- GPS-for-yaw angle binding (see gps:gps_yaw_deg) +- Halo6000 EFI driver can log all CAN packets for easier debugging +- handle_external_position_estimate binding allows sending position estimate from lua to EKF +- I2C:transfer support +- IMU gyros_consistent and accels_consistent bindings added +- INF_Inject.lua driver more robust to serial errors, improved logging, throttle and ignition control +- INS bindings for is calibrating, gyro and accel sensor values +- IPV4 address bindings (see SocketAPM_ipv4_addr_to_string) to allow UDP server that responds to individual clients +- Logging of booleans supported +- Lua language checks improved (finds more errors) +- MAVLink commands can be called from scripting +- MCU voltage binding (see analog:mcu_voltage) +- NMEA 2000 EFI driver (see EFI_NMEA2k.lua) +- "open directory failed" false warning when scripts in ROMFS fixed +- Param_Controller.lua supports quickly switching between 3 parameter sets via RC switch +- Pass by reference values are always initialized +- pelco_d_antennatracker.lua applet supports sending Pelco-D via RS-485 to PTZ cameras +- plane_aerobatics.lua minor enhancements +- REPL applet (read-evaluate-print-loop, see repl.lua) for interactive testing and experimentation +- "require" function failures in rare circumstances fixed +- "require" function works for modules in ROMFS (e.g. not on SD card) +- revert_param.lua supports more params (e.g ATC_RATE_R/P/Y, PTCH2SRV_TCONST, RLL2SRV_TCONST, TECS_) +- Scripts may receive mavlink messages which fail CRC (e.g messages which FC does not understand) +- SD card formatting supported +- Serial device simulation support (allows Lua to feed data to any supported serial protocol for e.g. sensor simulation) +- set_target_throttle_rate_rpy allows rate control from scripts (new for Copter) +- sitl_standby_sim.lua example shows how to switch between redundant flight controllers using an RC switch +- Slung payload oscillation suppression applet added (see copter-slung-payload.lua) +- Temperature sensor bindings added +- uint64 support +- Various performance and memory usage optimizations +- VTOL-quicktune.lua minor enhancements including increasing YAW_FLTE_MAX to 8 +- x-quad-cg-allocation.lua applet corrects for the CoM discrepancy in a quad-X frame + +13) GCS / mavlink related changes and fixes + +- BATTERY2 message deprecated (replaced by BATTERY_STATUS) +- CMD_MISSION_START/STOP rejected if first-item/last-item args provided +- Deny attempts to upload two missions simultaneously +- Fence and Rally points may be uploaded using FTP +- GPS_INPUT and HIL_GPS handles multiple GPSs +- HIGHRES_IMU mavlink message support (used by companion computers to receive IMU data from FC) +- MAV_CMD_COMPONENT_ARM_DISARM accepts force arm magic value of 21196 +- MAV_CMD_DO_SET_SAFETY_SWITCH_STATE support +- MAV_CMD_SET_HAGL support (Plane only) +- MAVFTP respects TX buffer flow control to improve FTP on low bandwidth links +- MAVLink receiver support (RADIO_RC_CHANNELS mavlink message) +- Message interval supports TERRAIN_REPORT msg +- Mission upload may be cancelled using MISSION_CLEAR_ALL +- MOUNT_CONFIGURE, MOUNT_CONTROL messages deprecated +- RC_CHANNELS_RAW deprecated +- Serial passthrough supports parity allowing STM32CubeProgrammer to be used to program FrSky R9 receivers +- SET_ATTITUDE_TARGET angular rate input frame fixed (Copter only) +- TIMESYNC and NAMED_VALUE_FLOAT messages not sent on high latency MAVLink ports + +14) Logging enhancements and fixes + +- AC_PID resets and I-term sets logged +- ANG provides attitude at high rate (equivalent to ATT) +- ATT logs angles as floats (better resolution than ints) +- CAND message gets driver index +- DCM log message includes roll/pitch and yaw error +- EDT2 log msg includes stress and status received via extended DShot Telemetry v2 +- EFI ECYL cylinder head and exhaust temp logs in degC (was Kelvin) +- ESCX log msg includes DroneCAN ESCs status, temp, duty cycle and power pct +- FMT messages written as required instead of all at beginning +- Logging restarted after download completes when LOG_DISARMED = 1 +- MISE msg logs active mission command (CMD logged when commands are uploaded) +- ORGN message logging fixed when set using SET_GPS_GLOBAL_ORIGIN +- RPM sensor logging gets instance field, quality and health fields +- Short filename support removed (e.g log1.BIN instead of 00000001.BIN) +- Temperature sensor logging option for only sensors with no source (see TEMP_LOG) +- UART data rates logged at 1hz (see UART message) + +15) ROS2 / DDS support + +- Airspeed published +- Battery topic reports all available batteries +- Compile-time configurable rates for each publisher +- DDS_TIMEOUT_MS and DDS_MAX_RETRY set timeout and num retries when client pings XRCE agent +- GPS global origin published at 1 Hz +- High frequency raw imu data transmission +- Joystick support +- Support sending waypoints to Copter and Rover +- Remove the XML refs file in favor of binary entity creation + +16) Safety related enhancements and fixes + +- Accel/Gyro inconsistent message fixed for using with only single IMU +- Battery monitor failure reported to GCS, triggers battery failsafe but does not take action +- Far from EKF origin pre-arm check removed (Copter only) +- Fence breach warning message slightly improved +- Fence enhancements incluiding alt min fence (Copter only, see FENCE_AUTOENABLE, FENCE_OPTIONS) +- Fences can be stored to SD Card (see BRD_SD_FENCE param) +- ICEngine stopped when in E-Stop or safety engaged (Plane only) +- LEDs flash green lights based on EKF location not GPS +- Parachute option to skip disarm before parachute release (see CHUTE_OPTIONS) +- Plane FENCE_AUTOENABLE of 1 or 2 deprecation warning added +- Pre-arm check if OpenDroneID is compiled in but disabled +- Pre-arm check of duplicate RC aux functions fixed (was skipping recently added aux functions) +- Pre-arm checks alert user more quickly on failure +- Prearm check for misconfigured EAHRS_SENSORS and GPS_TYPE +- Proximity sensor based avoidance keeps working even if one proximity sensor fails (Copter, Rover) +- RC aux functions for Arm, Disarm, E-Stop and Parachute ignored when RC is first turned on +- Warning of duplicate SERIALx_PROTOCOL = RCIN + +17) Other bug fixes and minor enhancements + +- Accel cal fixed for auxiliary IMUs (e.g. IMU4 and higher) +- Bootloader fix to reduce unnecessary mass erasing of flash when using STLink or Jlink tools +- Bootloader rejects allocation of broadcast node ID +- CAN forward registering/de-registering fix (affected Mission Planner DroneCAN UI) +- Dijkstras fix to correct use of uninitialised variable +- DShot rates are not limited by NeoPixel rates +- Ethernet and CAN bootloader fix to prevent getting stuck in bootloader mode +- Filesystem does not show entries for empty @ files +- Filesystem efficiency improvements when reading files +- Flight statistics only reset if user sets STAT_RESET to zero (avoids accidental reset) +- Flight time statistics updated on disarm (avoids issue if vehicle powered-down soon after disarm) +- Internal error thrown if we lose parameters due to saving queue being too small +- MAVLink via DroneCAN baud rate fix +- SPI pins may also be used as GPIOs +- Terrain cache size configurable (see TERRAIN_CACHE_SZ) + +18) Developer focused fixes and enhancements + +- AP_Camera gets example python code showing how to use GStreamer with UDP and RTSP video streams +- Cygwin build fix for non-SITL builds +- Cygwin build fixed with malloc wrap +- DroneCAN and scripting support FlexDebug messages (see CAN_Dn_UC_OPTION, FlexDebug.lua) +- EKF3 code generator documentation and cleanup +- GPS jamming simulator added +- MacOS compiler warnings reduced +- SFML joystick support +- SITL support for OpenBSD +- Text warning if older Fence or Rally point protocols are used +------------------------------------------------------------------ Release 4.5.7 08 Oct 2024 Changes from 4.5.7-beta1 diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index b2939557adfdf..a3da572611c77 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -64,18 +64,6 @@ enum class RtlAutoland { }; -enum ChannelMixing { - MIXING_DISABLED = 0, - MIXING_UPUP = 1, - MIXING_UPDN = 2, - MIXING_DNUP = 3, - MIXING_DNDN = 4, - MIXING_UPUP_SWP = 5, - MIXING_UPDN_SWP = 6, - MIXING_DNUP_SWP = 7, - MIXING_DNDN_SWP = 8, -}; - // PID broadcast bitmask enum tuning_pid_bits { TUNING_BITS_ROLL = (1 << 0), diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index e4f97acc2c089..8bece6d4187c3 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -1,14 +1,15 @@ #include "Plane.h" Mode::Mode() : - ahrs(plane.ahrs) + unused_integer{17}, #if HAL_QUADPLANE_ENABLED - , quadplane(plane.quadplane), pos_control(plane.quadplane.pos_control), attitude_control(plane.quadplane.attitude_control), loiter_nav(plane.quadplane.loiter_nav), - poscontrol(plane.quadplane.poscontrol) + quadplane(plane.quadplane), + poscontrol(plane.quadplane.poscontrol), #endif + ahrs(plane.ahrs) { } diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 230b372499ff4..8260924afca89 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -159,6 +159,9 @@ class Mode // Output pilot throttle, this is used in stabilized modes without auto throttle control void output_pilot_throttle(); + // makes the initialiser list in the constructor manageable + uint8_t unused_integer; + #if HAL_QUADPLANE_ENABLED // References for convenience, used by QModes AC_PosControl*& pos_control; diff --git a/ArduPlane/mode_qland.cpp b/ArduPlane/mode_qland.cpp index f3a08aabde762..e59e3c9a50e95 100644 --- a/ArduPlane/mode_qland.cpp +++ b/ArduPlane/mode_qland.cpp @@ -9,7 +9,6 @@ bool ModeQLand::_enter() quadplane.throttle_wait = false; quadplane.setup_target_position(); poscontrol.set_state(QuadPlane::QPOS_LAND_DESCEND); - poscontrol.pilot_correction_done = false; quadplane.last_land_final_agl = plane.relative_ground_altitude(plane.g.rangefinder_landing); quadplane.landing_detect.lower_limit_start_ms = 0; quadplane.landing_detect.land_start_ms = 0; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index e7fc346758491..87be3e1160436 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -4596,6 +4596,11 @@ void QuadPlane::mode_enter(void) poscontrol.last_velocity_match_ms = 0; poscontrol.set_state(QuadPlane::QPOS_NONE); + // Clear any pilot corrections + poscontrol.pilot_correction_done = false; + poscontrol.pilot_correction_active = false; + poscontrol.target_vel_cms.zero(); + // clear guided takeoff wait on any mode change, but remember the // state for special behaviour guided_wait_takeoff_on_mode_enter = guided_wait_takeoff; diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index cb2048be6d42f..49835adcfdf66 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -107,7 +107,7 @@ void Plane::init_rc_out_main() */ void Plane::init_rc_out_aux() { - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); servos_output(); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 49e3e07b460fc..aa3de533d3cf6 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -619,6 +619,11 @@ void Plane::set_throttle(void) // throttle is suppressed (above) to zero in final flare in auto mode, but we allow instead thr_min if user prefers, eg turbines: SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, aparm.throttle_min.get()); + } else if ((flight_stage == AP_FixedWing::FlightStage::TAKEOFF) + && (aparm.takeoff_throttle_idle.get() > 0) + ) { + // we want to spin at idle throttle before the takeoff conditions are met + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, aparm.takeoff_throttle_idle.get()); } else { // default SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); @@ -853,8 +858,8 @@ void Plane::set_servos(void) // start with output corked. the cork is released when we run // servos_output(), which is run from all code paths in this // function - SRV_Channels::cork(); - + AP::srv().cork(); + // this is to allow the failsafe module to deliberately crash // the plane. Only used in extreme circumstances to meet the // OBC rules @@ -1012,7 +1017,8 @@ void Plane::indicate_waiting_for_rud_neutral_to_takeoff(void) */ void Plane::servos_output(void) { - SRV_Channels::cork(); + auto &srv = AP::srv(); + srv.cork(); // support twin-engine aircraft servos_twin_engine_mix(); @@ -1050,7 +1056,7 @@ void Plane::servos_output(void) SRV_Channels::output_ch_all(); - SRV_Channels::push(); + srv.push(); if (g2.servo_channels.auto_trim_enabled()) { servos_auto_trim(); diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index df4729ba01692..002fbfbaad591 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -290,7 +290,7 @@ void Sub::one_hz_loop() } // update assigned functions and enable auxiliary servos - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); #if HAL_LOGGING_ENABLED // log terrain data diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 0d0bad47ebd89..f17901638d76a 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -369,10 +369,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif MSG_NAV_CONTROLLER_OUTPUT, @@ -437,7 +443,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { #endif }; static const ap_message STREAM_PARAMS_msgs[] = { - MSG_NEXT_PARAM + MSG_NEXT_PARAM, + MSG_AVAILABLE_MODES }; const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { @@ -962,3 +969,47 @@ uint8_t GCS_MAVLINK_Sub::high_latency_tgt_airspeed() const return 0; } #endif // HAL_HIGH_LATENCY2_ENABLED + +// Send the mode with the given index (not mode number!) return the total number of modes +// Index starts at 1 +uint8_t GCS_MAVLINK_Sub::send_available_mode(uint8_t index) const +{ + const Mode* modes[] { + &sub.mode_manual, + &sub.mode_stabilize, + &sub.mode_acro, + &sub.mode_althold, + &sub.mode_surftrak, + &sub.mode_poshold, + &sub.mode_auto, + &sub.mode_guided, + &sub.mode_circle, + &sub.mode_surface, + &sub.mode_motordetect, + }; + + const uint8_t mode_count = ARRAY_SIZE(modes); + + // Convert to zero indexed + const uint8_t index_zero = index - 1; + if (index_zero >= mode_count) { + // Mode does not exist!? + return mode_count; + } + + // Ask the mode for its name and number + const char* name = modes[index_zero]->name(); + const uint8_t mode_number = (uint8_t)modes[index_zero]->number(); + + mavlink_msg_available_modes_send( + chan, + mode_count, + index, + MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD, + mode_number, + 0, // MAV_MODE_PROPERTY bitmask + name + ); + + return mode_count; +} diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index c38ec3f4abb6e..7dc2d0c1a2586 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -37,6 +37,10 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK { uint64_t capabilities() const override; + // Send the mode with the given index (not mode number!) return the total number of modes + // Index starts at 1 + uint8_t send_available_mode(uint8_t index) const override; + private: void handle_message(const mavlink_message_t &msg) override; diff --git a/ArduSub/mode.h b/ArduSub/mode.h index 11a6447167511..35780cb9c04d0 100644 --- a/ArduSub/mode.h +++ b/ArduSub/mode.h @@ -72,6 +72,9 @@ class Mode virtual const char *name() const = 0; virtual const char *name4() const = 0; + // returns a unique number specific to this mode + virtual Mode::Number number() const = 0; + // functions for reporting to GCS virtual bool get_wp(Location &loc) { return false; } virtual int32_t wp_bearing() const { return 0; } @@ -202,6 +205,7 @@ class ModeManual : public Mode const char *name() const override { return "MANUAL"; } const char *name4() const override { return "MANU"; } + Mode::Number number() const override { return Mode::Number::MANUAL; } }; @@ -224,6 +228,7 @@ class ModeAcro : public Mode const char *name() const override { return "ACRO"; } const char *name4() const override { return "ACRO"; } + Mode::Number number() const override { return Mode::Number::ACRO; } }; @@ -246,6 +251,7 @@ class ModeStabilize : public Mode const char *name() const override { return "STABILIZE"; } const char *name4() const override { return "STAB"; } + Mode::Number number() const override { return Mode::Number::STABILIZE; } }; @@ -272,6 +278,7 @@ class ModeAlthold : public Mode const char *name() const override { return "ALT_HOLD"; } const char *name4() const override { return "ALTH"; } + Mode::Number number() const override { return Mode::Number::ALT_HOLD; } }; @@ -293,6 +300,7 @@ class ModeSurftrak : public ModeAlthold const char *name() const override { return "SURFTRAK"; } const char *name4() const override { return "STRK"; } + Mode::Number number() const override { return Mode::Number::SURFTRAK; } private: @@ -342,6 +350,8 @@ class ModeGuided : public Mode const char *name() const override { return "GUIDED"; } const char *name4() const override { return "GUID"; } + Mode::Number number() const override { return Mode::Number::GUIDED; } + autopilot_yaw_mode get_default_auto_yaw_mode(bool rtl) const; private: @@ -387,6 +397,7 @@ class ModeAuto : public ModeGuided const char *name() const override { return "AUTO"; } const char *name4() const override { return "AUTO"; } + Mode::Number number() const override { return Mode::Number::AUTO; } private: void auto_wp_run(); @@ -417,6 +428,7 @@ class ModePoshold : public ModeAlthold const char *name() const override { return "POSHOLD"; } const char *name4() const override { return "POSH"; } + Mode::Number number() const override { return Mode::Number::POSHOLD; } }; @@ -439,6 +451,7 @@ class ModeCircle : public Mode const char *name() const override { return "CIRCLE"; } const char *name4() const override { return "CIRC"; } + Mode::Number number() const override { return Mode::Number::CIRCLE; } }; class ModeSurface : public Mode @@ -460,6 +473,7 @@ class ModeSurface : public Mode const char *name() const override { return "SURFACE"; } const char *name4() const override { return "SURF"; } + Mode::Number number() const override { return Mode::Number::CIRCLE; } }; @@ -482,4 +496,5 @@ class ModeMotordetect : public Mode const char *name() const override { return "MOTORDETECT"; } const char *name4() const override { return "DETE"; } + Mode::Number number() const override { return Mode::Number::MOTOR_DETECT; } }; diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 5d45a2c00fbe2..504c6651d933b 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -18,11 +18,12 @@ void Sub::motors_output() verify_motor_test(); } else { motors.set_interlock(true); - SRV_Channels::cork(); + auto &srv = AP::srv(); + srv.cork(); SRV_Channels::calc_pwm(); SRV_Channels::output_ch_all(); motors.output(); - SRV_Channels::push(); + srv.push(); } } diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index 83f8e6207394b..8ae8c0b704aa3 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -210,7 +210,7 @@ void Blimp::one_hz_loop() #endif // update assigned functions and enable auxiliary servos - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); AP_Notify::flags.flying = !ap.land_complete; diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index b132d80160a5c..1eb5985e81a1c 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -327,10 +327,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, // MISSION_CURRENT +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif MSG_NAV_CONTROLLER_OUTPUT, @@ -395,7 +401,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { #endif }; static const ap_message STREAM_PARAMS_msgs[] = { - MSG_NEXT_PARAM + MSG_NEXT_PARAM, + MSG_AVAILABLE_MODES }; static const ap_message STREAM_ADSB_msgs[] = { MSG_ADSB_VEHICLE, @@ -608,3 +615,41 @@ uint8_t GCS_MAVLINK_Blimp::high_latency_wind_direction() const return wrap_360(degrees(atan2f(-wind.y, -wind.x))) / 2; } #endif // HAL_HIGH_LATENCY2_ENABLED + +// Send the mode with the given index (not mode number!) return the total number of modes +// Index starts at 1 +uint8_t GCS_MAVLINK_Blimp::send_available_mode(uint8_t index) const +{ + const Mode* modes[] { + &blimp.mode_land, + &blimp.mode_manual, + &blimp.mode_velocity, + &blimp.mode_loiter, + &blimp.mode_rtl, + }; + + const uint8_t mode_count = ARRAY_SIZE(modes); + + // Convert to zero indexed + const uint8_t index_zero = index - 1; + if (index_zero >= mode_count) { + // Mode does not exist!? + return mode_count; + } + + // Ask the mode for its name and number + const char* name = modes[index_zero]->name(); + const uint8_t mode_number = (uint8_t)modes[index_zero]->number(); + + mavlink_msg_available_modes_send( + chan, + mode_count, + index, + MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD, + mode_number, + 0, // MAV_MODE_PROPERTY bitmask + name + ); + + return mode_count; +} diff --git a/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h index 35276b03a46d5..90c4881b2adf4 100644 --- a/Blimp/GCS_Mavlink.h +++ b/Blimp/GCS_Mavlink.h @@ -48,6 +48,10 @@ class GCS_MAVLINK_Blimp : public GCS_MAVLINK uint32_t log_radio_bit() const override { return MASK_LOG_PM; } #endif + // Send the mode with the given index (not mode number!) return the total number of modes + // Index starts at 1 + uint8_t send_available_mode(uint8_t index) const override; + private: void handle_message(const mavlink_message_t &msg) override; diff --git a/Blimp/mode.h b/Blimp/mode.h index 8a990fbb31b30..8718d47fa144e 100644 --- a/Blimp/mode.h +++ b/Blimp/mode.h @@ -52,6 +52,9 @@ class Mode virtual const char *name() const = 0; virtual const char *name4() const = 0; + // returns a unique number specific to this mode + virtual Mode::Number number() const = 0; + virtual bool is_landing() const { return false; @@ -159,6 +162,8 @@ class ModeManual : public Mode return "MANU"; } + Mode::Number number() const override { return Mode::Number::MANUAL; } + private: }; @@ -201,6 +206,8 @@ class ModeVelocity : public Mode return "VELY"; } + Mode::Number number() const override { return Mode::Number::VELOCITY; } + private: }; @@ -244,6 +251,8 @@ class ModeLoiter : public Mode return "LOIT"; } + Mode::Number number() const override { return Mode::Number::LOITER; } + private: Vector3f target_pos; float target_yaw; @@ -286,6 +295,8 @@ class ModeLand : public Mode return "LAND"; } + Mode::Number number() const override { return Mode::Number::LAND; } + private: }; @@ -328,4 +339,7 @@ class ModeRTL : public Mode { return "RTL"; } + + Mode::Number number() const override { return Mode::Number::RTL; } + }; diff --git a/Blimp/motors.cpp b/Blimp/motors.cpp index 35ba08b90cce9..90fb3308a925b 100644 --- a/Blimp/motors.cpp +++ b/Blimp/motors.cpp @@ -78,8 +78,10 @@ void Blimp::motors_output() // output any servo channels SRV_Channels::calc_pwm(); + auto &srv = AP::srv(); + // cork now, so that all channel outputs happen at once - SRV_Channels::cork(); + srv.cork(); // update output on any aux channels, for manual passthru SRV_Channels::output_ch_all(); @@ -88,5 +90,5 @@ void Blimp::motors_output() motors->output(); // push all channels - SRV_Channels::push(); -} \ No newline at end of file + srv.push(); +} diff --git a/Blimp/radio.cpp b/Blimp/radio.cpp index 4b0c5a9a709e2..47212c501272c 100644 --- a/Blimp/radio.cpp +++ b/Blimp/radio.cpp @@ -38,7 +38,7 @@ void Blimp::init_rc_in() void Blimp::init_rc_out() { // enable aux servos to cope with multiple output channels per motor - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); // refresh auxiliary channel to function map SRV_Channels::update_aux_servo_function(); diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index 6a39311eade23..a903e591c6e62 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -541,10 +541,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif MSG_NAV_CONTROLLER_OUTPUT, @@ -613,7 +619,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { #endif }; static const ap_message STREAM_PARAMS_msgs[] = { - MSG_NEXT_PARAM + MSG_NEXT_PARAM, + MSG_AVAILABLE_MODES }; static const ap_message STREAM_ADSB_msgs[] = { MSG_ADSB_VEHICLE, @@ -1148,3 +1155,54 @@ uint8_t GCS_MAVLINK_Rover::high_latency_wind_direction() const return 0; } #endif // HAL_HIGH_LATENCY2_ENABLED + +// Send the mode with the given index (not mode number!) return the total number of modes +// Index starts at 1 +uint8_t GCS_MAVLINK_Rover::send_available_mode(uint8_t index) const +{ + const Mode* modes[] { + &rover.mode_manual, + &rover.mode_acro, + &rover.mode_steering, + &rover.mode_hold, + &rover.mode_loiter, +#if MODE_FOLLOW_ENABLED + &rover.mode_follow, +#endif + &rover.mode_simple, + &rover.g2.mode_circle, + &rover.mode_auto, + &rover.mode_rtl, + &rover.mode_smartrtl, + &rover.mode_guided, + &rover.mode_initializing, +#if MODE_DOCK_ENABLED + (Mode *)rover.g2.mode_dock_ptr, +#endif + }; + + const uint8_t mode_count = ARRAY_SIZE(modes); + + // Convert to zero indexed + const uint8_t index_zero = index - 1; + if (index_zero >= mode_count) { + // Mode does not exist!? + return mode_count; + } + + // Ask the mode for its name and number + const char* name = modes[index_zero]->name4(); + const uint8_t mode_number = (uint8_t)modes[index_zero]->mode_number(); + + mavlink_msg_available_modes_send( + chan, + mode_count, + index, + MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD, + mode_number, + 0, // MAV_MODE_PROPERTY bitmask + name + ); + + return mode_count; +} diff --git a/Rover/GCS_Mavlink.h b/Rover/GCS_Mavlink.h index b39a8cfb3630e..fd92cac3d5cfa 100644 --- a/Rover/GCS_Mavlink.h +++ b/Rover/GCS_Mavlink.h @@ -40,6 +40,10 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK uint32_t log_radio_bit() const override { return MASK_LOG_PM; } #endif + // Send the mode with the given index (not mode number!) return the total number of modes + // Index starts at 1 + uint8_t send_available_mode(uint8_t index) const override; + private: void handle_message(const mavlink_message_t &msg) override; diff --git a/Rover/ReleaseNotes.txt b/Rover/ReleaseNotes.txt index 792f86077d3b0..9722121b97a89 100644 --- a/Rover/ReleaseNotes.txt +++ b/Rover/ReleaseNotes.txt @@ -1,5 +1,429 @@ Rover Release Notes: ------------------------------------------------------------------ +Release 4.6.0-beta 13 Nov 2024 + +Changes from 4.5.7 + +1) Board specific changes + +- AnyLeaf H7 supports compass and onboard logging +- Blitz743Pro supports CAN +- BlueRobotics Navigator supports BMP390 baro +- Bootloader ECC failure check fixed on boards with >128k bootloader space (e.g CubeRed) +- CB Unmanned Stamp H743 support +- ClearSky CSKY405 support +- CUAV-7-Nano default batt monitor fixed +- CubeRed bootloader fixes including disabling 2nd core by default +- CubeRed supports PPP networking between primary and secondary MCU +- CubeRedPrimary supports external compasses +- ESP32 main loop rate improvements +- ESP32 RC input fixes and wifi connection reliability improved +- ESP32 safety switch and GPIO pin support +- FlyingMoon no longer support MAX7456 +- Flywoo F405HD-AIOv2 ELRS RX pin pulled high during boot +- Flywoo H743 Pro support +- Flywoo/Goku F405 HD 1-2S ELRS AIO v2 +- FlywooF745 supports DPS310 baro +- FPV boards lose SMBus battery support (to save flash) +- GEPRC F745BTHD support +- GEPRCF745BTHD loses parachute support, non-BMP280 baros (to save flash) +- Here4FC bootloader fix for mismatch between RAM0 and periph that could prevent firmware updates +- Holybro Kakute F4 Wing support +- iFlight 2RAW H743 supports onboard logging +- JFB110 supports measuring servo rail voltage +- JFB110 supports safety switch LED +- JHEM-JHEF405 bootloader supports firmware updates via serial +- JHEMCU GF30H743 HD support +- JHEMCU-GF16-F405 autopilot support +- JHEMCU-GSF405A becomes FPV board (to save flash) +- KakuteF7 only supports BMP280 baro (to save flash) +- KakuteH7Mini supports ICM42688 IMU +- Linux auto detection of GPS baud rate fixed +- Linux board scheduler jitter reduced +- Linux board shutdown fixes +- MakeFlyEasy PixPilot-V6Pro support +- MatekF405, Pixhawk1-1M-bdshot, revo-mini loses blended GPS (to save flash) +- MatekH7A3 support Bi-directional DShot +- MicoAir405v2 and MicoAir405Mini support optical flow and OSD +- MicoAir743 internal compass orientation fixed +- MicroAir405Mini, MicroAir743, NxtPX4v2 support +- MicroAir405v2 Bi-directional DShot and LED DMA fixes +- MicroAir405v2 defined as FPV board with reduced features (to save flash) +- ModalAI VOXL2 support including Starling 2 and Starling 2 max +- mRo Control Zero Classic supports servo rail analog input +- mRo KitCAN revC fixed +- Mugin MUPilot support +- OmnibusF7v2 loses quadplane support (to save flash) +- Pixhack-v3 board added (same as fmuv3) +- Pixhawk6C bootloader supports flashing firmware from SD card +- RadiolinkPIX6 imu orientation fixed +- RadiolinkPIX6 supports SPA06 baro +- ReaperF745 V4 FC supports MPU6000 IMU +- RPI5 support +- SDModelH7v2 SERIAL3/7/9_PROTOCOL param defaults changed +- Solo serial ports default to MAVLink1 +- SpeedyBeeF405Wing gets Bi-directional DShot +- SpeedyBeeF405WING loses landing gear support, some camera gimbals (to save flash) +- Spektreworks boom board support +- TrueNavPro-G4 SPI does not share DMA +- X-MAV AP-H743v2 support + +2) AHRS/EKF enhancements and fixes + +- AHRS_OPTION to disable fallback to DCM (affects Plane and Rover, Copter never falls back) +- AHRS_OPTION to disable innovation check for airspeed sensor +- Airspeed sensor health check fixed when using multiple sensors and AHRS affinity +- DCM support for MAV_CMD_EXTERNAL_WIND_ESTIMATE (Plane only) +- EK2 supports disabling external nav (see EK2_OPTIONS) +- EK3 External Nav position jump after switch from Optical flow removed (see EK3_SRC_OPTION=1) +- EK3 uses filtered velocity corrections for IMU position +- EKF2, EKF3, ExternalAHRS all use common origin +- EKF3 accepts set origin even when using GPS +- EKF3 allows earth-frame fields to be estimated with an origin but no GPS +- EKF3 copes better with GPS jamming +- EKF3 logs mag fusion selection to XKFS +- EKF3 wind estimation when using GPS-for-yaw fixed +- External AHRS improvements including handling variances, pre-arm origin check +- Inertial Labs External AHRS fixes +- VectorNav driver fix for handling of error from sensor +- VectorNav External AHRS enhancements including validation of config commands and logging improvements +- VectorNav support for sensors outside VN-100 and VN-300 + +3) Driver enhancements and bug fixes + +- ADSB fix to display last character in status text sent to GCS +- Ainstein LR-D1 radar support +- Airspeed ASP5033 whoami check fixed when autopilot rebooted independently of the sensor +- AIRSPEED message sent to GCS +- Analog temperature sensor extended to 5th order polynomial (see TEMP_A5) +- ARSPD_OPTIONS to report calibration offset to GCS +- Baro EAS2TAS conversions continuously calculated reducing shocks to TECS (Plane only) +- Baro provides improved atmospheric model for high altitude flight +- BARO_ALT_OFFSET slew slowed to keep EKF happy +- BATTx_ESC_MASK param supports flexible mapping of ESCs to batteries +- BATTx_OPTION to not send battery voltage, current, etc to GCS +- Benewake RDS02U radar support +- Bi-directional DShot on IOMCU supports reversible mask +- Bi-directional DShot telemetry support on F103 8Mhz IOMCUs +- BMM350 compass support +- CAN rangefinders and proximity sensors may share a CAN bus (allows NRA24 and MR72 on a single CAN bus) +- Compass calibration world magnetic model checks can use any position source (e.g. not just GPS) +- CRSF baro and vertical speeed fixed +- CRSF RX bind command support +- DroneCAN battery monitor check to avoid memory corruption when type changed +- DroneCAN DNA server fixes including removing use of invalid node IDs, faster ID allocation, elimination of rare inability to save info +- DroneCAN EFI health check fix +- DroneCAN ESC battery monitors calculate consumed mah +- DroneCAN ESCs forced to zero when disarmed +- DroneCAN RPM message support +- DroneCAN timeout fix for auxiliary frames +- DroneCAN to serial tunneling params accepts short-hand baud rates (e.g. '57' for '57600') +- F9P, F10-N and Zed-F9P support for GPSx_GNSS_MODE to turn on/off SBAS, Galileo, Beidou and Glonass +- FuelLevel battery monitor fix to report capacity +- GPS_xxx params renamed to GPS1_xxx, GPS_xxx2 renamed to GPS2_xxx +- Hirth EFI logging includes modified throttle +- Hirth ICEngine supports reversed crank direction (see ICE_OPTIONS parameter) +- Hott and LTM telemetry deprecated (still available through custom build server) +- i-BUS telemetry support +- ICE_PWM_IGN_ON, ICE_PWM_IGN_OFF, ICE_PWM_STRT_ON, ICE_PWM_STRT_OFF replaced with SERVOx_MIN/MAX/REVERSED (Plane only) +- ICE_START_CHAN replaced with RC aux function (Plane only) +- ICEngine retry max added (see ICE_STRT_MX_RTRY) +- IE 2400 generator error message severity to GCS improved +- INA2xx battery monitor support (reads temp, gets MAX_AMPS and SHUNT params) +- MCP9600 temperature sensor I2C address fixed +- MLX90614 temperature sensor support +- MSP GPS ground course scaling fixed +- MSP RC uses scaled RC inputs (fixes issue with RCx_REVERSED having no effect) +- Networking supports reconnection to TCP server or client +- OSD params for adjusting horizontal spacing and vertical extension (see OSD_SB_H_OFS, OSD_SB_V_EXT) +- Relay inverted output support (see RELAYx_INVERTED parameter) +- ROMFS efficiency improvements +- RS-485 driver enable RTS flow control +- Sagetech MXS ADSP altitude fix (now uses abs alt instead of terrain alt) +- Septentrio GPS sat count correctly drops to zero when 255 received +- Septentrio supports selecting constellations (see GPS_GNSS_MODE) +- Single LED for user notification supported +- SPA06 baro supported +- Sum battery monitor optionally reports minimum voltage instead of average +- Sum battery monitor reports average temp +- Torqeedo dual motor support (see TRQ1, TRQ2 params) +- Ublox GPS driver uses 64 bit time for PPS interrupt (avoids possible dropout at 1hour and 12 minutes) +- UBlox GPS time ignored until at least 2D fix +- VideoTX supports additional freq bands (RushFPV 3.3Ghz) +- Volz logs desired and actual position, voltage, current, motor and PCB temp +- Volz server feedback and logging fixed +- Volz servo output in its own thread resulting in smoother movements +- W25N02KV flash support + +4) Networking enhancements and fixes + +- Allow multiple UDP clients to connect/disconnect/reconnect +- Ethernet supports faster log downloading (raised SDMMC clock limit on H7) + +5) Camera and gimbal enhancements + +- Alexmos precision improved slightly +- CAMERA_CAPTURE_STATUS mavlink msg sent to GCS (reports when images taken or video recorded, used by QGC) +- CAMERA_FOV_STATUS mavlink reports lat,lon of what camera is pointing at +- DO_MOUNT_CONTROL yaw angle interpreted as body-frame (was incorrectly being interpreted as earth-frame) +- Dual serial camera gimbal mounts fixed +- Lua script bindings to send CAMERA_INFORMATION and VIDEO_STREAM_INFORMATION messages to GCS +- Retract Mount2 aux function added (see RCx_OPTION = 113) +- Servo gimbal reported angles respect roll, pitch and yaw limits +- Siyi driver sends autopilot location and speed (recorded in images via EXIF) +- Siyi picture and video download scripts +- Siyi ZT6 and ZT30 support sending min, max temperature (see CAMERA_THERMAL_RANGE msg) +- Siyi ZT6 and ZT30 thermal palette can be changed using camera-change-setting.lua script +- Siyi ZT6 hardware id and set-lens fixed +- Topotek gimbal support +- Trigger distance ignores GPS status and only uses EKF reported location + +6) Harmonic notch enhancements + +- Harmonic notch is active in forward flight on quadplanes +- Harmonic notch filter freq clamping and disabling reworked +- Harmonic notch handles negative ESC RPMs +- Harmonic notch supports per-motor throttle-based harmonic notch + +7) Copter specific enhancements and bug fixes + +- Attitude control fix to dt update order (reduces rate controller noise) +- Auto mode fix to avoid prematurely advancing to next waypoint if given enough time +- Auto mode small target position jump when takeoff completes removed +- Auto mode yaw drift when passing WP removed if CONDITION_YAW command used and WP_YAW_BEHAVIOR = 0/None +- Auto, Guided flight mode pause RC aux switch (see RCx_OPTION = 178) +- AutoRTL (e.g. DO_LAND_START) uses copter stopping point to decide nearest mission command +- AutoRTL mode supports DO_RETURN_PATH_START (Copter, Heli only) +- AutoTune fix to prevent spool up after landing +- AutoTune performance and safety enhancements (less chance of ESC desync, fails when vehicle likely can't be tuned well) +- Autotune test gains RC aux switch function allows testing gains in any mode (see RCx_OPTION = 180) +- Config error avoided if auto mode is paused very soon after poweron +- FLIGHT_OPTIONS bit added to require position estimate before arming +- Follow mode slowdown calcs fixed when target is moving +- Ground oscillation suppressed by reducing gains (see ATC_LAND_R/P/Y_MULT) +- Guided mode internal error fix when taking off using SET_ATTITUDE_CONTROL message +- Guided mode internal error resolved when switching between thrust or climb rate based altitude control +- Guided mode yaw fixed when WP_YAW_BEHAVIOR = 0/None and CONDITION_YAW command received containing relative angle +- Landing detector fixed when in stabilize mode at full throttle but aircraft is upside down +- Landing detector logging added to ease support (see LDET message) +- Loiter unlimited command accepts NaNs (QGC sends these) +- Mavlink SYSTEM_STATUS set to BOOT during initialisation +- MOT_PWM_TYPE of 9 (PWMAngle) respects SERVOx_MIN/MAX/TRIM/REVERSED param values +- Payload place bug fix when aborted because gripper is already released +- RC based tuning (aka CH6 tuning) can use any RC aux function channel (see RCx_OPTION = 219) +- RTL_ALT minimum reduced to 30cm +- SystemID position controller support (Copter and Heli) +- TriCopter motor test and slew-up fixed +- WPNAV_SPEED min reduced to 10 cm/s (Copter only) +- Loiter mode zeros desired accel when re-entering from Auto during RC failsafe + +8) TradHeli specific enhancements + +- Autorotation yaw behaviour fix +- Autotune improvements including using low frequency dwell for feedforward gain tuning and conducting sweep in attitude instead of rate +- Blade pitch angle logging added (see SWSH log message) +- Constrain cyclic roll for intermeshing +- ICE / turbine cool down fix +- Inverted flight extended to non manual throttle modes +- Inverted flight transitions smoothed and restricted to only Stabilize mode +- SWSH logging fix for reversed collectives + +9) Plane specific enhancements and bug fixes + +- AIRSPEED_STALL holds vehicle stall speed and is used for minimum speed scaling +- Allow for different orientations of landing rangefinder +- Assistance requirements evaluted on mode change +- FBWB/CRUISE climb/sink rate limited by TECS limits +- FLIGHT_OPTION to immediately climb in AUTO mode (not doing glide slope) +- Glider pullup support (available only through custom build server) +- Loiter breakout improved to better handle destinations inside loiter circle +- Manual mode throttle made consistent with other modes (e.g battery comp and watt limit is done if enabled) +- Mavlink GUIDED_CHANGE_ALTITUDE supports terrain altitudes +- Minimum throttle not applied during SLT VTOL airbrake (reduces increase in airspeed and alt during back transition) +- Q_APPROACH_DIST set minimum distance to use the fixed wing approach logic +- Quadplane assistance check enhancements +- Quadplane Deca frame support +- Quadplane gets smoother takeoff by input shaping target accel and velocity +- Servo wiggles in altitude wait staged to be one after another +- Set_position_target_global_int accepts MAV_FRAME_GLOBAL_RELATIVE_ALT and MAV_FRAME_GLOBAL_TERRAIN_ALT frames +- Takeoff airspeed control improved (see TKOFF_MODE, TKOFF_THR_MIN) +- Takeoff fixes for fence autoenable +- Takeoff improvements including less overshoot of TKOFF_ALT +- TECS reset along with other controllers (important if plane dropped from balloon) +- Tilt quadplane ramp of motors on back transition fixed +- Tiltrotor tilt angles logged +- TKOFF_THR_MIN applied to SLT transitions +- Twin motor planes with DroneCAN ESCs fix to avoid max throttle at arming due to misconfiguration +- VTOLs switch to QLAND if a LONG_FAILSAFE is triggered during takeoff + +10) Rover specific enhancements and bug fixes + +- Auto mode reversed state maintained if momentarily switched to Hold mode +- Circle mode tracks better and avoids getting stuck at circle edge +- Flight time stats fixed +- MAV_CMD_NAV_SET_YAW_SPEED deprecated +- Omni3Mecanum frame support +- Stopping point uses max deceleration (was incorrectly using acceleration) +- Wheel rate controller slew rate fix + +11) Antenna Tracker specific enhancements and bug fixes + +- Never track lat,lon of 0,0 + +12) Scripting enhancements + +- advance-wp.lua applet supports advancing Auto mode WP via RC switch +- AHRS_switch.lua supports switching between EKF2 and EKF3 via RC switch +- battery_internal_resistance_check.lua monitors battery resistance +- CAN:get_device returns nil for unconfigured CAN device +- copter_terrain_brake.lua script added to prevent impact with terrain in Loiter mode (Copter only) +- Copter:get_target_location, update_target_location support +- crosstrack_restore.lua example allows returning to previous track in Auto (Plane only) +- Display text on OLED display supported +- Docs improved for many bindings +- EFI get_last_update_ms binding +- EFI_SkyPower.lua driver accepts 2nd supply voltage +- ESC_slew_rate.lua example script supports testing ESCs +- Filesystem CRC32 check to allow scripts to check module versions +- forced arming support +- GPIO get/set mode bindings (see gpio:set_mode, get_mode) +- GPS-for-yaw angle binding (see gps:gps_yaw_deg) +- Halo6000 EFI driver can log all CAN packets for easier debugging +- handle_external_position_estimate binding allows sending position estimate from lua to EKF +- I2C:transfer support +- IMU gyros_consistent and accels_consistent bindings added +- INF_Inject.lua driver more robust to serial errors, improved logging, throttle and ignition control +- INS bindings for is calibrating, gyro and accel sensor values +- IPV4 address bindings (see SocketAPM_ipv4_addr_to_string) to allow UDP server that responds to individual clients +- Logging of booleans supported +- Lua language checks improved (finds more errors) +- MAVLink commands can be called from scripting +- MCU voltage binding (see analog:mcu_voltage) +- NMEA 2000 EFI driver (see EFI_NMEA2k.lua) +- "open directory failed" false warning when scripts in ROMFS fixed +- Param_Controller.lua supports quickly switching between 3 parameter sets via RC switch +- Pass by reference values are always initialized +- pelco_d_antennatracker.lua applet supports sending Pelco-D via RS-485 to PTZ cameras +- plane_aerobatics.lua minor enhancements +- REPL applet (read-evaluate-print-loop, see repl.lua) for interactive testing and experimentation +- "require" function failures in rare circumstances fixed +- "require" function works for modules in ROMFS (e.g. not on SD card) +- revert_param.lua supports more params (e.g ATC_RATE_R/P/Y, PTCH2SRV_TCONST, RLL2SRV_TCONST, TECS_) +- Scripts may receive mavlink messages which fail CRC (e.g messages which FC does not understand) +- SD card formatting supported +- Serial device simulation support (allows Lua to feed data to any supported serial protocol for e.g. sensor simulation) +- set_target_throttle_rate_rpy allows rate control from scripts (new for Copter) +- sitl_standby_sim.lua example shows how to switch between redundant flight controllers using an RC switch +- Slung payload oscillation suppression applet added (see copter-slung-payload.lua) +- Temperature sensor bindings added +- uint64 support +- Various performance and memory usage optimizations +- VTOL-quicktune.lua minor enhancements including increasing YAW_FLTE_MAX to 8 +- x-quad-cg-allocation.lua applet corrects for the CoM discrepancy in a quad-X frame + +13) GCS / mavlink related changes and fixes + +- BATTERY2 message deprecated (replaced by BATTERY_STATUS) +- CMD_MISSION_START/STOP rejected if first-item/last-item args provided +- Deny attempts to upload two missions simultaneously +- Fence and Rally points may be uploaded using FTP +- GPS_INPUT and HIL_GPS handles multiple GPSs +- HIGHRES_IMU mavlink message support (used by companion computers to receive IMU data from FC) +- MAV_CMD_COMPONENT_ARM_DISARM accepts force arm magic value of 21196 +- MAV_CMD_DO_SET_SAFETY_SWITCH_STATE support +- MAV_CMD_SET_HAGL support (Plane only) +- MAVFTP respects TX buffer flow control to improve FTP on low bandwidth links +- MAVLink receiver support (RADIO_RC_CHANNELS mavlink message) +- Message interval supports TERRAIN_REPORT msg +- Mission upload may be cancelled using MISSION_CLEAR_ALL +- MOUNT_CONFIGURE, MOUNT_CONTROL messages deprecated +- RC_CHANNELS_RAW deprecated +- Serial passthrough supports parity allowing STM32CubeProgrammer to be used to program FrSky R9 receivers +- SET_ATTITUDE_TARGET angular rate input frame fixed (Copter only) +- TIMESYNC and NAMED_VALUE_FLOAT messages not sent on high latency MAVLink ports + +14) Logging enhancements and fixes + +- AC_PID resets and I-term sets logged +- ANG provides attitude at high rate (equivalent to ATT) +- ATT logs angles as floats (better resolution than ints) +- CAND message gets driver index +- DCM log message includes roll/pitch and yaw error +- EDT2 log msg includes stress and status received via extended DShot Telemetry v2 +- EFI ECYL cylinder head and exhaust temp logs in degC (was Kelvin) +- ESCX log msg includes DroneCAN ESCs status, temp, duty cycle and power pct +- FMT messages written as required instead of all at beginning +- Logging restarted after download completes when LOG_DISARMED = 1 +- MISE msg logs active mission command (CMD logged when commands are uploaded) +- ORGN message logging fixed when set using SET_GPS_GLOBAL_ORIGIN +- RPM sensor logging gets instance field, quality and health fields +- Short filename support removed (e.g log1.BIN instead of 00000001.BIN) +- Temperature sensor logging option for only sensors with no source (see TEMP_LOG) +- UART data rates logged at 1hz (see UART message) + +15) ROS2 / DDS support + +- Airspeed published +- Battery topic reports all available batteries +- Compile-time configurable rates for each publisher +- DDS_TIMEOUT_MS and DDS_MAX_RETRY set timeout and num retries when client pings XRCE agent +- GPS global origin published at 1 Hz +- High frequency raw imu data transmission +- Joystick support +- Support sending waypoints to Copter and Rover +- Remove the XML refs file in favor of binary entity creation + +16) Safety related enhancements and fixes + +- Accel/Gyro inconsistent message fixed for using with only single IMU +- Battery monitor failure reported to GCS, triggers battery failsafe but does not take action +- Far from EKF origin pre-arm check removed (Copter only) +- Fence breach warning message slightly improved +- Fence enhancements incluiding alt min fence (Copter only, see FENCE_AUTOENABLE, FENCE_OPTIONS) +- Fences can be stored to SD Card (see BRD_SD_FENCE param) +- ICEngine stopped when in E-Stop or safety engaged (Plane only) +- LEDs flash green lights based on EKF location not GPS +- Parachute option to skip disarm before parachute release (see CHUTE_OPTIONS) +- Plane FENCE_AUTOENABLE of 1 or 2 deprecation warning added +- Pre-arm check if OpenDroneID is compiled in but disabled +- Pre-arm check of duplicate RC aux functions fixed (was skipping recently added aux functions) +- Pre-arm checks alert user more quickly on failure +- Prearm check for misconfigured EAHRS_SENSORS and GPS_TYPE +- Proximity sensor based avoidance keeps working even if one proximity sensor fails (Copter, Rover) +- RC aux functions for Arm, Disarm, E-Stop and Parachute ignored when RC is first turned on +- Warning of duplicate SERIALx_PROTOCOL = RCIN + +17) Other bug fixes and minor enhancements + +- Accel cal fixed for auxiliary IMUs (e.g. IMU4 and higher) +- Bootloader fix to reduce unnecessary mass erasing of flash when using STLink or Jlink tools +- Bootloader rejects allocation of broadcast node ID +- CAN forward registering/de-registering fix (affected Mission Planner DroneCAN UI) +- Dijkstras fix to correct use of uninitialised variable +- DShot rates are not limited by NeoPixel rates +- Ethernet and CAN bootloader fix to prevent getting stuck in bootloader mode +- Filesystem does not show entries for empty @ files +- Filesystem efficiency improvements when reading files +- Flight statistics only reset if user sets STAT_RESET to zero (avoids accidental reset) +- Flight time statistics updated on disarm (avoids issue if vehicle powered-down soon after disarm) +- Internal error thrown if we lose parameters due to saving queue being too small +- MAVLink via DroneCAN baud rate fix +- SPI pins may also be used as GPIOs +- Terrain cache size configurable (see TERRAIN_CACHE_SZ) + +18) Developer focused fixes and enhancements + +- AP_Camera gets example python code showing how to use GStreamer with UDP and RTSP video streams +- Cygwin build fix for non-SITL builds +- Cygwin build fixed with malloc wrap +- DroneCAN and scripting support FlexDebug messages (see CAN_Dn_UC_OPTION, FlexDebug.lua) +- EKF3 code generator documentation and cleanup +- GPS jamming simulator added +- MacOS compiler warnings reduced +- SFML joystick support +- SITL support for OpenBSD +- Text warning if older Fence or Rally point protocols are used +------------------------------------------------------------------ Release 4.5.7 08 Oct 2024 Changes from 4.5.7-beta1 diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index a8fc94074d2fa..7460a561e056b 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -435,7 +435,7 @@ void Rover::one_second_loop(void) set_control_channels(); // cope with changes to aux functions - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); // update notify flags AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false); diff --git a/Rover/system.cpp b/Rover/system.cpp index 6911b78b74701..06405d5e31d4f 100644 --- a/Rover/system.cpp +++ b/Rover/system.cpp @@ -70,7 +70,7 @@ void Rover::init_ardupilot() init_rc_in(); // sets up rc channels deadzone g2.motors.init(get_frame_type()); // init motors including setting servo out channels ranges - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); // init wheel encoders g2.wheel_encoder.init(); diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index 34a91d76fb60e..6bb4cceef91f8 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -291,8 +291,8 @@ AP_HW_PhenixH7_Pro 1172 AP_HW_2RAWH743 1173 AP_HW_X-MAV-AP-H743V2 1174 AP_HW_BETAFPV_F4_2-3S_20A 1175 -AP_HW_BetaflightF4 1176 - +AP_HW_MicoAir743AIOv1 1176 +AP_HW_BetaflightF4 1177 AP_HW_FlywooF405HD_AIOv2 1180 AP_HW_FlywooH743Pro 1181 diff --git a/Tools/AP_Periph/GCS_MAVLink.cpp b/Tools/AP_Periph/GCS_MAVLink.cpp index a3df4631fef87..f35ee3a580c8c 100644 --- a/Tools/AP_Periph/GCS_MAVLink.cpp +++ b/Tools/AP_Periph/GCS_MAVLink.cpp @@ -32,8 +32,10 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_MCU_STATUS, #endif MSG_MEMINFO, -#if AP_GPS_ENABLED +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, #endif }; diff --git a/Tools/AP_Periph/GCS_MAVLink.h b/Tools/AP_Periph/GCS_MAVLink.h index fa3366ed11ffd..7472fc005f67b 100644 --- a/Tools/AP_Periph/GCS_MAVLink.h +++ b/Tools/AP_Periph/GCS_MAVLink.h @@ -41,6 +41,7 @@ class GCS_MAVLINK_Periph : public GCS_MAVLINK void send_nav_controller_output() const override {}; void send_pid_tuning() override {}; + virtual uint8_t send_available_mode(uint8_t index) const override { return 0; } }; /* diff --git a/Tools/AP_Periph/esc_apd_telem.cpp b/Tools/AP_Periph/esc_apd_telem.cpp index ed438d60f36c5..e7173bdca9e83 100644 --- a/Tools/AP_Periph/esc_apd_telem.cpp +++ b/Tools/AP_Periph/esc_apd_telem.cpp @@ -17,8 +17,9 @@ extern const AP_HAL::HAL& hal; #define TELEM_LEN 0x16 ESC_APD_Telem::ESC_APD_Telem (AP_HAL::UARTDriver *_uart, float num_poles) : - pole_count(num_poles), - uart(_uart) { + uart(_uart), + pole_count(num_poles) +{ uart->begin(115200); } diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp index 2dbfbe07f5149..8feb3e0805ce8 100644 --- a/Tools/AP_Periph/rc_out.cpp +++ b/Tools/AP_Periph/rc_out.cpp @@ -71,7 +71,7 @@ void AP_Periph_FW::rcout_init() hal.rcout->set_freq(esc_mask, g.esc_rate.get()); // setup ESCs with the desired PWM type, allowing for DShot - SRV_Channels::init(esc_mask, (AP_HAL::RCOutput::output_mode)g.esc_pwm_type.get()); + AP::srv().init(esc_mask, (AP_HAL::RCOutput::output_mode)g.esc_pwm_type.get()); // run DShot at 1kHz hal.rcout->set_dshot_rate(SRV_Channels::get_dshot_rate(), 400); @@ -83,7 +83,7 @@ void AP_Periph_FW::rcout_init() void AP_Periph_FW::rcout_init_1Hz() { // this runs at 1Hz to allow for run-time param changes - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); for (uint8_t i=0; i= esc_telem_update_period_ms) { last_esc_telem_update_ms = now_ms; diff --git a/Tools/GIT_Test/GIT_Success.txt b/Tools/GIT_Test/GIT_Success.txt index 13d517d005c22..a475ba17f1bd8 100644 --- a/Tools/GIT_Test/GIT_Success.txt +++ b/Tools/GIT_Test/GIT_Success.txt @@ -205,3 +205,4 @@ Naveen Kumar Kilaparthi Amr Attia Alessandro Martini Eren Mert YİĞİT +Prashant Powar diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 154ef72f8bcf4..7ccddaad109e0 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -361,10 +361,10 @@ def configure_env(self, cfg, env): '-Wpointer-arith', '-Wno-unused-parameter', '-Wno-missing-field-initializers', - '-Wno-reorder', '-Wno-redundant-decls', '-Wno-unknown-pragmas', '-Wno-expansion-to-defined', + '-Werror=reorder', '-Werror=cast-align', '-Werror=attributes', '-Werror=format-security', diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 97792527db53d..1482c6f829ade 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -9832,28 +9832,15 @@ def RefindGPS(self): def GPSForYaw(self): '''Moving baseline GPS yaw''' - self.context_push() self.load_default_params_file("copter-gps-for-yaw.parm") self.reboot_sitl() - ex = None - try: - self.wait_gps_fix_type_gte(6, message_type="GPS2_RAW", verbose=True) - m = self.assert_receive_message("GPS2_RAW") - self.progress(self.dump_message_verbose(m)) - want = 27000 - if abs(m.yaw - want) > 500: - raise NotAchievedException("Expected to get GPS-from-yaw (want %f got %f)" % (want, m.yaw)) - self.wait_ready_to_arm() - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - - self.reboot_sitl() - - if ex is not None: - raise ex + self.wait_gps_fix_type_gte(6, message_type="GPS2_RAW", verbose=True) + m = self.assert_receive_message("GPS2_RAW", very_verbose=True) + want = 27000 + if abs(m.yaw - want) > 500: + raise NotAchievedException("Expected to get GPS-from-yaw (want %f got %f)" % (want, m.yaw)) + self.wait_ready_to_arm() def SMART_RTL_EnterLeave(self): '''check SmartRTL behaviour when entering/leaving''' diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index bc0d157258f51..48334ab8d7c2e 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4831,6 +4831,34 @@ def TakeoffGround(self): self.fly_home_land_and_disarm() + def TakeoffIdleThrottle(self): + '''Apply idle throttle before takeoff.''' + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "TKOFF_THR_IDLE": 20.0, + "TKOFF_THR_MINSPD": 3.0, + }) + self.change_mode("TAKEOFF") + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Ensure that the throttle rises to idle throttle. + expected_idle_throttle = 1000+10*self.get_parameter("TKOFF_THR_IDLE") + self.assert_servo_channel_range(3, expected_idle_throttle-10, expected_idle_throttle+10) + + # Launch the catapult + self.set_servo(6, 1000) + + self.delay_sim_time(5) + self.change_mode('RTL') + + self.fly_home_land_and_disarm() + def DCMFallback(self): '''Really annoy the EKF and force fallback''' self.reboot_sitl() @@ -6389,6 +6417,7 @@ def tests1b(self): self.TakeoffTakeoff3, self.TakeoffTakeoff4, self.TakeoffGround, + self.TakeoffIdleThrottle, self.ForcedDCM, self.DCMFallback, self.MAVFTP, diff --git a/Tools/autotest/helicopter.py b/Tools/autotest/helicopter.py index 9f3f57cc4ddaf..c02608dbb2e3e 100644 --- a/Tools/autotest/helicopter.py +++ b/Tools/autotest/helicopter.py @@ -708,6 +708,48 @@ def NastyMission(self): self.change_mode('LOITER') self.fly_mission_points(self.scurve_nasty_up_mission()) + def MountFailsafeAction(self): + """Fly Mount Failsafe action""" + self.context_push() + + self.progress("Setting up servo mount") + roll_servo = 12 + pitch_servo = 11 + yaw_servo = 10 + open_servo = 9 + roll_limit = 50 + self.set_parameters({ + "MNT1_TYPE": 1, + "SERVO%u_MIN" % roll_servo: 1000, + "SERVO%u_MAX" % roll_servo: 2000, + "SERVO%u_FUNCTION" % yaw_servo: 6, # yaw + "SERVO%u_FUNCTION" % pitch_servo: 7, # roll + "SERVO%u_FUNCTION" % roll_servo: 8, # pitch + "SERVO%u_FUNCTION" % open_servo: 9, # mount open + "MNT1_OPTIONS": 2, # retract + "MNT1_DEFLT_MODE": 3, # RC targettting + "MNT1_ROLL_MIN": -roll_limit, + "MNT1_ROLL_MAX": roll_limit, + }) + + self.reboot_sitl() + + retract_roll = 25.0 + self.set_parameter("MNT1_NEUTRAL_X", retract_roll) + self.progress("Killing RC") + self.set_parameter("SIM_RC_FAIL", 2) + self.delay_sim_time(10) + want_servo_channel_value = int(1500 + 500*retract_roll/roll_limit) + self.wait_servo_channel_value(roll_servo, want_servo_channel_value, epsilon=1) + + self.progress("Resurrecting RC") + self.set_parameter("SIM_RC_FAIL", 0) + self.wait_servo_channel_value(roll_servo, 1500) + + self.context_pop() + + self.reboot_sitl() + def set_rc_default(self): super(AutoTestHelicopter, self).set_rc_default() self.progress("Lowering rotor speed") @@ -1122,6 +1164,7 @@ def tests(self): self.NastyMission, self.PIDNotches, self.AutoTune, + self.MountFailsafeAction, ]) return ret diff --git a/Tools/autotest/logger_metadata/enum_parse.py b/Tools/autotest/logger_metadata/enum_parse.py index d9ab17f4b41af..3f37a9837f443 100755 --- a/Tools/autotest/logger_metadata/enum_parse.py +++ b/Tools/autotest/logger_metadata/enum_parse.py @@ -92,6 +92,11 @@ def match_enum_line(self, line): if m is not None: return (m.group(1), 1 << int(m.group(2)), m.group(3)) + # Match: "#define FRED 1 // optional comment" + m = re.match(r"#define\s*([A-Z0-9_a-z]+)\s+(-?\d+) *(// *(.*) *)?$", line) + if m is not None: + return (m.group(1), m.group(2), m.group(4)) + if m is None: raise ValueError("Failed to match (%s)" % line) @@ -116,7 +121,13 @@ def debug(x): break line = line.rstrip() # print("state=%s line: %s" % (state, line)) - if re.match(r"\s*//.*", line): + # Skip single-line comments - unless they contain LoggerEnum tags + if re.match(r"\s*//.*", line) and "LoggerEnum" not in line: + continue + # Skip multi-line comments + if re.match(r"\s*/\*.*", line): + while "*/" not in line: + line = f.readline() continue if state == "outside": if re.match("class .*;", line) is not None: @@ -154,6 +165,19 @@ def debug(x): last_value = None state = state_inside skip_enumeration = False + continue + + # // @LoggerEnum: NAME - can be used around for #define sets + m = re.match(r".*@LoggerEnum: *([\w]+)", line) + if m is not None: + enum_name = m.group(1) + debug("%s: %s" % (source_file, enum_name)) + entries = [] + last_value = None + state = state_inside + skip_enumeration = False + continue + continue if state == "inside": if re.match(r"\s*$", line): @@ -164,7 +188,7 @@ def debug(x): continue if re.match(r"#else", line): continue - if re.match(r".*}\s*\w*(\s*=\s*[\w:]+)?;", line): + if re.match(r".*}\s*\w*(\s*=\s*[\w:]+)?;", line) or "@LoggerEnumEnd" in line: # potential end of enumeration if not skip_enumeration: if enum_name is None: diff --git a/Tools/autotest/sim_vehicle.py b/Tools/autotest/sim_vehicle.py index 19dbbb5ee7cd3..6899e55e9912c 100755 --- a/Tools/autotest/sim_vehicle.py +++ b/Tools/autotest/sim_vehicle.py @@ -964,6 +964,8 @@ def start_mavproxy(opts, stuff): cmd.extend(['--aircraft', opts.aircraft]) if opts.moddebug: cmd.append('--moddebug=%u' % opts.moddebug) + if opts.mavcesium: + cmd.extend(["--load-module", "cesium"]) if opts.fresh_params: # these were built earlier: @@ -1373,6 +1375,11 @@ def generate_frame_help(): default=False, action='store_true', help="load map module on startup") +group.add_option("", "--mavcesium", + default=False, + action='store_true', + help="load MAVCesium module on startup") + group.add_option("", "--console", default=False, action='store_true', diff --git a/Tools/autotest/test_build_options.py b/Tools/autotest/test_build_options.py index 12f5b8c3946c9..7cbc005c310aa 100755 --- a/Tools/autotest/test_build_options.py +++ b/Tools/autotest/test_build_options.py @@ -258,7 +258,6 @@ def define_is_whitelisted_for_feature_in_code(self, target, define): 'AP_COMPASS_MAG3110_ENABLED', # must be in hwdef, not probed 'AP_COMPASS_MMC5XX3_ENABLED', # must be in hwdef, not probed 'AP_MAVLINK_AUTOPILOT_VERSION_REQUEST_ENABLED', # completely elided - 'AP_MAVLINK_MSG_HIL_GPS_ENABLED', # no symbol available 'AP_MAVLINK_MSG_RELAY_STATUS_ENABLED', # no symbol available 'AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED', # no symbol available 'HAL_MSP_SENSORS_ENABLED', # no symbol available @@ -287,6 +286,7 @@ def define_is_whitelisted_for_feature_in_code(self, target, define): feature_define_whitelist.add('AP_WINCH_DAIWA_ENABLED') feature_define_whitelist.add('AP_WINCH_PWM_ENABLED') feature_define_whitelist.add(r'AP_MOTORS_FRAME_.*_ENABLED') + feature_define_whitelist.add('AP_COPTER_ADVANCED_FAILSAFE_ENABLED') if target.lower() != "plane": # only on Plane: diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 3cab908b5951c..5885c21267fc9 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -7939,7 +7939,7 @@ def get_servo_channel_value(self, channel, timeout=2): (str(m), channel_field)) return m_value - def wait_servo_channel_value(self, channel, value, timeout=2, comparator=operator.eq): + def wait_servo_channel_value(self, channel, value, epsilon=0, timeout=2, comparator=operator.eq): """wait for channel value comparison (default condition is equality)""" channel_field = "servo%u_raw" % channel opstring = ("%s" % comparator)[-3:-1] @@ -7957,8 +7957,11 @@ def wait_servo_channel_value(self, channel, value, timeout=2, comparator=operato if m_value is None: raise ValueError("message (%s) has no field %s" % (str(m), channel_field)) - self.progress("want SERVO_OUTPUT_RAW.%s=%u %s %u" % + self.progress("SERVO_OUTPUT_RAW.%s got=%u %s want=%u" % (channel_field, m_value, opstring, value)) + if comparator == operator.eq: + if abs(m_value - value) <= epsilon: + return m_value if comparator(m_value, value): return m_value diff --git a/Tools/bootloaders/BetaflightF4_bl.bin b/Tools/bootloaders/BetaflightF4_bl.bin index f82ed42b62d12..d51bf809cec08 100755 Binary files a/Tools/bootloaders/BetaflightF4_bl.bin and b/Tools/bootloaders/BetaflightF4_bl.bin differ diff --git a/Tools/bootloaders/BetaflightF4_bl.hex b/Tools/bootloaders/BetaflightF4_bl.hex index c7124c61d7856..6b0f96f1a6161 100644 --- a/Tools/bootloaders/BetaflightF4_bl.hex +++ b/Tools/bootloaders/BetaflightF4_bl.hex @@ -886,7 +886,7 @@ :103740000A0000000000000003000000000000006C :103750000000000000000000000000000000000069 :103760000000000000000000000000000000000059 -:10377000980400000000000000400F00000000005E +:10377000990400000000000000400F00000000005D :10378000FF009600000000080096000000000800FE :1037900004000000DC35000800000000000000000C :1037A0000000000000000000000000000000000019 diff --git a/Tools/environment_install/install-prereqs-ubuntu.sh b/Tools/environment_install/install-prereqs-ubuntu.sh index 7f5c075a824f9..d7b7d8c31440d 100755 --- a/Tools/environment_install/install-prereqs-ubuntu.sh +++ b/Tools/environment_install/install-prereqs-ubuntu.sh @@ -162,7 +162,7 @@ else fi # Lists of packages to install -BASE_PKGS="build-essential ccache g++ gawk git make wget valgrind screen python3-pexpect" +BASE_PKGS="build-essential ccache g++ gawk git make wget valgrind screen python3-pexpect astyle" PYTHON_PKGS="future lxml pymavlink pyserial MAVProxy geocoder empy==3.3.4 ptyprocess dronecan" PYTHON_PKGS="$PYTHON_PKGS flake8 junitparser wsproto" diff --git a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_parameter_service.py b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_parameter_service.py new file mode 100644 index 0000000000000..8e6e95601c971 --- /dev/null +++ b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_parameter_service.py @@ -0,0 +1,197 @@ +# Copyright 2023 ArduPilot.org. +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +""" +Bring up ArduPilot SITL and check that the get/set_parameters services are up and running. + +Checks whether a parameter is changed using service call. + +colcon test --packages-select ardupilot_dds_tests \ +--event-handlers=console_cohesion+ --pytest-args -k test_parameter_service + +""" + +import launch_pytest +import pytest +import rclpy +import rclpy.node +import threading +import time + +from launch import LaunchDescription + +from launch_pytest.tools import process as process_tools + +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from rclpy.qos import QoSHistoryPolicy + +from rcl_interfaces.srv import GetParameters +from rcl_interfaces.srv import SetParameters +from rcl_interfaces.msg import Parameter + +# Enums for parameter type +PARAMETER_NOT_SET = 0 +PARAMETER_INTEGER = 2 +PARAMETER_DOUBLE = 3 + + +class ParameterClient(rclpy.node.Node): + """Send GetParameters and SetParameters Requests.""" + + def __init__(self): + """Initialize the node.""" + super().__init__('parameter_client') + self.get_param_event_object = threading.Event() + self.set_param_event_object = threading.Event() + self.set_correct_object = threading.Event() + + def start_client(self): + """Start the parameter client.""" + qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + + # Define clients for getting and setting parameter + self.get_cli = self.create_client(GetParameters, 'ap/get_parameters') + while not self.get_cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('GetParameters service not available, waiting again...') + + self.set_cli = self.create_client(SetParameters, 'ap/set_parameters') + while not self.set_cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('SetParameters service not availabel, waiting again...') + + # Add a spin thread + self.ros_spin_thread = threading.Thread(target=lambda node: rclpy.spin(node), args=(self,)) + self.ros_spin_thread.start() + + def send_get_param_req(self, param_name): + self.get_req = GetParameters.Request() + self.get_req.names.append(param_name) + + self.get_future = self.get_cli.call_async(self.get_req) + while not self.get_future.done(): + self.get_logger().info("Waiting for GetParameters service response...") + time.sleep(0.1) + + resp = self.get_future.result() + value = resp.values[0] + + if value.type != PARAMETER_NOT_SET: + self.get_param_event_object.set() + + def send_set_param_req(self, param_name, param_value, param_type): + self.set_req = SetParameters.Request() + param_update = Parameter() + param_update.name = param_name + param_update.value.type = param_type + if param_type == PARAMETER_INTEGER: + param_update.value.integer_value = int(param_value) + else: + param_update.value.double_value = float(param_value) + + self.set_req.parameters.append(param_update) + + self.desired_value = param_value + get_response = self.get_future.result() + initial_value = get_response.values[0] + + if initial_value.type == PARAMETER_INTEGER: + self.param_value = initial_value.integer_value + elif initial_value.type == PARAMETER_DOUBLE: + self.param_value = initial_value.double_value + else: + self.param_value = 'nan' + + self.set_future = self.set_cli.call_async(self.set_req) + + while not self.set_future.done(): + self.get_logger().info("Waiting for SetParameters service response...") + time.sleep(0.1) + + if self.set_future.done(): + response = self.set_future.result() + if response.results[0].successful: + self.set_param_event_object.set() + + def check_param_change(self): + param_name = self.set_req.parameters[0].name + + self.send_get_param_req(param_name) + + get_response = self.get_future.result() + new_value = get_response.values[0] + + if new_value.type == PARAMETER_INTEGER: + updated_value = new_value.integer_value + elif new_value.type == PARAMETER_DOUBLE: + updated_value = new_value.double_value + else: + updated_value = 'nan' + + if updated_value == self.desired_value: + self.set_correct_object.set() + + +@launch_pytest.fixture +def launch_sitl_copter_dds_udp(sitl_copter_dds_udp): + """Fixture to create the launch description.""" + sitl_ld, sitl_actions = sitl_copter_dds_udp + + ld = LaunchDescription( + [ + sitl_ld, + launch_pytest.actions.ReadyToTest(), + ] + ) + actions = sitl_actions + yield ld, actions + + +@pytest.mark.launch(fixture=launch_sitl_copter_dds_udp) +def test_dds_udp_parameter_services(launch_context, launch_sitl_copter_dds_udp): + """Test Get/Set parameter services broadcast by AP_DDS.""" + _, actions = launch_sitl_copter_dds_udp + micro_ros_agent = actions["micro_ros_agent"].action + mavproxy = actions["mavproxy"].action + sitl = actions["sitl"].action + + # Wait for process to start. + process_tools.wait_for_start_sync(launch_context, micro_ros_agent, timeout=2) + process_tools.wait_for_start_sync(launch_context, mavproxy, timeout=2) + process_tools.wait_for_start_sync(launch_context, sitl, timeout=2) + + rclpy.init() + try: + node = ParameterClient() + node.start_client() + parameter_name = "LOIT_SPEED" + param_change_value = 1250 + param_type = PARAMETER_DOUBLE + node.send_get_param_req(parameter_name) + get_param_received_flag = node.get_param_event_object.wait(timeout=10.0) + assert get_param_received_flag, f"Did not get '{parameter_name}' param." + node.send_set_param_req(parameter_name, param_change_value, param_type) + set_param_received_flag = node.set_param_event_object.wait(timeout=10.0) + assert set_param_received_flag, f"Could not set '{parameter_name}' to '{param_change_value}'" + node.check_param_change() + set_param_changed_flag = node.set_correct_object.wait(timeout=10.0) + assert set_param_changed_flag, f"Did not confirm '{parameter_name}' value change" + + finally: + rclpy.shutdown() + yield diff --git a/Tools/ros2/ardupilot_msgs/CMakeLists.txt b/Tools/ros2/ardupilot_msgs/CMakeLists.txt index d066a2d32b2e4..f8af788e8b81e 100644 --- a/Tools/ros2/ardupilot_msgs/CMakeLists.txt +++ b/Tools/ros2/ardupilot_msgs/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/GlobalPosition.msg" + "msg/Status.msg" "srv/ArmMotors.srv" "srv/ModeSwitch.srv" DEPENDENCIES geometry_msgs std_msgs diff --git a/Tools/ros2/ardupilot_msgs/msg/Status.msg b/Tools/ros2/ardupilot_msgs/msg/Status.msg new file mode 100644 index 0000000000000..38ff02804d12c --- /dev/null +++ b/Tools/ros2/ardupilot_msgs/msg/Status.msg @@ -0,0 +1,27 @@ +std_msgs/Header header + +uint8 APM_ROVER = 1 +uint8 APM_ARDUCOPTER = 2 +uint8 APM_ARDUPLANE = 3 +uint8 APM_ANTENNATRACKER = 4 +uint8 APM_UNKNOWN = 5 +uint8 APM_REPLAY = 6 +uint8 APM_ARDUSUB = 7 +uint8 APM_IOFIRMWARE = 8 +uint8 APM_AP_PERIPH = 9 +uint8 APM_AP_DAL_STANDALONE = 10 +uint8 APM_AP_BOOTLOADER = 11 +uint8 APM_BLIMP = 12 +uint8 APM_HELI = 13 +uint8 vehicle_type # From AP_Vehicle_Type.h + +bool armed # true if vehicle is armed. +uint8 mode # Vehicle mode, enum depending on vehicle type. +bool flying # True if flying/driving/diving/tracking. +bool external_control # True is external control is enabled. + +uint8 FS_RADIO = 21 +uint8 FS_BATTERY = 22 +uint8 FS_GCS = 23 +uint8 FS_EKF = 24 +uint8[] failsafe # Array containing all active failsafe. diff --git a/Tools/scripts/build_ci.sh b/Tools/scripts/build_ci.sh index 19d2912158a8f..bdf43bb8cf399 100755 --- a/Tools/scripts/build_ci.sh +++ b/Tools/scripts/build_ci.sh @@ -243,6 +243,10 @@ for t in $CI_BUILD_TARGET; do $waf configure --board FreeflyRTK $waf clean $waf AP_Periph + echo "Building CubeNode-ETH peripheral fw" + $waf configure --board CubeNode-ETH + $waf clean + $waf AP_Periph continue fi @@ -473,7 +477,11 @@ for t in $CI_BUILD_TARGET; do if [ "$t" == "astyle-cleanliness" ]; then echo "Checking AStyle code cleanliness" + ./Tools/scripts/run_astyle.py --dry-run + if [ $? -ne 0 ]; then + echo The code failed astyle cleanliness checks. Please run ./Tools/scripts/run_astyle.py + fi continue fi diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 3f5dd534ff8f1..f31edfaf87ff3 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -156,6 +156,7 @@ def config_option(self): Feature('Copter', 'MODE_FLOWHOLD', 'MODE_FLOWHOLD_ENABLED', 'Enable Mode Flowhold', 0, "OPTICALFLOW"), Feature('Copter', 'MODE_FLIP', 'MODE_FLIP_ENABLED', 'Enable Mode Flip', 0, None), Feature('Copter', 'MODE_BRAKE', 'MODE_BRAKE_ENABLED', 'Enable Mode Brake', 0, None), + Feature('Copter', 'COPTER_ADVANCED_FAILSAFE', 'AP_COPTER_ADVANCED_FAILSAFE_ENABLED', 'Enable Advanced Failsafe', 0, "ADVANCED_FAILSAFE"), # NOQA: 501 Feature('Rover', 'ROVER_ADVANCED_FAILSAFE', 'AP_ROVER_ADVANCED_FAILSAFE_ENABLED', 'Enable Advanced Failsafe', 0, "ADVANCED_FAILSAFE"), # NOQA: 501 @@ -363,7 +364,6 @@ def config_option(self): Feature('MAVLink', 'MAVLINK_VERSION_REQUEST', 'AP_MAVLINK_AUTOPILOT_VERSION_REQUEST_ENABLED', 'Enable Old AUTOPILOT_VERSION_REQUEST mesage', 0, None), # noqa Feature('MAVLink', 'REQUEST_AUTOPILOT_CAPA', 'AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED', 'Enable Old REQUEST_AUTOPILOT_CAPABILITIES command', 0, None), # noqa Feature('MAVLink', 'MAV_MSG_RELAY_STATUS', 'AP_MAVLINK_MSG_RELAY_STATUS_ENABLED', 'Enable Send RELAY_STATUS message', 0, 'RELAY'), # noqa - Feature('MAVLink', 'MAV_MSG_HIL_GPS', 'AP_MAVLINK_MSG_HIL_GPS_ENABLED', 'Enable Receive HIL_GPS messages', 0, 'MAV'), # noqa Feature('MAVLink', 'MAV_MSG_MOUNT_CONTROL', 'AP_MAVLINK_MSG_MOUNT_CONTROL_ENABLED', 'Enable Deprecated MOUNT_CONTROL message', 0, "MOUNT"), # noqa Feature('MAVLink', 'MAV_MSG_MOUNT_CONFIGURE', 'AP_MAVLINK_MSG_MOUNT_CONFIGURE_ENABLED', 'Enable Deprecated MOUNT_CONFIGURE message', 0, "MOUNT"), # noqa Feature('MAVLink', 'AP_MAVLINK_BATTERY2_ENABLED', 'AP_MAVLINK_BATTERY2_ENABLED', 'Enable Send old BATTERY2 message', 0, None), # noqa diff --git a/Tools/scripts/decode_devid.py b/Tools/scripts/decode_devid.py index ee80f080ae0c8..8841d06d8769e 100755 --- a/Tools/scripts/decode_devid.py +++ b/Tools/scripts/decode_devid.py @@ -104,6 +104,7 @@ def num(s): 0x3A : "DEVTYPE_INS_ICM42670", 0x3B : "DEVTYPE_INS_ICM45686", 0x3C : "DEVTYPE_INS_SCHA63T", + 0x3D : "DEVTYPE_INS_IIM42653", } baro_types = { diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index eec1399c02a3b..de6eec6589523 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -273,11 +273,11 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_CUSTOMROTATIONS_ENABLED', 'AP_CustomRotations::init'), ('AP_OSD_LINK_STATS_EXTENSIONS_ENABLED', r'AP_OSD_Screen::draw_rc_tx_power'), ('HAL_ENABLE_DRONECAN_DRIVERS', r'AP_DroneCAN::init'), - ('AP_MAVLINK_MSG_HIL_GPS_ENABLED', r'mavlink_msg_hil_gps_decode'), ('AP_BARO_PROBE_EXTERNAL_I2C_BUSES', r'AP_Baro::_probe_i2c_barometers'), ('AP_RSSI_ENABLED', r'AP_RSSI::init'), ('AP_ROVER_ADVANCED_FAILSAFE_ENABLED', r'Rover::afs_fs_check'), + ('AP_COPTER_ADVANCED_FAILSAFE_ENABLED', r'Copter::afs_fs_check'), ('AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED', r'GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands'), ('AP_SERIALMANAGER_REGISTER_ENABLED', r'AP_SerialManager::register_port'), diff --git a/Tools/scripts/powr_change.py b/Tools/scripts/powr_change.py index 332f9814dcd9e..d7c295a08c757 100755 --- a/Tools/scripts/powr_change.py +++ b/Tools/scripts/powr_change.py @@ -70,8 +70,6 @@ def run(self): if new_acc_bit_set and not old_acc_bit_set: line += " ACCFLAGS+%s" % self.bit_description(bit) - elif not new_bit_set and old_bit_set: - line += " ACCFLAGS-%s" % self.bit_description(bit) if len(line) == 0: continue diff --git a/Tools/scripts/run_astyle.py b/Tools/scripts/run_astyle.py index 7f5a694f73dbe..23ebee79ac16f 100755 --- a/Tools/scripts/run_astyle.py +++ b/Tools/scripts/run_astyle.py @@ -60,7 +60,7 @@ def check(self): self.progress("astyle check failed: (%s)" % (ret.stdout)) self.retcode = 1 if "Formatted" in ret.stdout: - self.progress("Files needing formatting found") + self.progress("Files needing formatting found.") print(ret.stdout) self.retcode = 1 diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 00988f1a0a6b9..10f9deed01df9 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -618,9 +618,9 @@ class AP_AHRS { // return current vibration vector for primary IMU Vector3f get_vibration(void) const; - // return primary accels, for lua + // return primary accels const Vector3f &get_accel(void) const { - return AP::ins().get_accel(); + return AP::ins().get_accel(_get_primary_accel_index()); } // return primary accel bias. This should be subtracted from diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 8ff983a692ded..ffa148d6b5af1 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -175,7 +175,7 @@ extern AP_IOMCU iomcu; #endif #pragma GCC diagnostic push -#if defined (__clang__) +#if defined(__clang_major__) && __clang_major__ >= 14 #pragma GCC diagnostic ignored "-Wbitwise-instead-of-logical" #endif diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp index ecd2c79b89b25..d5d6c324bf7e8 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp @@ -63,28 +63,28 @@ const AP_Param::GroupInfo AP_BattMonitor_FuelLevel_Analog::var_info[] = { // @Param: FL_FF // @DisplayName: First order term // @Description: First order polynomial fit term - // @Range: 0 10 + // @Range: -10 10 // @User: Advanced AP_GROUPINFO("FL_FF", 45, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_first_order_coeff, 1), // @Param: FL_FS // @DisplayName: Second order term // @Description: Second order polynomial fit term - // @Range: 0 10 + // @Range: -10 10 // @User: Advanced AP_GROUPINFO("FL_FS", 46, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_second_order_coeff, 0), // @Param: FL_FT // @DisplayName: Third order term // @Description: Third order polynomial fit term - // @Range: 0 10 + // @Range: -10 10 // @User: Advanced AP_GROUPINFO("FL_FT", 47, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_third_order_coeff, 0), // @Param: FL_OFF // @DisplayName: Offset term // @Description: Offset polynomial fit term - // @Range: 0 10 + // @Range: -10 10 // @User: Advanced AP_GROUPINFO("FL_OFF", 48, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_offset, 0), diff --git a/libraries/AP_CANManager/AP_CANDriver.h b/libraries/AP_CANManager/AP_CANDriver.h index f93e96cec0e7a..5f1c4f74a2a76 100644 --- a/libraries/AP_CANManager/AP_CANDriver.h +++ b/libraries/AP_CANManager/AP_CANDriver.h @@ -40,5 +40,5 @@ class AP_CANDriver virtual bool add_11bit_driver(CANSensor *sensor) { return false; } // handler for outgoing frames for auxillary drivers - virtual bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) { return false; } + virtual bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us) { return false; } }; diff --git a/libraries/AP_CANManager/AP_CANSensor.cpp b/libraries/AP_CANManager/AP_CANSensor.cpp index e86ef31992475..ffa9bf3cc6d9f 100644 --- a/libraries/AP_CANManager/AP_CANSensor.cpp +++ b/libraries/AP_CANManager/AP_CANSensor.cpp @@ -135,7 +135,7 @@ bool CANSensor::add_interface(AP_HAL::CANIface* can_iface) return true; } -bool CANSensor::write_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) +bool CANSensor::write_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us) { if (!_initialized) { debug_can(AP_CANManager::LOG_ERROR, "Driver not initialized for write_frame"); @@ -171,12 +171,12 @@ void CANSensor::loop() #endif while (true) { - uint64_t timeout = AP_HAL::micros64() + LOOP_INTERVAL_US; + uint64_t deadline_us = AP_HAL::micros64() + LOOP_INTERVAL_US; // wait to receive frame bool read_select = true; bool write_select = false; - bool ret = _can_iface->select(read_select, write_select, nullptr, timeout); + bool ret = _can_iface->select(read_select, write_select, nullptr, deadline_us); if (ret && read_select) { uint64_t time; AP_HAL::CANIface::CanIOFlags flags {}; diff --git a/libraries/AP_CANManager/AP_CANSensor.h b/libraries/AP_CANManager/AP_CANSensor.h index 7afd1bbea0fa5..b5ac4ee3b0b0f 100644 --- a/libraries/AP_CANManager/AP_CANSensor.h +++ b/libraries/AP_CANManager/AP_CANSensor.h @@ -43,7 +43,7 @@ class CANSensor : public AP_CANDriver { virtual void handle_frame(AP_HAL::CANFrame &frame) = 0; // handler for outgoing frames - bool write_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us); + bool write_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us); #ifdef HAL_BUILD_AP_PERIPH static void set_periph(const uint8_t i, const AP_CAN::Protocol protocol, AP_HAL::CANIface* iface) { diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index b7fcf460c37b2..a39240143fb55 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -17,6 +17,7 @@ #include # endif // AP_DDS_ARM_SERVER_ENABLED #include +#include #include #if AP_DDS_ARM_SERVER_ENABLED @@ -66,6 +67,9 @@ static constexpr uint16_t DELAY_CLOCK_TOPIC_MS =AP_DDS_DELAY_CLOCK_TOPIC_MS; static constexpr uint16_t DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS = AP_DDS_DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS; #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED static constexpr uint16_t DELAY_PING_MS = 500; +#ifdef AP_DDS_STATUS_PUB_ENABLED +static constexpr uint16_t DELAY_STATUS_TOPIC_MS = AP_DDS_DELAY_STATUS_TOPIC_MS; +#endif // AP_DDS_STATUS_PUB_ENABLED // Define the subscriber data members, which are static class scope. // If these are created on the stack in the subscriber, @@ -81,6 +85,17 @@ geometry_msgs_msg_TwistStamped AP_DDS_Client::rx_velocity_control_topic {}; ardupilot_msgs_msg_GlobalPosition AP_DDS_Client::rx_global_position_control_topic {}; #endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED +// Define the parameter server data members, which are static class scope. +// If these are created on the stack, then the AP_DDS_Client::on_request +// frame size is exceeded. +#if AP_DDS_PARAMETER_SERVER_ENABLED +rcl_interfaces_srv_SetParameters_Request AP_DDS_Client::set_parameter_request {}; +rcl_interfaces_srv_SetParameters_Response AP_DDS_Client::set_parameter_response {}; +rcl_interfaces_srv_GetParameters_Request AP_DDS_Client::get_parameters_request {}; +rcl_interfaces_srv_GetParameters_Response AP_DDS_Client::get_parameters_response {}; +rcl_interfaces_msg_Parameter AP_DDS_Client::param {}; +#endif + const AP_Param::GroupInfo AP_DDS_Client::var_info[] { // @Param: _ENABLE @@ -604,6 +619,58 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPointStamped& msg) } #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED +#if AP_DDS_STATUS_PUB_ENABLED +bool AP_DDS_Client::update_topic(ardupilot_msgs_msg_Status& msg) +{ + // Fill the new message. + const auto &vehicle = AP::vehicle(); + const auto &battery = AP::battery(); + msg.vehicle_type = static_cast(AP::fwversion().vehicle_type); + msg.armed = hal.util->get_soft_armed(); + msg.mode = vehicle->get_mode(); + msg.flying = vehicle->get_likely_flying(); + msg.external_control = true; // Always true for now. To be filled after PR#28429. + uint8_t fs_iter = 0; + msg.failsafe_size = 0; + if (rc().in_rc_failsafe()) { + msg.failsafe[fs_iter++] = FS_RADIO; + } + if (battery.has_failsafed()) { + msg.failsafe[fs_iter++] = FS_BATTERY; + } + // TODO: replace flag with function. + if (AP_Notify::flags.failsafe_gcs) { + msg.failsafe[fs_iter++] = FS_GCS; + } + // TODO: replace flag with function. + if (AP_Notify::flags.failsafe_ekf) { + msg.failsafe[fs_iter++] = FS_EKF; + } + msg.failsafe_size = fs_iter; + + // Compare with the previous one. + bool is_message_changed {false}; + is_message_changed |= (last_status_msg_.flying != msg.flying); + is_message_changed |= (last_status_msg_.armed != msg.armed); + is_message_changed |= (last_status_msg_.mode != msg.mode); + is_message_changed |= (last_status_msg_.vehicle_type != msg.vehicle_type); + is_message_changed |= (last_status_msg_.failsafe_size != msg.failsafe_size); + is_message_changed |= (last_status_msg_.external_control != msg.external_control); + + if ( is_message_changed ) { + last_status_msg_.flying = msg.flying; + last_status_msg_.armed = msg.armed; + last_status_msg_.mode = msg.mode; + last_status_msg_.vehicle_type = msg.vehicle_type; + last_status_msg_.failsafe_size = msg.failsafe_size; + last_status_msg_.external_control = msg.external_control; + update_topic(msg.header.stamp); + return true; + } else { + return false; + } +} +#endif // AP_DDS_STATUS_PUB_ENABLED /* start the DDS thread */ @@ -801,6 +868,198 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u break; } #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED +#if AP_DDS_PARAMETER_SERVER_ENABLED + case services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id: { + const bool deserialize_success = rcl_interfaces_srv_SetParameters_Request_deserialize_topic(ub, &set_parameter_request); + if (deserialize_success == false) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Set Parameters Request : Failed to deserialize request.", msg_prefix); + break; + } + + if (set_parameter_request.parameters_size > 8U) { + break; + } + + // Set parameters and responses for each one requested + set_parameter_response.results_size = set_parameter_request.parameters_size; + for (size_t i = 0; i < set_parameter_request.parameters_size; i++) { + param = set_parameter_request.parameters[i]; + + enum ap_var_type var_type; + + // set parameter + AP_Param *vp; + char param_key[AP_MAX_NAME_SIZE + 1]; + strncpy(param_key, (char *)param.name, AP_MAX_NAME_SIZE); + param_key[AP_MAX_NAME_SIZE] = 0; + + // Currently only integer and double value types can be set. + // The following parameter value types are not handled: + // bool, string, byte_array, bool_array, integer_array, double_array and string_array + bool param_isnan = true; + bool param_isinf = true; + float param_value; + switch (param.value.type) { + case PARAMETER_INTEGER: { + param_isnan = isnan(param.value.integer_value); + param_isinf = isinf(param.value.integer_value); + param_value = float(param.value.integer_value); + break; + } + case PARAMETER_DOUBLE: { + param_isnan = isnan(param.value.double_value); + param_isinf = isinf(param.value.double_value); + param_value = float(param.value.double_value); + break; + } + default: { + break; + } + } + + // find existing param to get the old value + uint16_t parameter_flags = 0; + vp = AP_Param::find(param_key, &var_type, ¶meter_flags); + if (vp == nullptr || param_isnan || param_isinf) { + set_parameter_response.results[i].successful = false; + strncpy(set_parameter_response.results[i].reason, "Parameter not found", sizeof(set_parameter_response.results[i].reason)); + continue; + } + + // Add existing parameter checks used in GCS_Param.cpp + if (parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) { + // The user can set BRD_OPTIONS to enable set of internal + // parameters, for developer testing or unusual use cases + if (AP_BoardConfig::allow_set_internal_parameters()) { + parameter_flags &= ~AP_PARAM_FLAG_INTERNAL_USE_ONLY; + } + } + + if ((parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) || vp->is_read_only()) { + set_parameter_response.results[i].successful = false; + strncpy(set_parameter_response.results[i].reason, "Parameter is read only",sizeof(set_parameter_response.results[i].reason)); + continue; + } + + // Set and save the value if it changed + bool force_save = vp->set_and_save_by_name_ifchanged(param_key, param_value); + + if (force_save && (parameter_flags & AP_PARAM_FLAG_ENABLE)) { + AP_Param::invalidate_count(); + } + + set_parameter_response.results[i].successful = true; + strncpy(set_parameter_response.results[i].reason, "Parameter accepted", sizeof(set_parameter_response.results[i].reason)); + } + + const uxrObjectId replier_id = { + .id = services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id, + .type = UXR_REPLIER_ID + }; + + const uint32_t reply_size = rcl_interfaces_srv_SetParameters_Response_size_of_topic(&set_parameter_response, 0U); + uint8_t reply_buffer[reply_size]; + memset(reply_buffer, 0, reply_size * sizeof(uint8_t)); + ucdrBuffer reply_ub; + + ucdr_init_buffer(&reply_ub, reply_buffer, reply_size); + const bool serialize_success = rcl_interfaces_srv_SetParameters_Response_serialize_topic(&reply_ub, &set_parameter_response); + if (serialize_success == false) { + break; + } + + uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub)); + bool successful_params = true; + for (size_t i = 0; i < set_parameter_response.results_size; i++) { + // Check that all the parameters were set successfully + successful_params &= set_parameter_response.results[i].successful; + } + GCS_SEND_TEXT(successful_params ? MAV_SEVERITY_INFO : MAV_SEVERITY_WARNING, "%s Set Parameters Request : %s", msg_prefix, successful_params ? "SUCCESSFUL" : "ONE OR MORE PARAMS FAILED" ); + break; + } + case services[to_underlying(ServiceIndex::GET_PARAMETERS)].rep_id: { + const bool deserialize_success = rcl_interfaces_srv_GetParameters_Request_deserialize_topic(ub, &get_parameters_request); + if (deserialize_success == false) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Get Parameters Request : Failed to deserialize request.", msg_prefix); + break; + } + + if (get_parameters_request.names_size > 8U) { + break; + } + + bool successful_read = true; + get_parameters_response.values_size = get_parameters_request.names_size; + for (size_t i = 0; i < get_parameters_request.names_size; i++) { + enum ap_var_type var_type; + + AP_Param *vp; + char param_key[AP_MAX_NAME_SIZE + 1]; + strncpy(param_key, (char *)get_parameters_request.names[i], AP_MAX_NAME_SIZE); + param_key[AP_MAX_NAME_SIZE] = 0; + + vp = AP_Param::find(param_key, &var_type); + if (vp == nullptr) { + get_parameters_response.values[i].type = PARAMETER_NOT_SET; + successful_read &= false; + continue; + } + + switch (var_type) { + case AP_PARAM_INT8: { + get_parameters_response.values[i].type = PARAMETER_INTEGER; + get_parameters_response.values[i].integer_value = ((AP_Int8 *)vp)->get(); + successful_read &= true; + break; + } + case AP_PARAM_INT16: { + get_parameters_response.values[i].type = PARAMETER_INTEGER; + get_parameters_response.values[i].integer_value = ((AP_Int16 *)vp)->get(); + successful_read &= true; + break; + } + case AP_PARAM_INT32: { + get_parameters_response.values[i].type = PARAMETER_INTEGER; + get_parameters_response.values[i].integer_value = ((AP_Int32 *)vp)->get(); + successful_read &= true; + break; + } + case AP_PARAM_FLOAT: { + get_parameters_response.values[i].type = PARAMETER_DOUBLE; + get_parameters_response.values[i].double_value = vp->cast_to_float(var_type); + successful_read &= true; + break; + } + default: { + get_parameters_response.values[i].type = PARAMETER_NOT_SET; + successful_read &= false; + break; + } + } + } + + const uxrObjectId replier_id = { + .id = services[to_underlying(ServiceIndex::GET_PARAMETERS)].rep_id, + .type = UXR_REPLIER_ID + }; + + const uint32_t reply_size = rcl_interfaces_srv_GetParameters_Response_size_of_topic(&get_parameters_response, 0U); + uint8_t reply_buffer[reply_size]; + memset(reply_buffer, 0, reply_size * sizeof(uint8_t)); + ucdrBuffer reply_ub; + + ucdr_init_buffer(&reply_ub, reply_buffer, reply_size); + const bool serialize_success = rcl_interfaces_srv_GetParameters_Response_serialize_topic(&reply_ub, &get_parameters_response); + if (serialize_success == false) { + break; + } + + uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub)); + + GCS_SEND_TEXT(successful_read ? MAV_SEVERITY_INFO : MAV_SEVERITY_WARNING, "%s Get Parameters Request : %s", msg_prefix, successful_read ? "SUCCESSFUL" : "ONE OR MORE PARAM NOT FOUND"); + break; + } +#endif // AP_DDS_PARAMETER_SERVER_ENABLED } } @@ -947,7 +1206,7 @@ bool AP_DDS_Client::create() .id = 0x01, .type = UXR_PARTICIPANT_ID }; - const char* participant_name = "ardupilot_dds"; + const char* participant_name = AP_DDS_PARTICIPANT_NAME; const auto participant_req_id = uxr_buffer_create_participant_bin(&session, reliable_out, participant_id, static_cast(domain_id), participant_name, UXR_REPLACE); @@ -1257,6 +1516,23 @@ void AP_DDS_Client::write_gps_global_origin_topic() } #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED +#if AP_DDS_STATUS_PUB_ENABLED +void AP_DDS_Client::write_status_topic() +{ + WITH_SEMAPHORE(csem); + if (connected) { + ucdrBuffer ub {}; + const uint32_t topic_size = ardupilot_msgs_msg_Status_size_of_topic(&status_topic, 0); + uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::STATUS_PUB)].dw_id, &ub, topic_size); + const bool success = ardupilot_msgs_msg_Status_serialize_topic(&ub, &status_topic); + if (!success) { + // TODO sometimes serialization fails on bootup. Determine why. + // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + } + } +} +#endif // AP_DDS_STATUS_PUB_ENABLED + void AP_DDS_Client::update() { WITH_SEMAPHORE(csem); @@ -1336,6 +1612,14 @@ void AP_DDS_Client::update() write_gps_global_origin_topic(); } #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED +#if AP_DDS_STATUS_PUB_ENABLED + if (cur_time_ms - last_status_check_time_ms > DELAY_STATUS_TOPIC_MS) { + if (update_topic(status_topic)) { + write_status_topic(); + } + last_status_check_time_ms = cur_time_ms; + } +#endif // AP_DDS_STATUS_PUB_ENABLED status_ok = uxr_run_session_time(&session, 1); } diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 92bdbf2a4bd30..8453577ec1edd 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -25,6 +25,9 @@ #if AP_DDS_IMU_PUB_ENABLED #include "sensor_msgs/msg/Imu.h" #endif // AP_DDS_IMU_PUB_ENABLED +#if AP_DDS_STATUS_PUB_ENABLED +#include "ardupilot_msgs/msg/Status.h" +#endif // AP_DDS_STATUS_PUB_ENABLED #if AP_DDS_JOY_SUB_ENABLED #include "sensor_msgs/msg/Joy.h" #endif // AP_DDS_JOY_SUB_ENABLED @@ -46,6 +49,14 @@ #if AP_DDS_CLOCK_PUB_ENABLED #include "rosgraph_msgs/msg/Clock.h" #endif // AP_DDS_CLOCK_PUB_ENABLED +#if AP_DDS_PARAMETER_SERVER_ENABLED +#include "rcl_interfaces/srv/SetParameters.h" +#include "rcl_interfaces/msg/Parameter.h" +#include "rcl_interfaces/msg/SetParametersResult.h" +#include "rcl_interfaces/msg/ParameterValue.h" +#include "rcl_interfaces/msg/ParameterType.h" +#include "rcl_interfaces/srv/GetParameters.h" +#endif //AP_DDS_PARAMETER_SERVER_ENABLED #include #include @@ -175,6 +186,17 @@ class AP_DDS_Client static void update_topic(rosgraph_msgs_msg_Clock& msg); #endif // AP_DDS_CLOCK_PUB_ENABLED +#if AP_DDS_STATUS_PUB_ENABLED + ardupilot_msgs_msg_Status status_topic; + bool update_topic(ardupilot_msgs_msg_Status& msg); + // The last ms timestamp AP_DDS wrote/checked a status message + uint64_t last_status_check_time_ms; + // last status values; + ardupilot_msgs_msg_Status last_status_msg_; + //! @brief Serialize the current status and publish to the IO stream(s) + void write_status_topic(); +#endif // AP_DDS_STATUS_PUB_ENABLED + #if AP_DDS_STATIC_TF_PUB_ENABLED // outgoing transforms tf2_msgs_msg_TFMessage tx_static_transforms_topic; @@ -201,6 +223,14 @@ class AP_DDS_Client #endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED HAL_Semaphore csem; +#if AP_DDS_PARAMETER_SERVER_ENABLED + static rcl_interfaces_srv_SetParameters_Request set_parameter_request; + static rcl_interfaces_srv_SetParameters_Response set_parameter_response; + static rcl_interfaces_srv_GetParameters_Request get_parameters_request; + static rcl_interfaces_srv_GetParameters_Response get_parameters_response; + static rcl_interfaces_msg_Parameter param; +#endif + // connection parametrics bool status_ok{false}; bool connected{false}; @@ -255,6 +285,7 @@ class AP_DDS_Client // client key we present static constexpr uint32_t key = 0xAAAABBBB; + public: ~AP_DDS_Client(); diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h index 0e86496dd5777..667149d6aa756 100644 --- a/libraries/AP_DDS/AP_DDS_Service_Table.h +++ b/libraries/AP_DDS/AP_DDS_Service_Table.h @@ -6,8 +6,12 @@ enum class ServiceIndex: uint8_t { ARMING_MOTORS, #endif // #if AP_DDS_ARM_SERVER_ENABLED #if AP_DDS_MODE_SWITCH_SERVER_ENABLED - MODE_SWITCH + MODE_SWITCH, #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED +#if AP_DDS_PARAMETER_SERVER_ENABLED + SET_PARAMETERS, + GET_PARAMETERS +#endif // AP_DDS_PARAMETER_SERVER_ENABLED }; static inline constexpr uint8_t to_underlying(const ServiceIndex index) @@ -47,4 +51,26 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = { .reply_topic_name = "rr/ap/mode_switchReply", }, #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED +#if AP_DDS_PARAMETER_SERVER_ENABLED + { + .req_id = to_underlying(ServiceIndex::SET_PARAMETERS), + .rep_id = to_underlying(ServiceIndex::SET_PARAMETERS), + .service_rr = Service_rr::Replier, + .service_name = "rs/ap/set_parametersService", + .request_type = "rcl_interfaces::srv::dds_::SetParameters_Request_", + .reply_type = "rcl_interfaces::srv::dds_::SetParameters_Response_", + .request_topic_name = "rq/ap/set_parametersRequest", + .reply_topic_name = "rr/ap/set_parametersReply", + }, + { + .req_id = to_underlying(ServiceIndex::GET_PARAMETERS), + .rep_id = to_underlying(ServiceIndex::GET_PARAMETERS), + .service_rr = Service_rr::Replier, + .service_name = "rs/ap/get_parameterService", + .request_type = "rcl_interfaces::srv::dds_::GetParameters_Request_", + .reply_type = "rcl_interfaces::srv::dds_::GetParameters_Response_", + .request_topic_name = "rq/ap/get_parametersRequest", + .reply_topic_name = "rr/ap/get_parametersReply", + }, +#endif // AP_DDS_PARAMETER_SERVER_ENABLED }; diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index f578d2e8a486b..b9bde199dc050 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -48,6 +48,9 @@ enum class TopicIndex: uint8_t { #if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED GPS_GLOBAL_ORIGIN_PUB, #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED +#if AP_DDS_STATUS_PUB_ENABLED + STATUS_PUB, +#endif // AP_DDS_STATUS_PUB_ENABLED #if AP_DDS_JOY_SUB_ENABLED JOY_SUB, #endif // AP_DDS_JOY_SUB_ENABLED @@ -268,6 +271,24 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { }, }, #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED +#if AP_DDS_STATUS_PUB_ENABLED + { + .topic_id = to_underlying(TopicIndex::STATUS_PUB), + .pub_id = to_underlying(TopicIndex::STATUS_PUB), + .sub_id = to_underlying(TopicIndex::STATUS_PUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::STATUS_PUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::STATUS_PUB), .type=UXR_DATAREADER_ID}, + .topic_rw = Topic_rw::DataWriter, + .topic_name = "rt/ap/status", + .type_name = "ardupilot_msgs::msg::dds_::Status_", + .qos = { + .durability = UXR_DURABILITY_TRANSIENT_LOCAL, + .reliability = UXR_RELIABILITY_RELIABLE, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 1, + }, + }, +#endif // AP_DDS_STATUS_PUB_ENABLED #if AP_DDS_JOY_SUB_ENABLED { .topic_id = to_underlying(TopicIndex::JOY_SUB), diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h index b8f31e368c8a4..5785ca23b1212 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -94,6 +94,10 @@ #define AP_DDS_DELAY_BATTERY_STATE_TOPIC_MS 1000 #endif +#ifndef AP_DDS_DELAY_STATUS_TOPIC_MS +#define AP_DDS_DELAY_STATUS_TOPIC_MS 100 +#endif + #ifndef AP_DDS_CLOCK_PUB_ENABLED #define AP_DDS_CLOCK_PUB_ENABLED 1 #endif @@ -102,6 +106,10 @@ #define AP_DDS_DELAY_CLOCK_TOPIC_MS 10 #endif +#ifndef AP_DDS_STATUS_PUB_ENABLED +#define AP_DDS_STATUS_PUB_ENABLED 1 +#endif + #ifndef AP_DDS_JOY_SUB_ENABLED #define AP_DDS_JOY_SUB_ENABLED 1 #endif @@ -126,6 +134,10 @@ #define AP_DDS_MODE_SWITCH_SERVER_ENABLED 1 #endif +#ifndef AP_DDS_PARAMETER_SERVER_ENABLED +#define AP_DDS_PARAMETER_SERVER_ENABLED 1 +#endif + // Whether to include Twist support #define AP_DDS_NEEDS_TWIST AP_DDS_VEL_CTRL_ENABLED || AP_DDS_LOCAL_VEL_PUB_ENABLED @@ -139,3 +151,7 @@ #define AP_DDS_DEFAULT_UDP_IP_ADDR "127.0.0.1" #endif #endif + +#ifndef AP_DDS_PARTICIPANT_NAME +#define AP_DDS_PARTICIPANT_NAME "ap" +#endif diff --git a/libraries/AP_DDS/Idl/ardupilot_msgs/msg/Status.idl b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/Status.idl new file mode 100644 index 0000000000000..a06a0da8cb2e6 --- /dev/null +++ b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/Status.idl @@ -0,0 +1,56 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from ardupilot_msgs/msg/Status.msg +// generated code does not contain a copyright notice + +#include "std_msgs/msg/Header.idl" + +module ardupilot_msgs { + module msg { + module Status_Constants { + const uint8 APM_ROVER = 1; + const uint8 APM_ARDUCOPTER = 2; + const uint8 APM_ARDUPLANE = 3; + const uint8 APM_ANTENNATRACKER = 4; + const uint8 APM_UNKNOWN = 5; + const uint8 APM_REPLAY = 6; + const uint8 APM_ARDUSUB = 7; + const uint8 APM_IOFIRMWARE = 8; + const uint8 APM_AP_PERIPH = 9; + const uint8 APM_AP_DAL_STANDALONE = 10; + const uint8 APM_AP_BOOTLOADER = 11; + const uint8 APM_BLIMP = 12; + const uint8 APM_HELI = 13; + const uint8 FS_RADIO = 21; + const uint8 FS_BATTERY = 22; + const uint8 FS_GCS = 23; + const uint8 FS_EKF = 24; + }; + struct Status { + std_msgs::msg::Header header; + + @verbatim (language="comment", text= + "From AP_Vehicle_Type.h") + uint8 vehicle_type; + + @verbatim (language="comment", text= + "true if vehicle is armed.") + boolean armed; + + @verbatim (language="comment", text= + "Vehicle mode, enum depending on vehicle type.") + uint8 mode; + + @verbatim (language="comment", text= + "True if flying/driving/diving/tracking.") + boolean flying; + + @verbatim (language="comment", text= + "True is external control is enabled.") + boolean external_control; + + @verbatim (language="comment", text= + "Array containing all active failsafe.") + sequence failsafe; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/rcl_interfaces/msg/Parameter.idl b/libraries/AP_DDS/Idl/rcl_interfaces/msg/Parameter.idl new file mode 100644 index 0000000000000..992213a287458 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/msg/Parameter.idl @@ -0,0 +1,23 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from rcl_interfaces/msg/Parameter.msg +// generated code does not contain a copyright notice + +#include "rcl_interfaces/msg/ParameterValue.idl" + +module rcl_interfaces { + module msg { + @verbatim (language="comment", text= + "This is the message to communicate a parameter. It is an open struct with an enum in" "\n" + "the descriptor to select which value is active.") + struct Parameter { + @verbatim (language="comment", text= + "The full name of the parameter.") + string name; + + @verbatim (language="comment", text= + "The parameter's value which can be one of several types, see" "\n" + "`ParameterValue.msg` and `ParameterType.msg`.") + rcl_interfaces::msg::ParameterValue value; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterType.idl b/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterType.idl new file mode 100644 index 0000000000000..e6a6bfcca7da7 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterType.idl @@ -0,0 +1,28 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from rcl_interfaces/msg/ParameterType.msg +// generated code does not contain a copyright notice + + +module rcl_interfaces { + module msg { + module ParameterType_Constants { + @verbatim (language="comment", text= + "Default value, which implies this is not a valid parameter.") + const uint8 PARAMETER_NOT_SET = 0; + const uint8 PARAMETER_BOOL = 1; + const uint8 PARAMETER_INTEGER = 2; + const uint8 PARAMETER_DOUBLE = 3; + const uint8 PARAMETER_STRING = 4; + const uint8 PARAMETER_BYTE_ARRAY = 5; + const uint8 PARAMETER_BOOL_ARRAY = 6; + const uint8 PARAMETER_INTEGER_ARRAY = 7; + const uint8 PARAMETER_DOUBLE_ARRAY = 8; + const uint8 PARAMETER_STRING_ARRAY = 9; + }; + @verbatim (language="comment", text= + "These types correspond to the value that is set in the ParameterValue message.") + struct ParameterType { + uint8 structure_needs_at_least_one_member; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterValue.idl b/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterValue.idl new file mode 100644 index 0000000000000..3adbc5233cac2 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterValue.idl @@ -0,0 +1,58 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from rcl_interfaces/msg/ParameterValue.msg +// generated code does not contain a copyright notice + + +module rcl_interfaces { + module msg { + @verbatim (language="comment", text= + "Used to determine which of the next *_value fields are set." "\n" + "ParameterType.PARAMETER_NOT_SET indicates that the parameter was not set" "\n" + "(if gotten) or is uninitialized." "\n" + "Values are enumerated in `ParameterType.msg`.") + struct ParameterValue { + @verbatim (language="comment", text= + "The type of this parameter, which corresponds to the appropriate field below.") + uint8 type; + + @verbatim (language="comment", text= + "\"Variant\" style storage of the parameter value. Only the value corresponding" "\n" + "the type field will have valid information." "\n" + "Boolean value, can be either true or false.") + boolean bool_value; + + @verbatim (language="comment", text= + "Integer value ranging from -9,223,372,036,854,775,808 to" "\n" + "9,223,372,036,854,775,807.") + int64 integer_value; + + @verbatim (language="comment", text= + "A double precision floating point value following IEEE 754.") + double double_value; + + @verbatim (language="comment", text= + "A textual value with no practical length limit.") + string string_value; + + @verbatim (language="comment", text= + "An array of bytes, used for non-textual information.") + sequence byte_array_value; + + @verbatim (language="comment", text= + "An array of boolean values.") + sequence bool_array_value; + + @verbatim (language="comment", text= + "An array of 64-bit integer values.") + sequence integer_array_value; + + @verbatim (language="comment", text= + "An array of 64-bit floating point values.") + sequence double_array_value; + + @verbatim (language="comment", text= + "An array of string values.") + sequence string_array_value; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/rcl_interfaces/msg/SetParametersResult.idl b/libraries/AP_DDS/Idl/rcl_interfaces/msg/SetParametersResult.idl new file mode 100644 index 0000000000000..d1c68fe1200f1 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/msg/SetParametersResult.idl @@ -0,0 +1,20 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from rcl_interfaces/msg/SetParametersResult.msg +// generated code does not contain a copyright notice + + +module rcl_interfaces { + module msg { + @verbatim (language="comment", text= + "A true value of the same index indicates that the parameter was set" "\n" + "successfully. A false value indicates the change was rejected.") + struct SetParametersResult { + boolean successful; + + @verbatim (language="comment", text= + "Reason why the setting was either successful or a failure. This should only be" "\n" + "used for logging and user interfaces.") + string reason; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/rcl_interfaces/srv/GetParameters.idl b/libraries/AP_DDS/Idl/rcl_interfaces/srv/GetParameters.idl new file mode 100644 index 0000000000000..d6579f51b2c12 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/srv/GetParameters.idl @@ -0,0 +1,29 @@ +// generated from rosidl_adapter/resource/srv.idl.em +// with input from rcl_interfaces/srv/GetParameters.srv +// generated code does not contain a copyright notice + +#include "rcl_interfaces/msg/ParameterValue.idl" + +module rcl_interfaces { + module srv { + @verbatim (language="comment", text= + "TODO(wjwwood): Decide on the rules for grouping, nodes, and parameter \"names\"" "\n" + "in general, then link to that." "\n" + "" "\n" + "For more information about parameters and naming rules, see:" "\n" + "https://design.ros2.org/articles/ros_parameters.html" "\n" + "https://github.com/ros2/design/pull/241") + struct GetParameters_Request { + @verbatim (language="comment", text= + "A list of parameter names to get.") + sequence names; + }; + @verbatim (language="comment", text= + "List of values which is the same length and order as the provided names. If a" "\n" + "parameter was not yet set, the value will have PARAMETER_NOT_SET as the" "\n" + "type.") + struct GetParameters_Response { + sequence values; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/rcl_interfaces/srv/SetParameters.idl b/libraries/AP_DDS/Idl/rcl_interfaces/srv/SetParameters.idl new file mode 100644 index 0000000000000..ade6df7994030 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/srv/SetParameters.idl @@ -0,0 +1,21 @@ +// generated from rosidl_adapter/resource/srv.idl.em +// with input from rcl_interfaces/srv/SetParameters.srv +// generated code does not contain a copyright notice + +#include "rcl_interfaces/msg/Parameter.idl" +#include "rcl_interfaces/msg/SetParametersResult.idl" + +module rcl_interfaces { + module srv { + @verbatim (language="comment", text= + "A list of parameters to set.") + struct SetParameters_Request { + sequence parameters; + }; + @verbatim (language="comment", text= + "Indicates whether setting each parameter succeeded or not and why.") + struct SetParameters_Response { + sequence results; + }; + }; +}; diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index 61aed38a6d5a3..03a1aa92938bf 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -364,10 +364,10 @@ for additional details. ### Development Requirements Astyle is used to format the C++ code in AP_DDS. This is required for CI to pass the build. -See [Tools/CodeStyle/ardupilot-astyle.sh](../../Tools/CodeStyle/ardupilot-astyle.sh). +To run the automated formatter, run: ```bash -./Tools/CodeStyle/ardupilot-astyle.sh libraries/AP_DDS/*.h libraries/AP_DDS/*.cpp +./Tools/scripts/run_astyle.py ``` Pre-commit is used for other things like formatting python and XML code. diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.cpp b/libraries/AP_DroneCAN/AP_Canard_iface.cpp index 7382f57033c9d..fd38e303c4253 100644 --- a/libraries/AP_DroneCAN/AP_Canard_iface.cpp +++ b/libraries/AP_DroneCAN/AP_Canard_iface.cpp @@ -456,7 +456,7 @@ bool CanardInterface::add_11bit_driver(CANSensor *sensor) } // handler for outgoing frames for auxillary drivers -bool CanardInterface::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) +bool CanardInterface::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us) { const uint64_t tx_deadline_us = AP_HAL::micros64() + timeout_us; bool ret = false; diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.h b/libraries/AP_DroneCAN/AP_Canard_iface.h index 43d786639fdf0..671722028dffc 100644 --- a/libraries/AP_DroneCAN/AP_Canard_iface.h +++ b/libraries/AP_DroneCAN/AP_Canard_iface.h @@ -54,7 +54,7 @@ class CanardInterface : public Canard::Interface { bool add_11bit_driver(CANSensor *sensor); // handler for outgoing frames for auxillary drivers - bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us); + bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us); #if AP_TEST_DRONECAN_DRIVERS static CanardInterface& get_test_iface() { return test_iface; } diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 21243e5d08c40..639e017e82cb6 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -1956,7 +1956,7 @@ bool AP_DroneCAN::add_11bit_driver(CANSensor *sensor) } // handler for outgoing frames for auxillary drivers -bool AP_DroneCAN::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) +bool AP_DroneCAN::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us) { if (out_frame.isExtended()) { // don't allow extended frames to be sent by auxillary driver diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index 2d6e5ae2b7f78..9a7ae54c6639c 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -95,7 +95,7 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { bool add_11bit_driver(CANSensor *sensor) override; // handler for outgoing frames for auxillary drivers - bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) override; + bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us) override; uint8_t get_driver_index() const { return _driver_index; } diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 070276dd45c9f..79a69b88b2962 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1367,6 +1367,7 @@ uint16_t AP_GPS::gps_yaw_cdeg(uint8_t instance) const return yaw_cd; } +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) { const Location &loc = location(0); @@ -1400,8 +1401,9 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) 0, // TODO one-sigma heading accuracy standard deviation gps_yaw_cdeg(0)); } +#endif // AP_GPS_GPS_RAW_INT_SENDING_ENABLED -#if GPS_MAX_RECEIVERS > 1 +#if AP_GPS_GPS2_RAW_SENDING_ENABLED void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) { // always send the message if 2nd GPS is configured @@ -1442,9 +1444,9 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) sacc * 1000, // one-sigma standard deviation in mm/s 0); // TODO one-sigma heading accuracy standard deviation } -#endif // GPS_MAX_RECEIVERS +#endif // AP_GPS_GPS2_RAW_SENDING_ENABLED -#if HAL_GCS_ENABLED +#if AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst) { if (inst >= GPS_MAX_RECEIVERS) { diff --git a/libraries/AP_GPS/AP_GPS_Blended.h b/libraries/AP_GPS/AP_GPS_Blended.h index b68eb0f9660ed..77fe25ffb8884 100644 --- a/libraries/AP_GPS/AP_GPS_Blended.h +++ b/libraries/AP_GPS/AP_GPS_Blended.h @@ -64,7 +64,6 @@ class AP_GPS_Blended : public AP_GPS_Backend Vector3f _blended_antenna_offset; // blended antenna offset float _blended_lag_sec; // blended receiver lag in seconds float _blend_weights[GPS_MAX_RECEIVERS]; // blend weight for each GPS. The blend weights must sum to 1.0 across all instances. - bool _output_is_blended; // true when a blended GPS solution being output uint8_t _blend_health_counter; // 0 = perfectly health, 100 = very unhealthy AP_GPS::GPS_timing &timing; diff --git a/libraries/AP_GPS/AP_GPS_MAV.cpp b/libraries/AP_GPS/AP_GPS_MAV.cpp index b9100f5f17c46..2a059af35b018 100644 --- a/libraries/AP_GPS/AP_GPS_MAV.cpp +++ b/libraries/AP_GPS/AP_GPS_MAV.cpp @@ -143,51 +143,8 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg) state.last_gps_time_ms = now_ms; _new_data = true; break; - } - -#if AP_MAVLINK_MSG_HIL_GPS_ENABLED - case MAVLINK_MSG_ID_HIL_GPS: { - mavlink_hil_gps_t packet; - mavlink_msg_hil_gps_decode(&msg, &packet); - - // check if target instance belongs to incoming gps data. - if (state.instance != packet.id) { - return; - } - - state.time_week = 0; - state.time_week_ms = packet.time_usec/1000; - state.status = (AP_GPS::GPS_Status)packet.fix_type; + } - Location loc = {}; - loc.lat = packet.lat; - loc.lng = packet.lon; - loc.alt = packet.alt * 0.1f; - state.location = loc; - state.hdop = MIN(packet.eph, GPS_UNKNOWN_DOP); - state.vdop = MIN(packet.epv, GPS_UNKNOWN_DOP); - if (packet.vel < 65535) { - state.ground_speed = packet.vel * 0.01f; - } - Vector3f vel(packet.vn*0.01f, packet.ve*0.01f, packet.vd*0.01f); - state.velocity = vel; - if (packet.vd != 0) { - state.have_vertical_velocity = true; - } - if (packet.cog < 36000) { - state.ground_course = packet.cog * 0.01f; - } - state.have_speed_accuracy = false; - state.have_horizontal_accuracy = false; - state.have_vertical_accuracy = false; - if (packet.satellites_visible < 255) { - state.num_sats = packet.satellites_visible; - } - state.last_gps_time_ms = AP_HAL::millis(); - _new_data = true; - break; - } -#endif // AP_MAVLINK_MSG_HIL_GPS_ENABLED default: // ignore all other messages break; diff --git a/libraries/AP_GPS/AP_GPS_config.h b/libraries/AP_GPS/AP_GPS_config.h index 08ebdac605938..550add0e1fa62 100644 --- a/libraries/AP_GPS/AP_GPS_config.h +++ b/libraries/AP_GPS/AP_GPS_config.h @@ -104,3 +104,20 @@ #ifndef HAL_GPS_COM_PORT_DEFAULT #define HAL_GPS_COM_PORT_DEFAULT 1 #endif + + +#ifndef AP_GPS_GPS_RAW_INT_SENDING_ENABLED +#define AP_GPS_GPS_RAW_INT_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED +#endif + +#ifndef AP_GPS_GPS2_RAW_SENDING_ENABLED +#define AP_GPS_GPS2_RAW_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED && GPS_MAX_RECEIVERS > 1 +#endif + +#ifndef AP_GPS_GPS_RTK_SENDING_ENABLED +#define AP_GPS_GPS_RTK_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED && (AP_GPS_SBF_ENABLED || AP_GPS_ERB_ENABLED) +#endif + +#ifndef AP_GPS_GPS2_RTK_SENDING_ENABLED +#define AP_GPS_GPS2_RTK_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED && GPS_MAX_RECEIVERS > 1 && (AP_GPS_SBF_ENABLED || AP_GPS_ERB_ENABLED) +#endif diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index eb55bd76e433d..380730a9af679 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -172,7 +172,7 @@ bool AP_GPS_Backend::should_log() const #endif -#if HAL_GCS_ENABLED +#if AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan) { const uint8_t instance = state.instance; @@ -212,7 +212,8 @@ void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan) break; } } -#endif +#endif // AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED + /* diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 70f4cb36fc215..2a70a1c57153e 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -71,7 +71,9 @@ class AP_GPS_Backend #if HAL_GCS_ENABLED //MAVLink methods virtual bool supports_mavlink_gps_rtk_message() const { return false; } +#if AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED virtual void send_mavlink_gps_rtk(mavlink_channel_t chan); +#endif virtual void handle_msg(const mavlink_message_t &msg) { return ; } #endif diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 1c3aa1b1648e7..26a47e2c8cd99 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -6,6 +6,7 @@ */ #pragma once +// @LoggerEnum: HAL_BOARD #define HAL_BOARD_SITL 3 // #define HAL_BOARD_SMACCM 4 // unused // #define HAL_BOARD_PX4 5 // unused @@ -16,7 +17,9 @@ #define HAL_BOARD_ESP32 12 #define HAL_BOARD_QURT 13 #define HAL_BOARD_EMPTY 99 +// @LoggerEnumEnd +// @LoggerEnum: HAL_BOARD_SUBTYPE /* Default board subtype is -1 */ #define HAL_BOARD_SUBTYPE_NONE -1 @@ -70,6 +73,7 @@ #define HAL_BOARD_SUBTYPE_ESP32_NICK 6006 #define HAL_BOARD_SUBTYPE_ESP32_S3DEVKIT 6007 #define HAL_BOARD_SUBTYPE_ESP32_S3EMPTY 6008 +// @LoggerEnumEnd /* InertialSensor driver types */ #define HAL_INS_NONE 0 diff --git a/libraries/AP_HAL/HAL.h b/libraries/AP_HAL/HAL.h index 723e68f64ffc2..a2bef78e470de 100644 --- a/libraries/AP_HAL/HAL.h +++ b/libraries/AP_HAL/HAL.h @@ -67,9 +67,6 @@ class AP_HAL::HAL { scheduler(_scheduler), util(_util), opticalflow(_opticalflow), -#if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL - simstate(_simstate), -#endif flash(_flash), #if HAL_WITH_DSP dsp(_dsp), @@ -85,6 +82,9 @@ class AP_HAL::HAL { _serial7, _serial8, _serial9} +#if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL + ,simstate(_simstate) +#endif { #if HAL_NUM_CAN_IFACES > 0 if (_can_ifaces == nullptr) { diff --git a/libraries/AP_HAL/board/linux.h b/libraries/AP_HAL/board/linux.h index b92ea9c40663b..a7e6c3fc1740f 100644 --- a/libraries/AP_HAL/board/linux.h +++ b/libraries/AP_HAL/board/linux.h @@ -57,6 +57,7 @@ #define HAL_LINUX_HEAT_TARGET_TEMP 50 #define BEBOP_CAMV_PWM 9 #define BEBOP_CAMV_PWM_FREQ 43333333 + #define OPTICAL_FLOW_TYPE_DEFAULT Type::BEBOP #define HAL_OPTFLOW_ONBOARD_VDEV_PATH "/dev/video0" #define HAL_OPTFLOW_ONBOARD_SUBDEV_PATH "/dev/v4l-subdev0" #define HAL_OPTFLOW_ONBOARD_SENSOR_WIDTH 320 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BetaflightF4/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/BetaflightF4/hwdef-bl.dat index 5e51f7c451b50..c0b6a31c25946 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/BetaflightF4/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/BetaflightF4/hwdef-bl.dat @@ -7,7 +7,7 @@ MCU STM32F4xx STM32F405xx # board ID for firmware load -APJ_BOARD_ID 1176 +APJ_BOARD_ID 1177 # crystal frequency, setup to use external oscillator OSCILLATOR_HZ 8000000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BetaflightF4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/BetaflightF4/hwdef.dat index 3d2078191060b..6371049362647 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/BetaflightF4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/BetaflightF4/hwdef.dat @@ -7,7 +7,7 @@ MCU STM32F4xx STM32F405xx # board ID for firmware load -APJ_BOARD_ID 1176 +APJ_BOARD_ID 1177 # crystal frequency, setup to use external oscillator OSCILLATOR_HZ 8000000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc index cb5769d8d514b..f7d85e808225c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc @@ -212,17 +212,8 @@ PE15 VDD_5V_PERIPH_nOC INPUT PULLUP SPIDEV ms5611 SPI1 DEVID3 BARO_CS MODE3 20*MHZ 20*MHZ SPIDEV ms5611_ext SPI4 DEVID2 BARO_EXT_CS MODE3 20*MHZ 20*MHZ -SPIDEV mpu6000 SPI1 DEVID4 MPU_CS MODE3 2*MHZ 8*MHZ -SPIDEV icm20608-am SPI1 DEVID2 ACCEL_EXT_CS MODE3 4*MHZ 8*MHZ -SPIDEV mpu9250 SPI1 DEVID4 MPU_CS MODE3 4*MHZ 8*MHZ -SPIDEV mpu9250_ext SPI4 DEVID1 MPU_EXT_CS MODE3 4*MHZ 8*MHZ SPIDEV icm20948 SPI1 DEVID4 MPU_CS MODE3 4*MHZ 8*MHZ SPIDEV icm20948_ext SPI4 DEVID1 MPU_EXT_CS MODE3 4*MHZ 8*MHZ -SPIDEV hmc5843 SPI1 DEVID5 MAG_CS MODE3 11*MHZ 11*MHZ -SPIDEV lsm9ds0_g SPI1 DEVID1 GYRO_EXT_CS MODE3 11*MHZ 11*MHZ -SPIDEV lsm9ds0_am SPI1 DEVID2 ACCEL_EXT_CS MODE3 11*MHZ 11*MHZ -SPIDEV lsm9ds0_ext_g SPI4 DEVID4 GYRO_EXT_CS MODE3 11*MHZ 11*MHZ -SPIDEV lsm9ds0_ext_am SPI4 DEVID3 ACCEL_EXT_CS MODE3 11*MHZ 11*MHZ SPIDEV icm20602_ext SPI4 DEVID4 GYRO_EXT_CS MODE3 4*MHZ 8*MHZ SPIDEV ramtron SPI2 DEVID10 FRAM_CS MODE3 8*MHZ 8*MHZ SPIDEV external0m0 SPI4 DEVID5 MPU_EXT_CS MODE0 2*MHZ 2*MHZ @@ -231,18 +222,7 @@ SPIDEV external0m2 SPI4 DEVID5 MPU_EXT_CS MODE2 2*MHZ 2*MHZ SPIDEV external0m3 SPI4 DEVID5 MPU_EXT_CS MODE3 2*MHZ 2*MHZ SPIDEV pixartPC15 SPI4 DEVID13 ACCEL_EXT_CS MODE3 2*MHZ 2*MHZ -# three IMUs, but allow for different variants. First two IMUs are -# isolated, 3rd isn't -IMU Invensense SPI:mpu9250_ext ROTATION_PITCH_180 - -# the 3 rotations for the LSM9DS0 driver are for the accel, the gyro -# and the H variant of the gyro -IMU LSM9DS0 SPI:lsm9ds0_ext_g SPI:lsm9ds0_ext_am ROTATION_ROLL_180_YAW_270 ROTATION_ROLL_180_YAW_90 ROTATION_ROLL_180_YAW_90 - -# 3rd non-isolated IMU -IMU Invensense SPI:mpu9250 ROTATION_YAW_270 - -# alternative IMU set for newer cubes +# IMU set for newer cubes IMU Invensense SPI:icm20602_ext ROTATION_ROLL_180_YAW_270 IMU Invensensev2 SPI:icm20948_ext ROTATION_PITCH_180 IMU Invensensev2 SPI:icm20948 ROTATION_YAW_270 @@ -270,12 +250,7 @@ define HAL_DEFAULT_INS_FAST_SAMPLE 7 BARO MS56XX SPI:ms5611_ext BARO MS56XX SPI:ms5611 -# two compasses. First is in the LSM303D -COMPASS LSM303D SPI:lsm9ds0_ext_am ROTATION_YAW_270 -# 2nd compass is part of the 2nd invensense IMU -COMPASS AK8963:probe_mpu9250 1 ROTATION_YAW_270 - -# compass as part of ICM20948 on newer cubes +# one internal compass COMPASS AK09916:probe_ICM20948 0 ROTATION_ROLL_180_YAW_90 # offset the internal compass for EM impact of the IMU heater diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat index afc44a107c605..55ac0d221db2c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat @@ -351,17 +351,8 @@ PE15 VDD_5V_PERIPH_nOC INPUT PULLUP SPIDEV ms5611 SPI1 DEVID3 BARO_CS MODE3 20*MHZ 20*MHZ SPIDEV ms5611_ext SPI4 DEVID2 BARO_EXT_CS MODE3 20*MHZ 20*MHZ -SPIDEV mpu6000 SPI1 DEVID4 MPU_CS MODE3 2*MHZ 8*MHZ -SPIDEV icm20608-am SPI1 DEVID2 ACCEL_EXT_CS MODE3 4*MHZ 8*MHZ -SPIDEV mpu9250 SPI1 DEVID4 MPU_CS MODE3 4*MHZ 8*MHZ -SPIDEV mpu9250_ext SPI4 DEVID1 MPU_EXT_CS MODE3 4*MHZ 8*MHZ SPIDEV icm20948 SPI1 DEVID4 MPU_CS MODE3 4*MHZ 8*MHZ SPIDEV icm20948_ext SPI4 DEVID1 MPU_EXT_CS MODE3 4*MHZ 8*MHZ -SPIDEV hmc5843 SPI1 DEVID5 MAG_CS MODE3 11*MHZ 11*MHZ -SPIDEV lsm9ds0_g SPI1 DEVID1 GYRO_EXT_CS MODE3 11*MHZ 11*MHZ -SPIDEV lsm9ds0_am SPI1 DEVID2 ACCEL_EXT_CS MODE3 11*MHZ 11*MHZ -SPIDEV lsm9ds0_ext_g SPI4 DEVID4 GYRO_EXT_CS MODE3 11*MHZ 11*MHZ -SPIDEV lsm9ds0_ext_am SPI4 DEVID3 ACCEL_EXT_CS MODE3 11*MHZ 11*MHZ SPIDEV icm20602_ext SPI4 DEVID4 GYRO_EXT_CS MODE3 4*MHZ 8*MHZ SPIDEV ramtron SPI2 DEVID10 FRAM_CS MODE3 8*MHZ 8*MHZ SPIDEV external0m0 SPI4 DEVID5 MPU_EXT_CS MODE0 2*MHZ 2*MHZ @@ -382,18 +373,7 @@ SPIDEV pixartPC15 SPI4 DEVID13 ACCEL_EXT_CS MODE3 2*MHZ 2*MHZ #SPIDEV clock8 SPI4 DEVID5 MPU_EXT_CS MODE0 8*MHZ 8*MHZ # gives 5.5MHz #SPIDEV clock16 SPI4 DEVID5 MPU_EXT_CS MODE0 16*MHZ 16*MHZ # gives 10.6MHz -# three IMUs, but allow for different variants. First two IMUs are -# isolated, 3rd isn't -IMU Invensense SPI:mpu9250_ext ROTATION_PITCH_180 - -# the 3 rotations for the LSM9DS0 driver are for the accel, the gyro -# and the H variant of the gyro -IMU LSM9DS0 SPI:lsm9ds0_ext_g SPI:lsm9ds0_ext_am ROTATION_ROLL_180_YAW_270 ROTATION_ROLL_180_YAW_90 ROTATION_ROLL_180_YAW_90 - -# 3rd non-isolated IMU -IMU Invensense SPI:mpu9250 ROTATION_YAW_270 - -# alternative IMU set for newer cubes +#IMU set for newer cubes IMU Invensense SPI:icm20602_ext ROTATION_ROLL_180_YAW_270 IMU Invensensev2 SPI:icm20948_ext ROTATION_PITCH_180 IMU Invensensev2 SPI:icm20948 ROTATION_YAW_270 @@ -404,12 +384,7 @@ define HAL_DEFAULT_INS_FAST_SAMPLE 5 BARO MS56XX SPI:ms5611_ext BARO MS56XX SPI:ms5611 -# two compasses. First is in the LSM303D -COMPASS LSM303D SPI:lsm9ds0_ext_am ROTATION_YAW_270 -# 2nd compass is part of the 2nd invensense IMU -COMPASS AK8963:probe_mpu9250 1 ROTATION_YAW_270 - -# compass as part of ICM20948 on newer cubes +# one compass COMPASS AK09916:probe_ICM20948 0 ROTATION_ROLL_180_YAW_90 # also probe for external compasses diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FoxeerF405v2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FoxeerF405v2/hwdef.dat index cd08660e4922e..8a7d80b127a8a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FoxeerF405v2/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FoxeerF405v2/hwdef.dat @@ -147,6 +147,7 @@ ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin # Barometer setup BARO DPS310 I2C:0:0x76 +BARO BMP280 I2C:0:0x76 # IMU setup diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat index 97e2b86c68c14..8f0f642cdebb7 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat @@ -159,7 +159,9 @@ BARO DPS310 I2C:0:0x76 # IMU setup SPIDEV imu1 SPI2 DEVID1 GYRO1_CS MODE3 1*MHZ 8*MHZ +SPIDEV imu2 SPI2 DEVID1 GYRO1_CS MODE3 1*MHZ 16*MHZ IMU Invensense SPI:imu1 ROTATION_YAW_270 +IMU Invensensev3 SPI:imu2 ROTATION_YAW_180 DMA_NOSHARE TIM3_UP TIM4_UP TIM8_UP SPI2* DMA_PRIORITY TIM3_UP TIM4_UP TIM8_UP SPI2* diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat index 5140f1130b1a9..64d400165f889 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat @@ -130,6 +130,7 @@ define HAL_BARO_20789_I2C_ADDR_ICM 0x68 define HAL_I2C_CLEAR_BUS define CONFIG_HAL_BOARD_SUBTYPE HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 +define OPTICAL_FLOW_TYPE_DEFAULT Type::PIXART # the web UI uses an abin file for firmware uploads env BUILD_ABIN True diff --git a/libraries/AP_HAL_ESP32/AnalogIn.cpp b/libraries/AP_HAL_ESP32/AnalogIn.cpp index 05487e62a0052..7f027a31a31e9 100644 --- a/libraries/AP_HAL_ESP32/AnalogIn.cpp +++ b/libraries/AP_HAL_ESP32/AnalogIn.cpp @@ -141,8 +141,8 @@ void adc_calibration_deinit(adc_cali_handle_t handle) //ardupin is the ardupilot assigned number, starting from 1-8(max) AnalogSource::AnalogSource(int16_t ardupin, adc_channel_t adc_channel, float scaler, float initial_value) : - _ardupin(ardupin), _adc_channel(adc_channel), + _ardupin(ardupin), _scaler(scaler), _value(initial_value), _latest_value(initial_value), diff --git a/libraries/AP_HAL_Linux/Flow_PX4.cpp b/libraries/AP_HAL_Linux/Flow_PX4.cpp index e285763c4fa57..08939bccf9193 100644 --- a/libraries/AP_HAL_Linux/Flow_PX4.cpp +++ b/libraries/AP_HAL_Linux/Flow_PX4.cpp @@ -53,8 +53,8 @@ Flow_PX4::Flow_PX4(uint32_t width, uint32_t bytesperline, float bottom_flow_feature_threshold, float bottom_flow_value_threshold) : _width(width), - _bytesperline(bytesperline), _search_size(max_flow_pixel), + _bytesperline(bytesperline), _bottom_flow_feature_threshold(bottom_flow_feature_threshold), _bottom_flow_value_threshold(bottom_flow_value_threshold) { diff --git a/libraries/AP_HAL_QURT/SPIDevice.cpp b/libraries/AP_HAL_QURT/SPIDevice.cpp index 616cf6a76f0aa..a4fcdc6660202 100644 --- a/libraries/AP_HAL_QURT/SPIDevice.cpp +++ b/libraries/AP_HAL_QURT/SPIDevice.cpp @@ -67,6 +67,12 @@ bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len, return transfer_fullduplex(send, (uint8_t*) send, send_len); } + // Special case handling. This can happen when a send buffer is specified + // even though we are doing only a read. + if (send == recv && send_len == recv_len) { + return transfer_fullduplex(send, recv, send_len); + } + // This is a read transaction uint8_t buf[send_len+recv_len]; if (send_len > 0) { diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index af55acaa1f844..4850e40b027a4 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -185,12 +185,14 @@ void SITL_State::wait_clock(uint64_t wait_time_usec) // conditions. if (speedup > 1 && hal.scheduler->in_main_thread()) { while (true) { - const int queue_length = ((HALSITL::UARTDriver*)hal.serial(0))->get_system_outqueue_length(); + HALSITL::UARTDriver *uart = (HALSITL::UARTDriver*)hal.serial(0); + const int queue_length = uart->get_system_outqueue_length(); // ::fprintf(stderr, "queue_length=%d\n", (signed)queue_length); if (queue_length < 1024) { break; } _serial_0_outqueue_full_count++; + uart->handle_reading_from_device_to_readbuffer(); usleep(1000); } } diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 4ff1c0c046b22..b5452aceda2df 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -110,9 +110,6 @@ class HALSITL::SITL_State : public SITL_State_Common { uint32_t wind_start_delay_micros; uint32_t last_wind_update_us; - // simulated GPS devices - SITL::GPS *gps[2]; // constrained by # of parameter sets - // returns a voltage between 0V to 5V which should appear as the // voltage from the sensor float _sonar_pin_voltage() const; diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index 77617cdb02ea4..e1c6f5d863444 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -1208,7 +1208,7 @@ void AP_IOMCU::bind_dsm(uint8_t mode) setup for mixing. This allows fixed wing aircraft to fly in manual mode if the FMU dies */ -bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan, +bool AP_IOMCU::setup_mixing(int8_t override_chan, float mixing_gain, uint16_t manual_rc_mask) { if (!is_chibios_backend) { @@ -1229,16 +1229,19 @@ bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan, MIX_UPDATE(mixing.servo_function[i], c->get_function()); MIX_UPDATE(mixing.servo_reversed[i], c->get_reversed()); } - // update RCMap - MIX_UPDATE(mixing.rc_channel[0], rcmap->roll()); - MIX_UPDATE(mixing.rc_channel[1], rcmap->pitch()); - MIX_UPDATE(mixing.rc_channel[2], rcmap->throttle()); - MIX_UPDATE(mixing.rc_channel[3], rcmap->yaw()); + auto &xrc = rc(); + // note that if not all of these channels are specified correctly + // in parameters then these may be a "dummy" RC channel pointer. + // In that case c->ch() will be zero. + const RC_Channel *channels[] { + &xrc.get_roll_channel(), + &xrc.get_pitch_channel(), + &xrc.get_throttle_channel(), + &xrc.get_yaw_channel() + }; for (uint8_t i=0; i<4; i++) { - const RC_Channel *c = RC_Channels::rc_channel(mixing.rc_channel[i]-1); - if (!c) { - continue; - } + const auto *c = channels[i]; + MIX_UPDATE(mixing.rc_channel[i], c->ch()); MIX_UPDATE(mixing.rc_min[i], c->get_radio_min()); MIX_UPDATE(mixing.rc_max[i], c->get_radio_max()); MIX_UPDATE(mixing.rc_trim[i], c->get_radio_trim()); diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h index 1d03debb9a86b..da6d55a03dce7 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.h +++ b/libraries/AP_IOMCU/AP_IOMCU.h @@ -11,7 +11,6 @@ #if HAL_WITH_IO_MCU #include "iofirmware/ioprotocol.h" -#include #include #include @@ -151,7 +150,7 @@ class AP_IOMCU void soft_reboot(); // setup for FMU failsafe mixing - bool setup_mixing(RCMapper *rcmap, int8_t override_chan, + bool setup_mixing(int8_t override_chan, float mixing_gain, uint16_t manual_rc_mask); // Check if pin number is valid and configured for GPIO diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 00d149e149223..817cb2e947c85 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -136,6 +136,7 @@ class AP_InertialSensor_Backend DEVTYPE_INS_ICM42670 = 0x3A, DEVTYPE_INS_ICM45686 = 0x3B, DEVTYPE_INS_SCHA63T = 0x3C, + DEVTYPE_INS_IIM42653 = 0x3D, }; protected: diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index 024d1f56a23b7..d22a7a224f30b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -136,6 +136,7 @@ extern const AP_HAL::HAL& hal; #define INV3_ID_ICM42605 0x42 #define INV3_ID_ICM42688 0x47 #define INV3_ID_IIM42652 0x6f +#define INV3_ID_IIM42653 0x56 #define INV3_ID_ICM42670 0x67 #define INV3_ID_ICM45686 0xE9 @@ -185,12 +186,12 @@ AP_InertialSensor_Invensensev3::~AP_InertialSensor_Invensensev3() #if HAL_INS_HIGHRES_SAMPLE if (highres_sampling) { if (fifo_buffer != nullptr) { - hal.util->free_type(fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_HIGHRES_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); + hal.util->free_type(fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_HIGHRES_SAMPLE_SIZE + 1, AP_HAL::Util::MEM_DMA_SAFE); } } else #endif if (fifo_buffer != nullptr) { - hal.util->free_type(fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); + hal.util->free_type(fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE + 1, AP_HAL::Util::MEM_DMA_SAFE); } } @@ -258,6 +259,12 @@ void AP_InertialSensor_Invensensev3::start() devtype = DEVTYPE_INS_IIM42652; temp_sensitivity = 1.0 / 2.07; break; + case Invensensev3_Type::IIM42653: + devtype = DEVTYPE_INS_IIM42653; + temp_sensitivity = 1.0 / 2.07; + gyro_scale = GYRO_SCALE_4000DPS; + accel_scale = ACCEL_SCALE_32G; + break; case Invensensev3_Type::ICM42688: devtype = DEVTYPE_INS_ICM42688; temp_sensitivity = 1.0 / 2.07; @@ -294,6 +301,7 @@ void AP_InertialSensor_Invensensev3::start() switch (inv3_type) { case Invensensev3_Type::ICM42688: // HiRes 19bit case Invensensev3_Type::IIM42652: // HiRes 19bit + case Invensensev3_Type::IIM42653: // HiRes 19bit case Invensensev3_Type::ICM45686: // HiRes 20bit highres_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI; break; @@ -358,10 +366,10 @@ void AP_InertialSensor_Invensensev3::start() // allocate fifo buffer #if HAL_INS_HIGHRES_SAMPLE if (highres_sampling) { - fifo_buffer = hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_HIGHRES_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); + fifo_buffer = hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_HIGHRES_SAMPLE_SIZE + 1, AP_HAL::Util::MEM_DMA_SAFE); } else #endif - fifo_buffer = hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); + fifo_buffer = hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE + 1, AP_HAL::Util::MEM_DMA_SAFE); if (fifo_buffer == nullptr) { AP_HAL::panic("Invensensev3: Unable to allocate FIFO buffer"); @@ -519,6 +527,8 @@ void AP_InertialSensor_Invensensev3::read_fifo() uint8_t reg_counth; uint8_t reg_data; + uint8_t* samples = nullptr; + uint8_t* tfr_buffer = (uint8_t*)fifo_buffer; switch (inv3_type) { case Invensensev3_Type::ICM45686: @@ -555,18 +565,28 @@ void AP_InertialSensor_Invensensev3::read_fifo() while (n_samples > 0) { uint8_t n = MIN(n_samples, INV3_FIFO_BUFFER_LEN); - if (!block_read(reg_data, (uint8_t*)fifo_buffer, n * fifo_sample_size)) { + + // we don't use read_registers() here to ensure that the fifo buffer that we have allocated + // gets passed all the way down to the SPI DMA handling. This involves one transfer to send + // the register read and then another using the same buffer and length which is handled specially + // for the read + tfr_buffer[0] = reg_data | BIT_READ_FLAG; + // transfer will also be sending data, make sure that data is zero + memset(tfr_buffer + 1, 0, n * fifo_sample_size); + if (!dev->transfer(tfr_buffer, n * fifo_sample_size + 1, tfr_buffer, n * fifo_sample_size + 1)) { goto check_registers; } + samples = tfr_buffer + 1; + #if HAL_INS_HIGHRES_SAMPLE if (highres_sampling) { - if (!accumulate_highres_samples((FIFODataHighRes*)fifo_buffer, n)) { + if (!accumulate_highres_samples((FIFODataHighRes*)samples, n)) { need_reset = true; break; } } else #endif - if (!accumulate_samples((FIFOData*)fifo_buffer, n)) { + if (!accumulate_samples((FIFOData*)samples, n)) { need_reset = true; break; } @@ -812,6 +832,7 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void) case Invensensev3_Type::ICM42688: case Invensensev3_Type::ICM42605: case Invensensev3_Type::IIM42652: + case Invensensev3_Type::IIM42653: case Invensensev3_Type::ICM42670: { /* fix for the "stuck gyro" issue, which affects all IxM42xxx @@ -955,6 +976,9 @@ bool AP_InertialSensor_Invensensev3::check_whoami(void) case INV3_ID_IIM42652: inv3_type = Invensensev3_Type::IIM42652; return true; + case INV3_ID_IIM42653: + inv3_type = Invensensev3_Type::IIM42653; + return true; case INV3_ID_ICM42670: inv3_type = Invensensev3_Type::ICM42670; return true; @@ -1030,6 +1054,7 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void) switch (inv3_type) { case Invensensev3_Type::ICM45686: case Invensensev3_Type::ICM40609: + case Invensensev3_Type::IIM42653: _clip_limit = 29.5f * GRAVITY_MSS; break; case Invensensev3_Type::ICM42688: diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h index b3946aa81dd47..6b4fc49a8c5c4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h @@ -37,6 +37,7 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend ICM42605, // No HiRes ICM40605, // No HiRes IIM42652, // HiRes 19bit + IIM42653, // HiRes 19bit ICM42670, // HiRes 19bit ICM45686 // HiRes 20bit }; diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index ae5197297c373..432ce26376bb5 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -1134,7 +1134,9 @@ struct PACKED log_VER { // @Description: Ardupilot version // @Field: TimeUS: Time since system startup // @Field: BT: Board type +// @FieldValueEnum: BT: HAL_BOARD // @Field: BST: Board subtype +// @FieldValueEnum: BST: HAL_BOARD_SUBTYPE // @Field: Maj: Major version number // @Field: Min: Minor version number // @Field: Pat: Patch number @@ -1143,6 +1145,7 @@ struct PACKED log_VER { // @Field: FWS: Firmware version string // @Field: APJ: Board ID // @Field: BU: Build vehicle type +// @FieldValueEnum: BU: APM_BUILD // @Field: FV: Filter version // @LoggerMessage: MOTB diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index bc0374232050d..ab69c468245a6 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -330,8 +330,8 @@ bool AP_Mission::start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Miss } // handle angle target - const bool pitch_angle_valid = !isnan(cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg) && (fabsF(cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg) <= 90); - const bool yaw_angle_valid = !isnan(cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg) && (fabsF(cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg) <= 360); + const bool pitch_angle_valid = abs(cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg) <= 90; + const bool yaw_angle_valid = abs(cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg) <= 360; if (pitch_angle_valid && yaw_angle_valid) { mount->set_angle_target(gimbal_instance, 0, cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg, cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg, cmd.content.gimbal_manager_pitchyaw.flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); return true; diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp index f28db5c9243fb..336eea61e3d80 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp +++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp @@ -518,7 +518,7 @@ void stability_test() // arm motors motors->armed(true); motors->set_interlock(true); - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); hal.console->printf("Roll,Pitch,Yaw,Thr,"); for (uint8_t i=0; i #include #include "AP_Mount.h" -#if HAL_MOUNT_ENABLED - #include "AP_Mount_Backend.h" #include "AP_Mount_Servo.h" #include "AP_Mount_SoloGimbal.h" diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index 693d197860897..79a8a5eaf0586 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_Alexmos.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_ALEXMOS_ENABLED + +#include "AP_Mount_Alexmos.h" + #include //definition of the commands id for the Alexmos Serial Protocol @@ -100,20 +103,9 @@ void AP_Mount_Alexmos::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - 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; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.h b/libraries/AP_Mount/AP_Mount_Alexmos.h index a8765e17436f2..10225afa7255d 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.h +++ b/libraries/AP_Mount/AP_Mount_Alexmos.h @@ -3,9 +3,12 @@ */ #pragma once -#include "AP_Mount_Backend.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_ALEXMOS_ENABLED + +#include "AP_Mount_Backend.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 9d064633b1916..531d01c30a6e3 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -1,5 +1,9 @@ -#include "AP_Mount_Backend.h" +#include "AP_Mount_config.h" + #if HAL_MOUNT_ENABLED + +#include "AP_Mount_Backend.h" + #include #include #include @@ -66,6 +70,29 @@ bool AP_Mount_Backend::set_mode(MAV_MOUNT_MODE mode) return true; } +// called when mount mode is RC-targetting, updates the mnt_target object from RC inputs: +void AP_Mount_Backend::update_mnt_target_from_rc_target() +{ + if (rc().in_rc_failsafe()) { + if (option_set(Options::NEUTRAL_ON_RC_FS)) { + mnt_target.angle_rad.set(_params.neutral_angles.get() * DEG_TO_RAD, false); + mnt_target.target_type = MountTargetType::ANGLE; + return; + } + } + + 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; + } +} + // set angle target in degrees // roll and pitch are in earth-frame // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 16082d58f450b..67b673d656ebe 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -249,9 +249,13 @@ class AP_Mount_Backend // options parameter bitmask handling enum class Options : uint8_t { RCTARGETING_LOCK_FROM_PREVMODE = (1U << 0), // RC_TARGETING mode's lock/follow state maintained from previous mode + NEUTRAL_ON_RC_FS = (1U << 1), // move mount to netral position on RC failsafe }; bool option_set(Options opt) const { return (_params.options.get() & (uint8_t)opt) != 0; } + // called when mount mode is RC-targetting, updates the mnt_target object from RC inputs: + void update_mnt_target_from_rc_target(); + // returns true if user has configured a valid roll angle range // allows user to disable roll even on 3-axis gimbal bool roll_range_valid() const { return (_params.roll_angle_min < _params.roll_angle_max); } diff --git a/libraries/AP_Mount/AP_Mount_Backend_Serial.cpp b/libraries/AP_Mount/AP_Mount_Backend_Serial.cpp index ec27051057565..35fe60265b6a7 100644 --- a/libraries/AP_Mount/AP_Mount_Backend_Serial.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend_Serial.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_Backend_Serial.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_ENABLED + +#include "AP_Mount_Backend_Serial.h" + #include // Default init function for every mount diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.cpp b/libraries/AP_Mount/AP_Mount_Gremsy.cpp index 259de8e90cc4d..9e88a32417a7e 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.cpp +++ b/libraries/AP_Mount/AP_Mount_Gremsy.cpp @@ -1,7 +1,9 @@ -#include "AP_Mount_Gremsy.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_GREMSY_ENABLED +#include "AP_Mount_Gremsy.h" + #include #include @@ -48,20 +50,9 @@ void AP_Mount_Gremsy::update() } // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - 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; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.h b/libraries/AP_Mount/AP_Mount_Gremsy.h index 79bd42af77670..581eda36908ab 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.h +++ b/libraries/AP_Mount/AP_Mount_Gremsy.h @@ -3,10 +3,12 @@ */ #pragma once -#include "AP_Mount_Backend.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_GREMSY_ENABLED +#include "AP_Mount_Backend.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_Params.cpp b/libraries/AP_Mount/AP_Mount_Params.cpp index e2c727f4b41c4..d077c603ec976 100644 --- a/libraries/AP_Mount/AP_Mount_Params.cpp +++ b/libraries/AP_Mount/AP_Mount_Params.cpp @@ -168,7 +168,7 @@ const AP_Param::GroupInfo AP_Mount_Params::var_info[] = { // @Param: _OPTIONS // @DisplayName: Mount options // @Description: Mount options bitmask - // @Bitmask: 0:RC lock state from previous mode + // @Bitmask: 0:RC lock state from previous mode, 1:Return to neutral angles on RC failsafe // @User: Standard AP_GROUPINFO("_OPTIONS", 16, AP_Mount_Params, options, 0), diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.cpp b/libraries/AP_Mount/AP_Mount_SToRM32.cpp index 59e4da66bcbeb..71875ad1592dc 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_SToRM32.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_STORM32MAVLINK_ENABLED + +#include "AP_Mount_SToRM32.h" + #include #include @@ -52,21 +55,10 @@ void AP_Mount_SToRM32::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - 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; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); resend_now = true; break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.h b/libraries/AP_Mount/AP_Mount_SToRM32.h index 33aef375c410f..6ff1a1ad1abaf 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.h +++ b/libraries/AP_Mount/AP_Mount_SToRM32.h @@ -3,10 +3,12 @@ */ #pragma once -#include "AP_Mount_Backend.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_STORM32MAVLINK_ENABLED +#include "AP_Mount_Backend.h" + #include #include diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index 82387a0e93ec1..ace890366352d 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_SToRM32_serial.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_STORM32SERIAL_ENABLED + +#include "AP_Mount_SToRM32_serial.h" + #include #include #include @@ -49,21 +52,10 @@ void AP_Mount_SToRM32_serial::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - 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; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); resend_now = true; break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.h b/libraries/AP_Mount/AP_Mount_SToRM32_serial.h index 895143e80bb72..07b3d3d165de8 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.h +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.h @@ -3,10 +3,12 @@ */ #pragma once -#include "AP_Mount_Backend_Serial.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_STORM32SERIAL_ENABLED +#include "AP_Mount_Backend_Serial.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_Scripting.cpp b/libraries/AP_Mount/AP_Mount_Scripting.cpp index c2b93304e67ee..165d0d1a58a07 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.cpp +++ b/libraries/AP_Mount/AP_Mount_Scripting.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_Scripting.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_SCRIPTING_ENABLED + +#include "AP_Mount_Scripting.h" + #include #include #include @@ -45,21 +48,10 @@ void AP_Mount_Scripting::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - 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; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); target_loc_valid = false; break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Scripting.h b/libraries/AP_Mount/AP_Mount_Scripting.h index ecf1f7cbd89c7..52537f171dd1f 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.h +++ b/libraries/AP_Mount/AP_Mount_Scripting.h @@ -4,10 +4,12 @@ #pragma once -#include "AP_Mount_Backend.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_SCRIPTING_ENABLED +#include "AP_Mount_Backend.h" + #include #include #include @@ -50,4 +52,4 @@ class AP_Mount_Scripting : public AP_Mount_Backend bool target_loc_valid; // true if target_loc holds a valid target location }; -#endif // HAL_MOUNT_SIYISERIAL_ENABLED +#endif // HAL_MOUNT_SCRIPTING_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 1fba00e92a1be..f2446c2af7a5f 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_Servo.h" +#include "AP_Mount_config.h" + #if HAL_MOUNT_SERVO_ENABLED +#include "AP_Mount_Servo.h" + #include #include @@ -55,20 +58,9 @@ void AP_Mount_Servo::update() } // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - 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; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Servo.h b/libraries/AP_Mount/AP_Mount_Servo.h index 8ead11561fd08..a1da63210bc5c 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.h +++ b/libraries/AP_Mount/AP_Mount_Servo.h @@ -3,10 +3,12 @@ */ #pragma once -#include "AP_Mount_Backend.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_SERVO_ENABLED +#include "AP_Mount_Backend.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index d0e82cd503ee1..e16a29586de5f 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_Siyi.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_SIYI_ENABLED + +#include "AP_Mount_Siyi.h" + #include #include #include @@ -121,20 +124,9 @@ void AP_Mount_Siyi::update() break; } - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - 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; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index 20285b0b5bd6c..7f7f280b224aa 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -19,10 +19,12 @@ #pragma once -#include "AP_Mount_Backend_Serial.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_SIYI_ENABLED +#include "AP_Mount_Backend_Serial.h" + #include #include #include @@ -381,4 +383,4 @@ class AP_Mount_Siyi : public AP_Mount_Backend_Serial uint8_t sent_time_count; }; -#endif // HAL_MOUNT_SIYISERIAL_ENABLED +#endif // HAL_MOUNT_SIYI_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index 02c8c3f2475c1..251c859693b2d 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -65,20 +65,10 @@ void AP_Mount_SoloGimbal::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { + case MAV_MOUNT_MODE_RC_TARGETING: _gimbal.set_lockedToBody(false); - 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; - } + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp index 09682a8429b64..4e468a7ae5173 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.cpp +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -1,7 +1,9 @@ -#include "AP_Mount_Topotek.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_TOPOTEK_ENABLED +#include "AP_Mount_Topotek.h" + #include #include #include @@ -31,7 +33,7 @@ extern const AP_HAL::HAL& hal; # define AP_MOUNT_TOPOTEK_ID3CHAR_START_TRACKING "LOC" // start image tracking # define AP_MOUNT_TOPOTEK_ID3CHAR_LRF "LRF" // laser rangefinder control, data bytes: 00:ranging stop, 01:ranging start, 02:single measurement, 03:continuous measurement # define AP_MOUNT_TOPOTEK_ID3CHAR_PIP "PIP" // set picture-in-picture setting, data bytes: // 00:main only, 01:main+sub, 02:sub+main, 03:sub only, 0A:next -# define AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_ATT "GAA" // get gimbal attitude, data bytes: 00:stop, 01:start +# define AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_ATT "GIA" // get gimbal attitude, data bytes: 00:stop, 01:start # define AP_MOUNT_TOPOTEK_ID3CHAR_SD_CARD "SDC" // get SD card state, data bytes: 00:get remaining capacity, 01:get total capacity # define AP_MOUNT_TOPOTEK_ID3CHAR_TIME "UTC" // set time and date, data bytes: HHMMSSDDMMYY # define AP_MOUNT_TOPOTEK_ID3CHAR_GET_VERSION "VSN" // get firmware version, data bytes always 00 @@ -156,20 +158,9 @@ void AP_Mount_Topotek::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - 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; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: @@ -765,7 +756,7 @@ void AP_Mount_Topotek::read_incoming_packets() // request gimbal attitude void AP_Mount_Topotek::request_gimbal_attitude() { - // sample command: #TPUG2wGAA01 + // sample command: #TPUG2wGIA01 send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_ATT, true, 1); } @@ -811,7 +802,7 @@ void AP_Mount_Topotek::send_angle_target(const MountTarget& angle_rad) // calculate and send yaw target // sample command #tpUG6wGIY - const char* format_str = "%04x%02x"; + const char* format_str = "%04X%02X"; const uint8_t speed = 99; const uint16_t yaw_angle_cd = (uint16_t)constrain_int16(degrees(angle_rad.get_bf_yaw()) * 100, MAX(-18000, _params.yaw_angle_min * 100), MIN(18000, _params.yaw_angle_max * 100)); @@ -872,7 +863,7 @@ void AP_Mount_Topotek::send_rate_target(const MountTarget& rate_rads) // prepare and send command // sample command: #tpUG6wYPR uint8_t databuff[7]; - hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02x%02x%02x", yaw_angle_speed, pitch_angle_speed, roll_angle_speed); + hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02X%02X%02X", yaw_angle_speed, pitch_angle_speed, roll_angle_speed); send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_YPR_RATE, true, databuff, ARRAY_SIZE(databuff)-1); } @@ -1150,7 +1141,7 @@ int16_t AP_Mount_Topotek::hexchar4_to_int16(char high, char mid_high, char mid_l bool AP_Mount_Topotek::send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value) { uint8_t databuff[3]; - hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02x", value); + hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02X", value); return send_variablelen_packet(HeaderType::FIXED_LEN, address, id, write, databuff, ARRAY_SIZE(databuff)-1); } diff --git a/libraries/AP_Mount/AP_Mount_Topotek.h b/libraries/AP_Mount/AP_Mount_Topotek.h index 3d52f0ff62827..9c8882936c758 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.h +++ b/libraries/AP_Mount/AP_Mount_Topotek.h @@ -272,7 +272,7 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial // stores command ID and corresponding member functions that are compared with the command received by the gimbal UartCmdFunctionHandler uart_recv_cmd_compare_list[AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM] = { - {{"GAC"}, &AP_Mount_Topotek::gimbal_angle_analyse}, + {{"GIA"}, &AP_Mount_Topotek::gimbal_angle_analyse}, {{"REC"}, &AP_Mount_Topotek::gimbal_record_analyse}, {{"SDC"}, &AP_Mount_Topotek::gimbal_sdcard_analyse}, {{"LRF"}, &AP_Mount_Topotek::gimbal_dist_info_analyse}, diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index f9b06f3b80ad3..aa443483e82b2 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_Viewpro.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_VIEWPRO_ENABLED + +#include "AP_Mount_Viewpro.h" + #include #include #include @@ -94,20 +97,9 @@ void AP_Mount_Viewpro::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - 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; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.h b/libraries/AP_Mount/AP_Mount_Viewpro.h index 1e045c8fd1ae2..1100586cd1ce1 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.h +++ b/libraries/AP_Mount/AP_Mount_Viewpro.h @@ -16,10 +16,12 @@ #pragma once -#include "AP_Mount_Backend_Serial.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_VIEWPRO_ENABLED +#include "AP_Mount_Backend_Serial.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp index 6353ce41d00d3..86296d814be01 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.cpp +++ b/libraries/AP_Mount/AP_Mount_Xacti.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_Xacti.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_XACTI_ENABLED + +#include "AP_Mount_Xacti.h" + #include #include #include @@ -135,20 +138,9 @@ void AP_Mount_Xacti::update() break; } - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - 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; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Xacti.h b/libraries/AP_Mount/AP_Mount_Xacti.h index 31a6684023561..0057f4ae371ad 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.h +++ b/libraries/AP_Mount/AP_Mount_Xacti.h @@ -8,10 +8,12 @@ #pragma once -#include "AP_Mount_Backend.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_XACTI_ENABLED +#include "AP_Mount_Backend.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index c7b063b20e2ec..c6e0791c2a6b0 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/libraries/AP_Mount/AP_Mount_config.h @@ -2,6 +2,8 @@ #include #include +#include +#include #ifndef HAL_MOUNT_ENABLED #define HAL_MOUNT_ENABLED 1 diff --git a/libraries/AP_Mount/SoloGimbal.cpp b/libraries/AP_Mount/SoloGimbal.cpp index eba243eabac4a..6e270de10251a 100644 --- a/libraries/AP_Mount/SoloGimbal.cpp +++ b/libraries/AP_Mount/SoloGimbal.cpp @@ -1,9 +1,11 @@ +#include "AP_Mount_config.h" + +#if HAL_SOLO_GIMBAL_ENABLED + #include #include #include "SoloGimbal.h" -#if HAL_SOLO_GIMBAL_ENABLED - #include #include #include diff --git a/libraries/AP_Mount/SoloGimbalEKF.cpp b/libraries/AP_Mount/SoloGimbalEKF.cpp index b599b4c2516b8..b514706f159a9 100644 --- a/libraries/AP_Mount/SoloGimbalEKF.cpp +++ b/libraries/AP_Mount/SoloGimbalEKF.cpp @@ -1,3 +1,7 @@ +#include "AP_Mount_config.h" + +#if HAL_SOLO_GIMBAL_ENABLED + #include // uncomment this to force the optimisation of this code, note that @@ -9,7 +13,6 @@ #endif #include "SoloGimbalEKF.h" -#if HAL_SOLO_GIMBAL_ENABLED #include #include #include diff --git a/libraries/AP_Mount/SoloGimbal_Parameters.cpp b/libraries/AP_Mount/SoloGimbal_Parameters.cpp index 60bf156acb3ba..ff76f0583dfed 100644 --- a/libraries/AP_Mount/SoloGimbal_Parameters.cpp +++ b/libraries/AP_Mount/SoloGimbal_Parameters.cpp @@ -1,5 +1,8 @@ -#include "SoloGimbal_Parameters.h" +#include "AP_Mount_config.h" + #if HAL_SOLO_GIMBAL_ENABLED + +#include "SoloGimbal_Parameters.h" #include #include #include diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 415d7424a4777..6bfba2ee17eb8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -453,9 +453,12 @@ class NavEKF3 { AP_Int32 _options; // bit mask of processing options // enum for processing options - enum class Options { + enum class Option { JammingExpected = (1<<0), }; + bool option_is_enabled(Option option) const { + return (_options & (uint32_t)option) != 0; + } // Possible values for _flowUse #define FLOW_USE_NONE 0 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 9e1eab32eb29d..a7146dd06e71b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -638,7 +638,7 @@ void NavEKF3_core::readGpsData() useGpsVertVel = false; } - if ((frontend->_options & (int32_t)NavEKF3::Options::JammingExpected) && + if (frontend->option_is_enabled(NavEKF3::Option::JammingExpected) && (lastTimeGpsReceived_ms - secondLastGpsTime_ms) > frontend->gpsNoFixTimeout_ms) { const bool doingBodyVelNav = (imuSampleTime_ms - prevBodyVelFuseTime_ms < 1000); const bool doingFlowNav = (imuSampleTime_ms - prevFlowFuseTime_ms < 1000);; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index 3492718ea4c40..2b86e63f73d4f 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -19,13 +19,7 @@ extern const AP_HAL::HAL& hal; #ifndef OPTICAL_FLOW_TYPE_DEFAULT - #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 || defined(HAL_HAVE_PIXARTFLOW_SPI) - #define OPTICAL_FLOW_TYPE_DEFAULT Type::PIXART - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP - #define OPTICAL_FLOW_TYPE_DEFAULT Type::BEBOP - #else #define OPTICAL_FLOW_TYPE_DEFAULT Type::NONE - #endif #endif const AP_Param::GroupInfo AP_OpticalFlow::var_info[] = { diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index fe3a73ab2d984..2a64e19f59b75 100644 --- a/libraries/AP_Relay/AP_Relay.cpp +++ b/libraries/AP_Relay/AP_Relay.cpp @@ -353,7 +353,11 @@ void AP_Relay::init() continue; } - if (function == AP_Relay_Params::FUNCTION::RELAY) { + bool use_default_param = (function == AP_Relay_Params::FUNCTION::RELAY); +#ifdef HAL_BUILD_AP_PERIPH + use_default_param |= (function >= AP_Relay_Params::FUNCTION::DroneCAN_HARDPOINT_0 && function <= AP_Relay_Params::FUNCTION::DroneCAN_HARDPOINT_15); +#endif + if (use_default_param) { // relay by instance number, set the state to match our output const AP_Relay_Params::DefaultState default_state = _params[instance].default_state; if ((default_state == AP_Relay_Params::DefaultState::OFF) || diff --git a/libraries/AP_Scripting/applets/copter-slung-payload.md b/libraries/AP_Scripting/applets/copter-slung-payload.md index 32d76256c43a7..b4953565ccf22 100644 --- a/libraries/AP_Scripting/applets/copter-slung-payload.md +++ b/libraries/AP_Scripting/applets/copter-slung-payload.md @@ -4,13 +4,13 @@ This script moves a Copter so as to reduce a slung payload's oscillation. Requi # Parameters -SLUP_ENABLE : Set to 1 to enable this script -SLUP_VEL_P : Oscillation controller velocity P gain. Higher values will result in the vehicle moving more quickly in sync with the payload -SLUP_DIST_MAX : maximum acceptable distance between vehicle and payload. Within this distance oscillation suppression will operate -SLUP_SYSID : System id of payload's autopilot. If zero any system id is accepted -SLUP_WP_POS_P : Return to waypoint position P gain. Higher values result in the vehicle returning more quickly to the latest waypoint -SLUP_RESTOFS_TC : Slung Payload resting offset estimate filter time constant. Higher values result in smoother estimate but slower response -SLUP_DEBUG : Slung payload debug output, set to 1 to enable debug +- SLUP_ENABLE : Set to 1 to enable this script +- SLUP_VEL_P : Oscillation controller velocity P gain. Higher values will result in the vehicle moving more quickly in sync with the payload +- SLUP_DIST_MAX : maximum acceptable distance between vehicle and payload. Within this distance oscillation suppression will operate +- SLUP_SYSID : System id of payload's autopilot. If zero any system id is accepted +- SLUP_WP_POS_P : Return to waypoint position P gain. Higher values result in the vehicle returning more quickly to the latest waypoint +- SLUP_RESTOFS_TC : Slung Payload resting offset estimate filter time constant. Higher values result in smoother estimate but slower response +- SLUP_DEBUG : Slung payload debug output, set to 1 to enable debug # How To Use diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index f25cf5294e6f9..49277cfce07ba 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -427,6 +427,9 @@ void AP_TECS::_update_speed(float DT) // Get measured airspeed or default to trim speed and constrain to range between min and max if // airspeed sensor data cannot be used + + // Equivalent airspeed + float _EAS; if (!use_airspeed || !_ahrs.airspeed_estimate(_EAS)) { // If no airspeed available use average of min and max _EAS = constrain_float(aparm.airspeed_cruise.get(), (float)aparm.airspeed_min.get(), (float)aparm.airspeed_max.get()); diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 5bc757c0b79a7..57748edba1a07 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -271,9 +271,6 @@ class AP_TECS { float _vel_dot; float _vel_dot_lpf; - // Equivalent airspeed - float _EAS; - // True airspeed limits float _TASmax; float _TASmin; diff --git a/libraries/AP_Vehicle/AP_FixedWing.h b/libraries/AP_Vehicle/AP_FixedWing.h index 722c53dcaf9af..8135a7d160352 100644 --- a/libraries/AP_Vehicle/AP_FixedWing.h +++ b/libraries/AP_Vehicle/AP_FixedWing.h @@ -12,6 +12,7 @@ struct AP_FixedWing { AP_Int8 throttle_cruise; AP_Int8 takeoff_throttle_max; AP_Int8 takeoff_throttle_min; + AP_Int8 takeoff_throttle_idle; AP_Int32 takeoff_options; AP_Int16 airspeed_min; AP_Int16 airspeed_max; diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 562ac606075a4..3005e40d3adcb 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -438,7 +438,7 @@ void AP_Vehicle::setup() #if AP_SRV_CHANNELS_ENABLED - SRV_Channels::init(); + AP::srv().init(); #endif // gyro FFT needs to be initialized really late diff --git a/libraries/AP_Vehicle/AP_Vehicle_Type.h b/libraries/AP_Vehicle/AP_Vehicle_Type.h index 0bc298658f8c8..3636f2364983c 100644 --- a/libraries/AP_Vehicle/AP_Vehicle_Type.h +++ b/libraries/AP_Vehicle/AP_Vehicle_Type.h @@ -19,6 +19,7 @@ Also note that code needs to support other APM_BUILD_DIRECTORY values for example sketches */ +// @LoggerEnum: APM_BUILD #define APM_BUILD_Rover 1 #define APM_BUILD_ArduCopter 2 #define APM_BUILD_ArduPlane 3 @@ -32,6 +33,7 @@ #define APM_BUILD_AP_Bootloader 11 #define APM_BUILD_Blimp 12 #define APM_BUILD_Heli 13 +// @LoggerEnumEnd #ifdef APM_BUILD_DIRECTORY /* diff --git a/libraries/AP_VideoTX/AP_Tramp.cpp b/libraries/AP_VideoTX/AP_Tramp.cpp index 54f62a382f051..afe6f07ac7bc2 100644 --- a/libraries/AP_VideoTX/AP_Tramp.cpp +++ b/libraries/AP_VideoTX/AP_Tramp.cpp @@ -143,6 +143,8 @@ char AP_Tramp::handle_response(void) debug("device config: freq: %u, cfg pwr: %umw, act pwr: %umw, pitmode: %u", unsigned(freq), unsigned(power), unsigned(cur_act_power), unsigned(pit_mode)); + // update the "_configuration_finished" flag, otherwise OSD item VTX_POWER blinks forever + vtx.set_configuration_finished(!update_pending); return 'v'; } diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp index 43f1c17feb705..4dac4792b3bc3 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp @@ -405,7 +405,7 @@ void AP_Volz_Protocol::update() "TimeUS,I,Dang,ang,pc,sc,pv,sv,mt,pt", "s#ddAAvvOO", "F000000000", - "QBffffffHH", + "QBffffffhh", AP_HAL::micros64(), i + 1, // convert to 1 indexed to match actuator IDs and SERVOx numbering telem.data[i].desired_angle, diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h index 02b18fc4ea421..f14df3023090d 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h @@ -140,8 +140,8 @@ class AP_Volz_Protocol { float secondary_current; float primary_voltage; float secondary_voltage; - uint16_t motor_temp_deg; - uint16_t pcb_temp_deg; + int16_t motor_temp_deg; + int16_t pcb_temp_deg; } data[NUM_SERVO_CHANNELS]; uint32_t last_log_ms; } telem; diff --git a/libraries/AR_Motors/AP_MotorsUGV.cpp b/libraries/AR_Motors/AP_MotorsUGV.cpp index e27ef29cdc11a..4b63101d5f5bb 100644 --- a/libraries/AR_Motors/AP_MotorsUGV.cpp +++ b/libraries/AR_Motors/AP_MotorsUGV.cpp @@ -340,10 +340,11 @@ void AP_MotorsUGV::output(bool armed, float ground_speed, float dt) output_sail(); // send values to the PWM timers for output + auto &srv = AP::srv(); SRV_Channels::calc_pwm(); - SRV_Channels::cork(); + srv.cork(); SRV_Channels::output_ch_all(); - SRV_Channels::push(); + srv.push(); } // test steering or throttle output as a percentage of the total (range -100 to +100) @@ -408,10 +409,11 @@ bool AP_MotorsUGV::output_test_pct(motor_test_order motor_seq, float pct) case MOTOR_TEST_LAST: return false; } + auto &srv = AP::srv(); SRV_Channels::calc_pwm(); - SRV_Channels::cork(); + srv.cork(); SRV_Channels::output_ch_all(); - SRV_Channels::push(); + srv.push(); return true; } @@ -474,10 +476,11 @@ bool AP_MotorsUGV::output_test_pwm(motor_test_order motor_seq, float pwm) default: return false; } + auto &srv = AP::srv(); SRV_Channels::calc_pwm(); - SRV_Channels::cork(); + srv.cork(); SRV_Channels::output_ch_all(); - SRV_Channels::push(); + srv.push(); return true; } diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 8edbc60474a7c..8bb1108de3d18 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -412,6 +412,10 @@ class GCS_MAVLINK void send_uavionix_adsb_out_status() const; void send_autopilot_state_for_gimbal_device() const; + // Send the mode with the given index (not mode number!) return the total number of modes + // Index starts at 1 + virtual uint8_t send_available_mode(uint8_t index) const = 0; + // lock a channel, preventing use by MAVLink void lock(bool _lock) { _locked = _lock; @@ -1126,6 +1130,16 @@ class GCS_MAVLINK // true if we should NOT do MAVLink on this port (usually because // someone's doing SERIAL_CONTROL over mavlink) bool _locked; + + // Handling of AVAILABLE_MODES + struct { + bool should_send; + // Note these start at 1 + uint8_t requested_index; + uint8_t next_index; + } available_modes; + bool send_available_modes(); + }; /// @class GCS diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 9a1528b9dca4e..9922a4f8377b0 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1030,13 +1030,17 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_SCALED_PRESSURE, MSG_SCALED_PRESSURE}, { MAVLINK_MSG_ID_SCALED_PRESSURE2, MSG_SCALED_PRESSURE2}, { MAVLINK_MSG_ID_SCALED_PRESSURE3, MSG_SCALED_PRESSURE3}, -#if AP_GPS_ENABLED +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED { MAVLINK_MSG_ID_GPS_RAW_INT, MSG_GPS_RAW}, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED { MAVLINK_MSG_ID_GPS_RTK, MSG_GPS_RTK}, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED { MAVLINK_MSG_ID_GPS2_RAW, MSG_GPS2_RAW}, - { MAVLINK_MSG_ID_GPS2_RTK, MSG_GPS2_RTK}, #endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED + { MAVLINK_MSG_ID_GPS2_RTK, MSG_GPS2_RTK}, #endif { MAVLINK_MSG_ID_SYSTEM_TIME, MSG_SYSTEM_TIME}, { MAVLINK_MSG_ID_RC_CHANNELS_SCALED, MSG_SERVO_OUT}, @@ -1148,6 +1152,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c #if AP_AIRSPEED_ENABLED { MAVLINK_MSG_ID_AIRSPEED, MSG_AIRSPEED}, #endif + { MAVLINK_MSG_ID_AVAILABLE_MODES, MSG_AVAILABLE_MODES}, }; for (uint8_t i=0; i mode_count)) { + // Sending all and just sent the last + available_modes.should_send = false; + } + + return true; +} + bool GCS_MAVLINK::try_send_message(const enum ap_message id) { bool ret = true; @@ -6237,28 +6282,32 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) CHECK_PAYLOAD_SIZE(SYSTEM_TIME); send_system_time(); break; -#if AP_GPS_ENABLED + +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED case MSG_GPS_RAW: CHECK_PAYLOAD_SIZE(GPS_RAW_INT); AP::gps().send_mavlink_gps_raw(chan); break; +#endif // AP_GPS_GPS_RAW_INT_SENDING_ENABLED +#if AP_GPS_GPS_RTK_SENDING_ENABLED case MSG_GPS_RTK: CHECK_PAYLOAD_SIZE(GPS_RTK); AP::gps().send_mavlink_gps_rtk(chan, 0); break; -#if GPS_MAX_RECEIVERS > 1 +#endif // AP_GPS_GPS_RTK_SENDING_ENABLED +#if AP_GPS_GPS2_RAW_SENDING_ENABLED case MSG_GPS2_RAW: CHECK_PAYLOAD_SIZE(GPS2_RAW); AP::gps().send_mavlink_gps2_raw(chan); break; -#endif -#if GPS_MAX_RECEIVERS > 1 +#endif // AP_GPS_GPS2_RAW_SENDING_ENABLED +#if AP_GPS_GPS2_RTK_SENDING_ENABLED case MSG_GPS2_RTK: CHECK_PAYLOAD_SIZE(GPS2_RTK); AP::gps().send_mavlink_gps_rtk(chan, 1); break; -#endif -#endif // AP_GPS_ENABLED +#endif // AP_GPS_GPS2_RTK_SENDING_ENABLED + #if AP_AHRS_ENABLED case MSG_LOCAL_POSITION: CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED); @@ -6518,6 +6567,10 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) break; #endif + case MSG_AVAILABLE_MODES: + ret = send_available_modes(); + break; + default: // try_send_message must always at some stage return true for // a message, or we will attempt to infinitely retry the diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h index f12155294df31..17e74ee0ba7ed 100644 --- a/libraries/GCS_MAVLink/GCS_Dummy.h +++ b/libraries/GCS_MAVLink/GCS_Dummy.h @@ -35,6 +35,7 @@ class GCS_MAVLINK_Dummy : public GCS_MAVLINK void send_nav_controller_output() const override {}; void send_pid_tuning() override {}; + uint8_t send_available_mode(uint8_t index) const override { return 0; } }; /* diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h index 5f059cb9a1042..2c8800a959c25 100644 --- a/libraries/GCS_MAVLink/GCS_config.h +++ b/libraries/GCS_MAVLink/GCS_config.h @@ -71,15 +71,6 @@ #define AP_MAVLINK_RALLY_POINT_PROTOCOL_ENABLED HAL_GCS_ENABLED && HAL_RALLY_ENABLED #endif -// CODE_REMOVAL -// handling of HIL_GPS is slated to be removed in 4.7; GPS_INPUT can be used -// in its place -// ArduPilot 4.6 stops compiling support in -// ArduPilot 4.7 removes the code entirely -#ifndef AP_MAVLINK_MSG_HIL_GPS_ENABLED -#define AP_MAVLINK_MSG_HIL_GPS_ENABLED 0 -#endif - // CODE_REMOVAL // ArduPilot 4.5 sends deprecation warnings for MOUNT_CONTROL/MOUNT_CONFIGURE // ArduPilot 4.6 stops compiling them in diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index a14be7cd53897..b82e64611f9dc 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -103,5 +103,6 @@ enum ap_message : uint8_t { MSG_HIGHRES_IMU, #endif MSG_AIRSPEED, + MSG_AVAILABLE_MODES, MSG_LAST // MSG_LAST must be the last entry in this enum }; diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 4b4f30d79e288..1d4c7cc77fd0c 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -1921,14 +1921,6 @@ RC_Channel::AuxSwitchPos RC_Channel::get_aux_switch_pos() const return position; } -// return switch position value as LOW, MIDDLE, HIGH -// if reading the switch fails then it returns LOW -RC_Channel::AuxSwitchPos RC_Channels::get_channel_pos(const uint8_t rcmapchan) const -{ - const RC_Channel* chan = rc().channel(rcmapchan-1); - return chan != nullptr ? chan->get_aux_switch_pos() : RC_Channel::AuxSwitchPos::LOW; -} - RC_Channel *RC_Channels::find_channel_for_option(const RC_Channel::AUX_FUNC option) { for (uint8_t i=0; iupdate_channel_masks(); #if HAL_SUPPORT_RCOUT_SERIAL - blheli_ptr->update(); + blheli.update(); #endif } diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index 67a9af5b00ad6..c1fda6226c37c 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/libraries/SRV_Channel/SRV_Channels.cpp @@ -41,28 +41,8 @@ extern const AP_HAL::HAL& hal; SRV_Channel *SRV_Channels::channels; SRV_Channels *SRV_Channels::_singleton; -#if AP_VOLZ_ENABLED -AP_Volz_Protocol *SRV_Channels::volz_ptr; -#endif - -#if AP_SBUSOUTPUT_ENABLED -AP_SBusOut *SRV_Channels::sbus_ptr; -#endif - -#if AP_ROBOTISSERVO_ENABLED -AP_RobotisServo *SRV_Channels::robotis_ptr; -#endif - -#if AP_FETTEC_ONEWIRE_ENABLED -AP_FETtecOneWire *SRV_Channels::fetteconwire_ptr; -#endif - uint16_t SRV_Channels::override_counter[NUM_SERVO_CHANNELS]; -#if HAL_SUPPORT_RCOUT_SERIAL -AP_BLHeli *SRV_Channels::blheli_ptr; -#endif - uint32_t SRV_Channels::disabled_mask; uint32_t SRV_Channels::digital_mask; uint32_t SRV_Channels::reversible_mask; @@ -379,26 +359,6 @@ SRV_Channels::SRV_Channels(void) } #endif } - -#if AP_FETTEC_ONEWIRE_ENABLED - fetteconwire_ptr = &fetteconwire; -#endif - -#if AP_VOLZ_ENABLED - volz_ptr = &volz; -#endif - -#if AP_SBUSOUTPUT_ENABLED - sbus_ptr = &sbus; -#endif - -#if AP_ROBOTISSERVO_ENABLED - robotis_ptr = &robotis; -#endif // AP_ROBOTISSERVO_ENABLED - -#if HAL_SUPPORT_RCOUT_SERIAL - blheli_ptr = &blheli; -#endif } // SRV_Channels initialization @@ -406,7 +366,7 @@ void SRV_Channels::init(uint32_t motor_mask, AP_HAL::RCOutput::output_mode mode) { // initialize BLHeli late so that all of the masks it might setup don't get trodden on by motor initialization #if HAL_SUPPORT_RCOUT_SERIAL - blheli_ptr->init(motor_mask, mode); + blheli.init(motor_mask, mode); #endif #ifndef HAL_BUILD_AP_PERIPH hal.rcout->set_dshot_rate(_singleton->dshot_rate, AP::scheduler().get_loop_rate_hz()); @@ -519,26 +479,26 @@ void SRV_Channels::push() #if AP_VOLZ_ENABLED // give volz library a chance to update - volz_ptr->update(); + volz.update(); #endif #if AP_SBUSOUTPUT_ENABLED // give sbus library a chance to update - sbus_ptr->update(); + sbus.update(); #endif #if AP_ROBOTISSERVO_ENABLED // give robotis library a chance to update - robotis_ptr->update(); + robotis.update(); #endif #if HAL_SUPPORT_RCOUT_SERIAL // give blheli telemetry a chance to update - blheli_ptr->update_telemetry(); + blheli.update_telemetry(); #endif #if AP_FETTEC_ONEWIRE_ENABLED - fetteconwire_ptr->update(); + fetteconwire.update(); #endif #if AP_KDECAN_ENABLED @@ -585,11 +545,12 @@ void SRV_Channels::zero_rc_outputs() * send an invalid signal to all channels to prevent * undesired/unexpected behavior */ - cork(); + auto &srv = AP::srv(); + srv.cork(); for (uint8_t i=0; iwrite(i, 0); } - push(); + srv.push(); } /* @@ -619,3 +580,12 @@ void SRV_Channels::set_emergency_stop(bool state) { #endif emergency_stop = state; } + +namespace AP { + +SRV_Channels &srv() +{ + return *SRV_Channels::get_singleton(); +} + +};