diff --git a/docs/Board - ChebuzzF3.md b/docs/Board - ChebuzzF3.md deleted file mode 100644 index 7df5604125..0000000000 --- a/docs/Board - ChebuzzF3.md +++ /dev/null @@ -1,88 +0,0 @@ - -## Connections - -Board orientation. - -These notes assume that when the board is placed with the header pins facing up, the bottom right of the board is next to the 8 sets of INPUT pin headers. -Inner means between the two rows of header sockets, outer means between the left/right board edges and the header sockets. - - -### SPI2 / External SPI - -sclk GPIOB 13 -miso GPIOB 14 -mosi GPIOB 15 - - -There are 4 pins, labelled CS1-4 next to a label that reads Ext SPI. The 3rd pin is connected to the flash chip on -the bottom right inner of the board. The other pins on the flash chip are wired up to PB3/4/5 - -### SPI3 / SPI - -sclk GPIOB 3 -miso GPIOB 4 -mosi GPIOB 5 - -ssel 1 GPIOB 10 / Ext SPI CS1 -ssel 2 GPIOB 11 / Ext SPI CS2 -ssel 3 GPIOB 12 / Ext SPI CS3 - wired up to Slave Select of M25P16 15MBitFlash chip -ssel 4 GPIOB 13 / Ext SPI CS4 - not usable since it is used for SPI2 sclk - -### RC Input - -INPUT -PA8 / CH1 - TIM1_CH1 -PB8 / CH2 - TIM16_CH1 -PB9 / CH3 - TIM17_CH1 -PC6 / CH4 - TIM8_CH1 -PC7 / CH5 - TIM8_CH2 -PC8 / CH6 - TIM8_CH3 -PF9 / CH7 - TIM15_CH1 -PF10 / CH8 - TIM15_CH2 - -### PWM Outputs - -OUTPUT -PD12 / CH1 - TIM4_CH1 -PD13 / CH2 - TIM4_CH2 -PD14 / CH3 - TIM4_CH3 -PD15 / CH4 - TIM4_CH4 -PA1 / CH5 - TIM2_CH2 -PA2 / CH6 - TIM2_CH3 -PA3 / CH7 - TIM2_CH4 -PB0 / CH8 - TIM3_CH3 -PB1 / CH9 - TIM3_CH4 -PA4 / CH10 - TIM3_CH2 - -### Other ports - -There is space for a MS5611 pressure sensor at the top left inner of the board. - -There is an I2C socket on the left outer of the board which connects to a PCA9306 I2C level shifter directly opposite (inner). -The PCA9306 is not populated on some boards and thus the I2C socket is unusable. - -There is a CAN socket on the top right outer of the board which connects to a MAX3015 CAN Tranceiver. -The MAX3015 is not populated on some boards and thus the CAN socket is unusable. - -There are some solder pads labelled Ext 1-4 at the top right inner of the board. - -GPIOE 6 / PE6 / Ext 1 -GPIOD 3 / PD3 / Ext 2 -GPIOD 4 / PD4 / Ext 3 -GPIOB 3 / PB3 / Ext 4 - -There are some solder pads labelled ADC0-3 & Diff Press at the top left inner of the board -They are connected to the ADC socket at the top left outer of the board - -PC3 / Diff Press - ADC12_IN9 (Differential Pressure) -PC2 / ADC2 - ADC12_IN8 -PC1 / ADC1 - ADC12_IN7 -PC0 / ADC0 - ADC12_IN6 - -There is space for a MPXV5004/MPVZ5004 differential pressure sensor, if populated it's analog pin connects to PC3. - -There are sockets for 5 UARTs labelled USART1-5. - -There is a socket labelled RX_IN. - -GPIOD 2 / PD2 / RX_IN diff --git a/docs/Board - ColibriRace.md b/docs/Board - ColibriRace.md deleted file mode 100755 index 89a3de4c58..0000000000 --- a/docs/Board - ColibriRace.md +++ /dev/null @@ -1,126 +0,0 @@ -# Board - Colibri RACE - -The Colibri RACE is a STM32F3 based flight control designed specifically to work with the TBS POWERCUBE multi rotor stack. - -## Hardware Features: -* STM32F303 based chipset for ultimate performance -* PPM, SBUS, DSM, DSMX input (5V and 3.3V provided over internal BUS). No inverters or hacks needed. -* 6 PWM ESC output channels (autoconnect, internal BUS) - * RGB LED strip support incl. power management - * Extension port for GPS / external compass / pressure sensor - * UART port for peripherals (Blackbox, FrSky telemetry etc.) - * Choose between plug & play sockets or solder pads for R/C and buzzer - * 5V buzzer output - * MPU6500 new generation accelerometer/gyro - * 3x status LED (DCDC pwr/ 3.3V pwr/ status) - * Battery monitoring for 12V, 5V and VBat supply - * Size: 36mmx36mm (30.5mm standard raster) - * Weight: 4.4g - - For more details please visit: - http://www.team-blacksheep.com/powercube - -## Serial Ports - - | Value | Identifier | Board Markings | Notes | - | ----- | ------------ | -------------- | ------------------------------------------| - | 1 | VCP | USB PORT | Main Port For MSP | - | 2 | USART1 | FREE PORT | PC4 and PC5(Tx and Rx respectively) | - | 3 | USART2 | PPM Serial | PA15 | - | 4 | USART3 | GPS PORT | PB10 and PB11(Tx and Rx respectively) | - -## Pinouts - - Full pinout details are available in the manual, here: - - http://www.team-blacksheep.com/colibri_race - - -### SWD - ICSP - - | Pin | Function | Notes | - | --- | -------------- | -------------------------------------------- | - | 1 | VCC_IN | 3.3 Volt | - | 2 | SWDIO | | - | 3 | nRESET | | - | 4 | SWCLK | | - | 5 | Ground | | - | 6 | SWO/TDO | | - -### Internal Bus - - | Pin | Function | Notes | - | --- | -------------- | -------------------------------------------- | - | 1 | PWM1 | MOTOR 1 | - | 2 | PWM2 | MOTOR 2 | - | 3 | PWM3 | MOTOR 3 | - | 4 | PWM4 | MOTOR 4 | - | 5 | PWM5 | MOTOR 5 (For Y6 or Hex X) | - | 6 | PWM6 | MOTOR 6 (For Y6 or Hex X) | - | 7 | BST SDA | Use For TBS CorePro Control Device | - | 8 | BST SCL | Use For TBS CorePro Control Device | - | 9 | PWM7 | Can be a normal GPIO (PA1) or PWM | - | 10 | PWM8 | Can be a normal GPIO (PA2) or PWM | - | 11 | 12.2V DCDC | If 12v is detected, the Blue LED will turn on| - | 12 | 5.1V DCDC | Voltage for MCU | - -### Servo - - | Pin | Function | Notes | - | --- | -------------- | -------------------------------------------- | - | 1 | Ground | | - | 2 | VCC_OUT | 5.1 Volt output to LCD Strip | - | 3 | PWM Servo | PB14 - PWM10 | - -### IO_1 - LED Strip - - | Pin | Function | Notes | - | --- | ----------------- | -------------------------------------------- | - | 1 | LED_STRIP | Enable `feature LED_STRIP` | - | 2 | VCC_OUT | 5.1 Volt output to LCD Strip | - | 3 | Ground | | - -### IO_2 - Sensor Interface - - | Pin | Function | Notes | - | --- | ----------------- | -------------------------------------------- | - | 1 | VCC_OUT | 4.7 Volt output to the device | - | 2 | Ground | | - | 3 | UART3 TX | GPS | - | 4 | UART3 RX | GPS | - | 5 | SDA | mag, pressure, or other i2c device | - | 6 | SCL | mag, pressure, or other i2c device | - -### IO_3 - RC input - - IO_3 is used for RX_PPM/RX_SERIAL. Under the `PORT` tab, set RX_SERIAL to UART2 when using RX_SERIAL. - - | Pin | Function | Notes | - | --- | ----------------- | -------------------------------------------- | - | 1 | PPM/Serial | Can PPM or Serial input | - | 2 | VCC_OUT | 3.3 Volt output to the device | - | 3 | Ground | | - | 4 | VCC_OUT | 5.1 Volt output to the device | - -### IO_4 - Buzzer - - | Pin | Function | Notes | - | --- | ----------------- | -------------------------------------------- | - | 1 | BUZZER | Normal high (5.1v) | - | 2 | VCC_OUT | 5.1 Volt output to the device | - -### IO_5 - Free UART - - | Pin | Function | Notes | - | --- | ----------------- | -------------------------------------------- | - | 1 | UART1 TX | Free UART | - | 2 | UART1 RX | Free UART | - | 3 | Ground | | - | 4 | VCC_OUT | 4.7 Volt output to the device | - -### IO_6 - IR TX (extension) - - | Pin | Function | Notes | - | --- | ----------------- | -------------------------------------------- | - | 1 | IR TX | | - | 2 | Ground | | diff --git a/docs/Board - MotoLab.md b/docs/Board - MotoLab.md deleted file mode 100644 index 8a5f0f90ff..0000000000 --- a/docs/Board - MotoLab.md +++ /dev/null @@ -1,101 +0,0 @@ -# Board - MotoLab - -The MOTOLAB build target supports the STM32F3-based boards provided by MotoLab. - -At present this includes the TornadoFC and MotoF3. The TornadoFC is described here: - -http://www.rcgroups.com/forums/showthread.php?t=2473157 - -The MotoF3 documentation will be provided when the board is available. - -Both boards use the STM32F303 microcontroller and have the following features: - -* 256K bytes of flash memory -* Floating point math coprocessor -* Three hardware serial port UARTs -* USB using the built-in USB phy that does not interfere with any hadware UART -* Stable voltage regulation -* High-current buzzer/LED output -* Serial LED interface -* Low-pass filtered VBAT input with 1/10 divider ratio -* 8 short-circuit protected PWM outputs, with 5V buffering on the TornadoFC -* On-board 6S-compatible switching regulator (MotoF3) -* Direct mounting option for a Pololu switching regulator for up to 6S lipo operation (TornadoFC) - - -# Flashing - -The MotoLab boards use the internal DFU USB interface on the STM32F3 microcontroller which is not compatible with the INAV configurator flashing tool. - -Instead, on Windows you can use the Impulse Flashing Utility from ImpulseRC, available here: - -http://www.warpquad.com/ImpulseFlash.zip - -Download and unzip the program. Start the program, plug in the USB on the target board, and drag and drop the intended binary file onto the program icon. The program will put the STM32F3 into bootloader mode automatically and load the binary file to the flash. - -For programming on Linux, use the dfu-util program which is installed by default on Ubuntu-based systems. Connect the boot pins on the board and plug in the USB. - -Verify that the system identifies the DFU device with this command: -``` -dfu-util -l -``` - -The output should list a "Found DFU" device, something like this: -``` -dfu-util 0.5 - -(C) 2005-2008 by Weston Schmidt, Harald Welte and OpenMoko Inc. -(C) 2010-2011 Tormod Volden (DfuSe support) -This program is Free Software and has ABSOLUTELY NO WARRANTY - -dfu-util does currently only support DFU version 1.0 - -Found DFU: [0483:df11] devnum=0, cfg=1, intf=0, alt=0, name="@Internal Flash /0x08000000/128*0002Kg" -Found DFU: [0483:df11] devnum=0, cfg=1, intf=0, alt=1, name="@Option Bytes /0x1FFFF800/01*016 e" -``` - -Use this command to load the binary file to the flash memory on the board: -``` -dfu-util --alt 0 -s 0x08000000 -D -``` - -The output should look something like this: -``` -dfu-util 0.5 - -(C) 2005-2008 by Weston Schmidt, Harald Welte and OpenMoko Inc. -(C) 2010-2011 Tormod Volden (DfuSe support) -This program is Free Software and has ABSOLUTELY NO WARRANTY - -dfu-util does currently only support DFU version 1.0 - -Opening DFU USB device... ID 0483:df11 -Run-time device DFU version 011a -Found DFU: [0483:df11] devnum=0, cfg=1, intf=0, alt=0, name="@Internal Flash /0x08000000/128*0002Kg" -Claiming USB DFU Interface... -Setting Alternate Setting #0 ... -Determining device status: state = dfuDNLOAD-IDLE, status = 0 -aborting previous incomplete transfer -Determining device status: state = dfuIDLE, status = 0 -dfuIDLE, continuing -DFU mode device DFU version 011a -Device returned transfer size 2048 -No valid DFU suffix signature -Warning: File has no DFU suffix -DfuSe interface name: "Internal Flash " -``` - -A binary file is required for the Impulse flashing Utility and dfu-util. The binary file can be built as follows: -``` -make TARGET=MOTOLAB clean -make TARGET=MOTOLAB binary -``` - -To completely erase the flash, create an all-zero file with this command on linux: -``` -dd if=/dev/zero of=zero.bin bs=1 count=262144 -``` - -## Todo - -Pinout documentation diff --git a/docs/Board - OMNIBUS.md b/docs/Board - OMNIBUS.md deleted file mode 100644 index 7918e8a5bf..0000000000 --- a/docs/Board - OMNIBUS.md +++ /dev/null @@ -1,87 +0,0 @@ -# Board - OMNIBUS F3 - -> This board is not supported in recent INAV releases - -## Hardware Features - -Refer to the product web page: -[OMNIBUS AIO F3 Flight Control](http://shop.myairbot.com/index.php/flight-control/cleanflight-baseflight/omnibusv11.html) - -### Hardware Notes - -There are few things to note on how things are connected on the board. - -1. VBAT (J4) -This is a battery input to the board, and is also a input to voltage sensor. - -2. J11 Power distribution -The RAM is user defined power rail, and all RAM through holes (J6, J7 and J11) are connected together. By connecting 5V or VBAT to RAM at J11, the RAM becomes 5V or VBAT power rail respectively. The VBAT on J11 can also be used to power the Board if necessary. - -3. RSSI (J4) -The pin is labelled as RSSI, but it will not be used for RSSI input for a hardware configuration limitation. In this document, the "RSSI" is used to indicate the pin location, not the function. - -4. UART1 in boot-loader/DFU mode -The UART1 is scanned during boot-loader/DFU mode, together with USB for possible interaction with a host PC. It is observed that devices that autonomously transmits some data, such as GPS, will prevent the MCU to talk to the USB. It is advised not to connect or disconnect such devices to/from UART1. UART2 is safe from this catch. - -## iNav Specific Target Configuration - -The first support for the OMNIBUS F3 appeared in BetaFlight. -The OMNIBUS target in iNav has different configuration from the BetaFlight support, to maximize the hardware resource utilization for navigation oriented use cases. - - [PIN CONFIGURATION PIC HERE] - -### PWM Outputs - -Six PWM outputs (PWM1~PWM6) are supported, but PWM5 and PWM6 is not available when UART3 is in use. -PWM7 and PWM8 are dedicated for I2C; in this document, they are used to indicate the pin location, not the function. - -If servos are used on a multirotor mixer (i.e. Tricopter) PWM1 is remapped to servo and motor 1 is moved to PWM2 etc. - -Note: Tested only for QUAD-X configuration. - -### Hardware UART Ports - -PPM/SBUS jumper for J8 is assumed to be configured for PPM (SBUS=R18 removed). With newer boards (the 1.1 Version) you don't have to swap an smd resistor to use SBUS anymore. It just works out of the box. - -| UART | Location | Note | -|-------|----------|-------------------| -| UART1 |J13 | | -| UART2 |J12 | | -| UART3 |J22 | PWM5=TX3,PWM6=RX3 | - -All UARTs are Serial RX capable. - -### I2C - -I2C is available on J22 PWM7 and PWM8 - -|signal | Location | Alt. Location | -|-------|------------|---------------| -|SCL | J22 (PWM8) | J3 (SCL) | -|SDA | J22 (PWM7) | J3 (SDA) | - -### RANGEFINDER - -HC-SR04 rangefinder is supported when NOT using PPM. - -|signal | Location | -|-------|------------| -|TRIG | J8 (PPM) | -|ECHO | J4 (RSSI) | - -5V rangefinder can be connected directly without inline resistors. - -### OSD - -Integrated OSD is supported. - -### RSSI Sensor Input - -The RSSI sensor adc is not supported due to the hardware configuration limitation. - -## Usage in a Fixed Wing -Due to the way INAV handles PWM outputs the first 2 PWM outputs are reserved for the motor outputs. When using SBUS on UART3 as recommended this leaves only 2 additional outputs for the servos, as output 5 and 6 are blocked by UART3 serial for SBUS and 7 and 8 are used for I2C. - -You can free PWM outputs 5 and 6 by simply connecting SBUS up to UART1. For FrSky there is no hardware inverter needed as the F3 chip UARTs can handle this without additional hardware. Just make sure that `sbus_inversion = ON` is set. However, you will not be able to use UART3, e.G. for telemetry. - -This allows to control a standard airplane with rudder, ailerons and elevator. If you use flaps or a servo gimbal, you can bypass the FC by connecting it up to the receiver directly. diff --git a/docs/Board - Paris Air Hero 32 F3.md b/docs/Board - Paris Air Hero 32 F3.md deleted file mode 100755 index e1f0d58811..0000000000 --- a/docs/Board - Paris Air Hero 32 F3.md +++ /dev/null @@ -1,46 +0,0 @@ -# Board - Paris Air Hero 32 - -This is the AIR3 PARIS Sirius AirHERO 32 F3 board from MultiWiiCopter -Source: http://www.multiwiicopter.com/products/inav-air3-fixed-wing - -## Sensors - -MPU6500 via SPI interface. -BMP280 via SPI interface - -## Ports - -6 x 3pin ESC / Servo outputs -1 x 8pin JST connector (PPM/PWM/UART2) -1 x 4pin JST connector (UART3) - -## I2C bus - -I2C bus is made available with a special target - AIRHEROF3_QUAD. This target limits motor outputs to 4 and adds I2C bus at M5/M6 connectors. - -## Pinouts - -The 10 pin RC I/O connector has the following pinouts when used in RX_PPM/RX_SERIAL mode. - -From right to left when looking at the socket from the edge of the board. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------- | -| 1 | Ground | | -| 2 | +5V | | -| 3 | RX_PPM | Enable `feature RX_PPM` | -| 4 | AIRSPEED | Airspeed sensor (3.3V max) | -| 5 | USART2 TX | | -| 6 | USART2 RX | | -| 7 | SS1 RX | Enable `feature SOFT_SERIAL` | -| 8 | SS1 TX | | - - -## Serial Ports - -| Value | Identifier | RX | TX | Notes | -| ----- | ------------ | ---------- | ------------------ | ------------------------------------------------------------------------------------------- | -| 1 | USART1 | RX / PA10 | TX / PA9 | Internally connected to USB port via CP2102 IC | -| 2 | USART2 | RC4 / PA3 | RC3 / PA2 | | -| 3 | USART3 | F3 / PB11 | F2 / PB10 | | - diff --git a/docs/Board - Paris Air Hero 32.md b/docs/Board - Paris Air Hero 32.md deleted file mode 100644 index d07e9a309c..0000000000 --- a/docs/Board - Paris Air Hero 32.md +++ /dev/null @@ -1,46 +0,0 @@ -# Board - Paris Air Hero 32 - -## Sensors - -MPU6500 via SPI interface. - -## Ports - -6 x 3pin ESC / Servo outputs -1 x 8pin JST connector (PPM/PWM/UART2) -1 x 4pin JST connector (UART3/I2C) - -## Pinouts - -The 10 pin RC I/O connector has the following pinouts when used in RX_PPM/RX_SERIAL mode. - -From right to left when looking at the socket from the edge of the board. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------- | -| 1 | Ground | | -| 2 | +5V | | -| 3 | RX_PPM | Enable `feature RX_PPM` | -| 4 | RSSI_ADC | Enable `feature RSSI_ADC`. Connect to the output of a PWM-RSSI conditioner, 0v-3.3v input | -| 5 | USART2 TX | | -| 6 | USART2 RX | Built-in inverter | -| 7 | LED_STRIP | Enable `feature LED_STRIP` | -| 8 | unused | | - -When SOFTSERIAL is enabled, LED_STRIP and CURRENT_METER are unavailable, but one SoftSerial port is made available to use instead. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------- | -| 7 | SOFTSERIAL1 RX | Enable `feature SOFTSERIAL` | -| 8 | SOFTSERIAL1 TX | | - - -## Serial Ports - -| Value | Identifier | RX | TX | Notes | -| ----- | ------------ | ---------- | ------------------ | ------------------------------------------------------------------------------------------- | -| 1 | USART1 | RX / PA10 | TX / PA9 / TELEM | TELEM output is always inverted (for FrSky). Internally connected to USB port via CP2102 IC | -| 2 | USART2 | RC4 / PA3 | RC3 / PA2 | | -| 3 | USART3 | F3 / PB11 | F2 / PB10 | Flex port is configured as UART3 when port is configured | -| 4 | SOFTSERIAL1 | RC5 / PA6 | RC6 / PA7 | | - diff --git a/docs/Board - RMDO.md b/docs/Board - RMDO.md deleted file mode 100644 index c785930f46..0000000000 --- a/docs/Board - RMDO.md +++ /dev/null @@ -1,12 +0,0 @@ -# Board - RMDO - -The DoDo board is a clone of the SPRacingF3 board in terms of CPU pin mappings. See the SPRacingF3 documentation. - -Hardware differences compared to SPRacingF3 are as follows: - -* Rev 1 and Rev 2: the CPU is the cheaper version of the F3 with only 128KB FLASH. Rev 3: the CPU is a F3 version with 256KB FLASH. -* 2MBit flash size -* The barometer is the cheaper BMP280. -* It does not have any compass sensor. -* Onboard BEC. -* Different physical connectors/pins/pads/ports. diff --git a/docs/Board - SPRacingF3.md b/docs/Board - SPRacingF3.md deleted file mode 100644 index b54354f69b..0000000000 --- a/docs/Board - SPRacingF3.md +++ /dev/null @@ -1,97 +0,0 @@ -# Board - SPRacingF3 - -The Seriously Pro Racing MOF3 board (SPRacingF3) is the first board designed specifically for INAV. - -Full details available on the website, here: - -http://seriouslypro.com/spracingf3 - -## Hardware Features - -* No compromise I/O. Use all the features all the time; e.g. Connect your OSD + SmartPort + SBus + GPS + LED Strip + Battery Monitoring + HC-SR04 + 8 motors - all at the same time! -* On-board high-capacity black box flight log recorder - optimize your tuning and see the results of your setup without guesswork. (Acro and Deluxe) -* Next-generation STM32 F3 processor with hardware floating point unit for efficient flight calculations and faster ARM-Cortex M4 core. -* Stackable design - perfect for integrating with OSDs and power distribution boards. -* 16 PWM I/O lines for ESCs, Servos and legacy receivers. 8 available on standard pin headers. 8 via side mounted connectors. -* Supports SBus, SumH, SumD, Spektrum1024/2048, XBus, PPM, PWM receivers. No external inverters required (built-in). -* Dedicated output for programmable LEDs - great for orientation, racing and night flying. -* Dedicated I2C port for connection of OLED display without needing flight battery. -* Battery monitoring ports for voltage and current. -* Buzzer port for audible warnings and notifications. -* Solder pads in addition to connectors for HC-SR04, PPM, RSSI, Current, GPIO, LED Strip, 3.3v, -* Developer friendly debugging port (SWD) and boot mode selection, unbrickable bootloader. -* Symmetrical design for a super tidy wiring. -* Wire up using using pin headers, JST-SH sockets or solder pads. Use either right-angled or straight pin-headers. -* Barometer mounted on the bottom of the board for easy wind isolation. - -## Serial Ports - -| Value | Identifier | RX | TX | 5v Tolerant | Notes | -| ----- | ------------ | ------------ | ------------ | ----------- | ------------------------------------------------------------------------------------------- | -| 1 | USART1 | PA10 | PA9 | YES | Internally connected to USB port via CP2102 IC. Also available on a USART1 JST connector and on through hole pins. | -| 2 | USART2 | PA15 | PA14 | YES | Available on USART2 JST port only. | -| 3 | USART3 | PB11 / IO2_3 | PB10 / IO2_4 | NO | Available on IO_2, USART3 JST port and through hole pins. | - -* You cannot use SWD and USART2 at the same time. -* You may encounter flashing problems if you have something connected to the USART1 RX/TX pins. Power other devices of and/or disconnect them. - -## Pinouts - -Full pinout details are available in the manual, here: - -http://seriouslypro.com/spracingf3#manual - -### IO_1 - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | RX_PPM | Enable `feature RX_PPM` | -| 4 | GPIO | | -| 5 | SoftSerial1_RX | | -| 6 | SoftSerial1_TX | | -| 7 | LED_STRIP | Enable `feature LED_STRIP` | -| 8 | VCC | 3.3v output for LOW CURRENT application only | - -### IO_2 - -| Pin | Function | Notes | -| --- | ------------------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | RX_SERIAL | UART3 RX | -| 4 | | UART3_TX | -| 5 | HC-SR04_TRIG/SoftSerial2_RX | Enable `feature SOFTSERIAL` or HC-SR04 rangefinder | -| 6 | HC-SR04_ECHO/SoftSerial2_TX | Enable `feature SOFTSERIAL` or HC-SR04 rangefinder | -| 7 | ADC_1 | Current Sensor | -| 8 | ADC_2 | RSSI | - -### UART1/2/3 - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | TXD | | -| 4 | RXD | | - -### I2C - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | 5.0v | Voltage as-supplied by BEC OR USB, always on | -| 3 | SCL | | -| 4 | SDA | | - -### SWD - -The port cannot be used at the same time as UART2. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | NRST | Voltage as-supplied by BEC OR USB, always on | -| 3 | SWDIO | | -| 4 | SWDCLK | | diff --git a/docs/Board - SPRacingF3EVO.md b/docs/Board - SPRacingF3EVO.md deleted file mode 100644 index 874f3e38f6..0000000000 --- a/docs/Board - SPRacingF3EVO.md +++ /dev/null @@ -1,160 +0,0 @@ -# Board - Seriously Pro SP Racing F3 EVO - -The Seriously Pro Racing F3 Evo board (SPRacingF3EVO) is the evolution of the first board designed specifically for Cleanflight. - -Purchasing boards directly from SeriouslyPro / SP Racing and official retailers helps fund Cleanflight development, it's the reason the Seriously Pro boards exist! Official retailers are always listed on the SeriouslyPro.com website. - -Full details available on the website, here: - -http://seriouslypro.com/spracingf3evo - -## Hardware Features - -* Next-generation STM32 F3 processor with hardware floating point unit for efficient flight calculations and faster ARM-Cortex M4 core. -* MicroSD-Card socket for black box flight log recorder - optimize your tuning and see the results of your setup without guesswork. -* Race transponder built in - just turn up at a race and have your lap times recorded. -* Features the latest Accelerometer, Gyro and Mag/Compass and Baro/Altitude sensor technology. -* Wire up using using pin headers for all major connections for excellent crash-durability. Use either right-angled or straight pin-headers. -* No compromise I/O. Use all the features all the time; e.g. Connect your USB + OSD + SmartPort + SBus + GPS + LED Strip + Battery Monitoring + 8 motors - all at the same time! -* 8 PWM output lines for ESCs and Servos. Arranged for easy wiring on standard pin headers. -* Supports direct connection of SBus, SumH, SumD, Spektrum1024/2048, XBus receivers. No external inverters required (built-in). -* Supports direct connection of 3.3v Spektrum Satellite receivers via 3 pin through-hole JST-ZH connector. -* Dedicated PPM receiver input. -* 3 Serial Ports - NOT shared with the USB socket. -* Telemetry port -* Micro USB socket. -* Dedicated output for programmable LEDs - great for orientation, racing and night flying. (Currently mutually exclusive with the Transponder). -* Dedicated I2C port for connection of OLED display without needing flight battery. -* Battery monitoring for voltage and current. -* RSSI monitoring (analogue or PWM). -* Buzzer port for audible warnings and notifications. -* Developer friendly debugging port (SWD) and boot mode selection, unbrickable bootloader. -* Symmetrical design for a super tidy wiring. -* JST-SH sockets only for I2C, UART2 and SWD. UART2 also on through-hole pins. -* Flashing via USB or serial port. -* Stackable design - perfect for integrating with OSDs and power distribution boards. -* Standard mounting - 36x36mm with standard 30.5mm mounting holes. -* LEDs for 3v, 5v and Status for easy diagnostics. -* Copper-etched Cleanflight logo. - -## Serial Ports - -| Value | Identifier | RX | TX | 5v Tolerant | Notes | -| ----- | ------------ | ------------ | ------------ | ----------- | ------------------------------------------------------------------------------------------- | -| 1 | USART1 | PA10 | PA9 | YES | 2 through-hole pins. Use for connecting to OSD/GPS/BlueTooth. | -| 2 | USART2 | PA15 | PA14 / SWCLK | YES | JST socket and PPM header. Use to connect to RX. | -| 3 | USART3 | PB11 / AF7 | PB10 / AF7 | NO | Available on 4 through-hole pins. 3.3V signals only ! Use for GPS, Spektrum Satellite RX, SmartPort Telemetry, HoTT telemetry, etc. | - -* You cannot use SWD and USART2 at the same time. -* When using a Serial RX receiver the TXD (T2) pin cannot be used for telemetry. Use UART3 TXD instead. -* Two SoftSerial is supported. Enabling SoftSerial disables Servo output 3,4,5,6 -* Windows DFU Flashing requires Zadig (see configurator) - -### SoftSerial - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------- | -| 5 | SOFTSERIAL1 RX | SoftSerial replaces Servo 3,4,5,6| -| 6 | SOFTSERIAL1 TX | | -| 7 | SOFTSERIAL2 RX | | -| 8 | SOFTSERIAL2 TX | | - - -## Pinouts - -Full pinout details are available in the manual, here: - -http://seriouslypro.com/files/SPRacingF3EVO-Manual-latest.pdf - -### IO_1 - -The 6 pin IO_1 connector has the following pinouts when used in RX_SERIAL mode. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | RX_SERIAL | Enable `feature RX_SERIAL` | -| 4 | | | -| 5 | +V BATTERY | Voltage as-supplied by Battery. | -| 6 | -V BATTERY | Voltage as-supplied by Battery. | - -When RX_PPM is used the IO_1 pinout is as follows. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | RX_PPM | Enable `feature RX_PPM` | -| 4 | TELEMETRY | Enable `feature TELEMETRY` | -| 5 | +V BATTERY | Voltage as-supplied by Battery. | -| 6 | -V BATTERY | Voltage as-supplied by Battery. | - -### IO_2 - -When TRANSPONDER is used and the IR solder pads are shorted, the 6 pin IO_2 pinout is as follows. - -| Pin | Function | Notes | -| --- | ------------------------- | -------------------------------------------- | -| 1 | IR- | Short leg of the IR LED | -| 2 | IR+ | Long leg of the IR LED | -| 3 | CURRENT | Current Sensor | -| 4 | RSSI | RSSI (PWM or Analog - select by solder pads) | -| 5 | BUZZER+ | 5V Source | -| 6 | BUZZER- | Buzzer signal | - -When LEDSTRIP is used and the LED solder pads are shorted, the 6 pin IO_2 pinout is as follows. - -| Pin | Function | Notes | -| --- | ------------------------- | -------------------------------------------- | -| 1 | | | -| 2 | LEDSTRIP | WS2812 Ledstrip data | -| 3 | CURRENT | Current Sensor | -| 4 | RSSI | RSSI (PWM or Analog - select by solder pads) | -| 5 | BUZZER+ | 5V Source | -| 6 | BUZZER- | Buzzer signal | - -### UART1 - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 3 | TXD | | -| 4 | RXD | | - -### UART2/3 - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | TXD | | -| 4 | RXD | | - -### Spektrum Satellite - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 3 | 3.3V | | -| 2 | Ground | | -| 1 | RXD | | - -### I2C - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | 5.0v | Voltage as-supplied by BEC OR USB, always on | -| 3 | SCL | 3.3V signals only | -| 4 | SDA | 3.3V signals only | - -### SWD - -The port cannot be used at the same time as UART2. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | NRST | | -| 3 | SWDIO | | -| 4 | SWDCLK | | - diff --git a/docs/Board - SPRacingF3EVO_1SS.md b/docs/Board - SPRacingF3EVO_1SS.md deleted file mode 100644 index eeaf4233ee..0000000000 --- a/docs/Board - SPRacingF3EVO_1SS.md +++ /dev/null @@ -1,159 +0,0 @@ -# Board - Seriously Pro SP Racing F3 EVO - -The Seriously Pro Racing F3 Evo board (SPRacingF3EVO) is the evolution of the first board designed specifically for Cleanflight. - -Purchasing boards directly from SeriouslyPro / SP Racing and official retailers helps fund Cleanflight development, it's the reason the Seriously Pro boards exist! Official retailers are always listed on the SeriouslyPro.com website. - -Full details available on the website, here: - -http://seriouslypro.com/spracingf3evo - -## Hardware Features - -* Next-generation STM32 F3 processor with hardware floating point unit for efficient flight calculations and faster ARM-Cortex M4 core. -* MicroSD-Card socket for black box flight log recorder - optimize your tuning and see the results of your setup without guesswork. -* Race transponder built in - just turn up at a race and have your lap times recorded. -* Features the latest Accelerometer, Gyro and Mag/Compass and Baro/Altitude sensor technology. -* Wire up using using pin headers for all major connections for excellent crash-durability. Use either right-angled or straight pin-headers. -* No compromise I/O. Use all the features all the time; e.g. Connect your USB + OSD + SmartPort + SBus + GPS + LED Strip + Battery Monitoring + 8 motors - all at the same time! -* 8 PWM output lines for ESCs and Servos. Arranged for easy wiring on standard pin headers. -* Supports direct connection of SBus, SumH, SumD, Spektrum1024/2048, XBus receivers. No external inverters required (built-in). -* Supports direct connection of 3.3v Spektrum Satellite receivers via 3 pin through-hole JST-ZH connector. -* Dedicated PPM receiver input. -* 3 Serial Ports - NOT shared with the USB socket. -* Telemetry port -* Micro USB socket. -* Dedicated output for programmable LEDs - great for orientation, racing and night flying. (Currently mutually exclusive with the Transponder). -* Dedicated I2C port for connection of OLED display without needing flight battery. -* Battery monitoring for voltage and current. -* RSSI monitoring (analogue or PWM). -* Buzzer port for audible warnings and notifications. -* Developer friendly debugging port (SWD) and boot mode selection, unbrickable bootloader. -* Symmetrical design for a super tidy wiring. -* JST-SH sockets only for I2C, UART2 and SWD. UART2 also on through-hole pins. -* Flashing via USB or serial port. -* Stackable design - perfect for integrating with OSDs and power distribution boards. -* Standard mounting - 36x36mm with standard 30.5mm mounting holes. -* LEDs for 3v, 5v and Status for easy diagnostics. -* Copper-etched Cleanflight logo. - -## Serial Ports - -| Value | Identifier | RX | TX | 5v Tolerant | Notes | -| ----- | ------------ | ------------ | ------------ | ----------- | ------------------------------------------------------------------------------------------- | -| 1 | USART1 | PA10 | PA9 | YES | 2 through-hole pins. Use for connecting to OSD/GPS/BlueTooth. | -| 2 | USART2 | PA15 | PA14 / SWCLK | YES | JST socket and PPM header. Use to connect to RX. | -| 3 | USART3 | PB11 / AF7 | PB10 / AF7 | NO | Available on 4 through-hole pins. 3.3V signals only ! Use for GPS, Spektrum Satellite RX, SmartPort Telemetry, HoTT telemetry, etc. | - -* You cannot use SWD and USART2 at the same time. -* When using a Serial RX receiver the TXD (T2) pin cannot be used for telemetry. Use UART3 TXD instead. -* One Software serial is supported in th SPRacingF3EVO_1SS version, see table below -* Windows DFU Flashing requires Zadig (see configurator) - -### SoftSerial - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------- | -| 7 | SOFTSERIAL1 RX | SoftSerial disables Servo 5,6 | -| 8 | SOFTSERIAL1 TX | | - -## Pinouts - -Full pinout details are available in the manual, here: - -http://seriouslypro.com/files/SPRacingF3EVO-Manual-latest.pdf - -### IO_1 - -The 6 pin IO_1 connector has the following pinouts when used in RX_SERIAL mode. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | RX_SERIAL | Enable `feature RX_SERIAL` | -| 4 | | | -| 5 | +V BATTERY | Voltage as-supplied by Battery. | -| 6 | -V BATTERY | Voltage as-supplied by Battery. | - -When RX_PPM is used the IO_1 pinout is as follows. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | RX_PPM | Enable `feature RX_PPM` | -| 4 | TELEMETRY | Enable `feature TELEMETRY` | -| 5 | +V BATTERY | Voltage as-supplied by Battery. | -| 6 | -V BATTERY | Voltage as-supplied by Battery. | - -### IO_2 - -When TRANSPONDER is used and the IR solder pads are shorted, the 6 pin IO_2 pinout is as follows. - -| Pin | Function | Notes | -| --- | ------------------------- | -------------------------------------------- | -| 1 | IR- | Short leg of the IR LED | -| 2 | IR+ | Long leg of the IR LED | -| 3 | CURRENT | Current Sensor | -| 4 | RSSI | RSSI (PWM or Analog - select by solder pads) | -| 5 | BUZZER+ | 5V Source | -| 6 | BUZZER- | Buzzer signal | - -When LEDSTRIP is used and the LED solder pads are shorted, the 6 pin IO_2 pinout is as follows. - -| Pin | Function | Notes | -| --- | ------------------------- | -------------------------------------------- | -| 1 | | | -| 2 | LEDSTRIP | WS2812 Ledstrip data | -| 3 | CURRENT | Current Sensor | -| 4 | RSSI | RSSI (PWM or Analog - select by solder pads) | -| 5 | BUZZER+ | 5V Source | -| 6 | BUZZER- | Buzzer signal | - -### UART1 - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 3 | TXD | | -| 4 | RXD | | - -### UART2/3 - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | TXD | | -| 4 | RXD | | - -### Spektrum Satellite - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 3 | 3.3V | | -| 2 | Ground | | -| 1 | RXD | | - -### I2C - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | 5.0v | Voltage as-supplied by BEC OR USB, always on | -| 3 | SCL | 3.3V signals only | -| 4 | SDA | 3.3V signals only | - -### SWD - -The port cannot be used at the same time as UART2. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | NRST | | -| 3 | SWDIO | | -| 4 | SWDCLK | | - - - diff --git a/docs/Board - Sparky.md b/docs/Board - Sparky.md deleted file mode 100644 index bc4612c574..0000000000 --- a/docs/Board - Sparky.md +++ /dev/null @@ -1,207 +0,0 @@ -# Board - Sparky - -The Sparky is a very low cost and very powerful board. - -* 3 hardware serial ports. -* Built-in serial port inverters which allows S.BUS receivers to be used without external inverters. -* USB (can be used at the same time as the serial ports). -* 10 PWM outputs. -* Dedicated PPM/SerialRX input pin. -* MPU9150 I2C Acc/Gyro/Mag -* Baro - -Tested with revision 1 & 2 boards. - -## TODO - -* Rangefinder -* Display (via Flex port) -* SoftSerial - though having 3 hardware serial ports makes it a little redundant. -* Airplane PWM mappings. - -# Voltage and current monitoring (ADC support) - -Voltage monitoring is possible when enabled via PWM9 pin and current can be monitored via PWM8 pin. The voltage divider and current sensor need to be connected externally. The vbatscale cli parameter need to be adjusted to fit the sensor specification. For more details regarding the sensor hardware you can check here: https://github.com/TauLabs/TauLabs/wiki/User-Guide:-Battery-Configuration - -# Flashing - -## Via Device Firmware Upload (DFU, USB) - Windows - -These instructions are for flashing the Sparky board under Windows using DfuSE. -Credits go to Thomas Shue (Full video of the below steps can be found here: https://www.youtube.com/watch?v=I4yHiRVRY94) - -Required Software: -DfuSE Version 3.0.2 (latest version 3.0.4 causes errors): http://code.google.com/p/multipilot32/downloads/detail?name=DfuSe.rar -STM VCP Driver 1.4.0: http://www.st.com/web/en/catalog/tools/PF257938 - -A binary file is required for DFU, not a .hex file. If one is not included in the release then build one as follows. - -``` -Unpack DfuSE and the STM VCP Drivers into a folder on your Hardrive -Download the latest Sparky release (inav_SPARKY.hex) from: -https://github.com/iNavFlight/inav/releases and store it on your Hardrive - -In your DfuSE folder go to BIN and start DfuFileMgr.exe -Select: "I want to GENERATE a DFUfile from S19,HEX or BIN files" press OK -Press: "S19 or Hex.." -Go to the folder where you saved the inav_SPARKY.hex file, select it and press open -(you might need to change the filetype in the DfuSE explorer window to "hex Files (*.hex)" to be able to see the file) -Press: "Generate" and select the .dfu output file and location -If all worked well you should see " Success for 'Image for lternate Setting 00 (ST..)'!" - -``` - -Put the device into DFU mode by powering on the sparky with the bootloader pins temporarily bridged. The only light that should come on is the blue PWR led. - -Check the windows device manager to make sure the board is recognized correctly. -It should show up as "STM Device in DFU mode" under Universal Serial Bus Controllers - -If it shows up as "STMicroelectronics Virtual COM" under Ports (COM & LPT) instead then the board is not in DFU mode. Disconnect the board, short the bootloader pins again while connecting the board. - -If the board shows up as "STM 32 Bootloader" device in the device manager, the drivers need to be updated manually. -Select the device in the device manager, press "update drivers", select "manual update drivers" and choose the location where you extracted the STM VCP Drivers, select "let me choose which driver to install". You shoud now be able to select either the STM32 Bootloader driver or the STM in DFU mode driver. Select the later and install. - - -Then flash the binary as below. - -``` -In your DfuSE folder go to BIN and start DfuSeDemo.exe -Select the Sparky Board (STM in DFU Mode) from the Available DFU and compatible HID Devices drop down list -Press "Choose.." at the bootom of the window and select the .dfu file created in the previous step -"File correctly loaded" should appear in the status bar -Press "Upgrade" and confirm with "Yes" -The status bar will show the upload progress and confirm that the upload is complete at the end - -``` - -Disconnect and reconnect the board from USB and continue to configure it via the INAV configurator as per normal - - -## Via Device Firmware Upload (DFU, USB) - Mac OS X / Linux - -These instructions are for dfu-util, tested using dfu-util 0.7 for OSX from the OpenTX project. - -http://www.open-tx.org/2013/07/15/dfu-util-07-for-mac-taranis-flashing-utility/ - -A binary file is required for DFU, not a .hex file. If one is not included in the release then build one as follows. - -``` -make TARGET=SPARKY clean -make TARGET=SPARKY binary -``` - -Put the device into DFU mode by powering on the sparky with the bootloader pins temporarily bridged. The only light that should come on is the blue PWR led. - -Run 'dfu-util -l' to make sure the device is listed, as below. - -``` -$ dfu-util -l -dfu-util 0.7 - -Copyright 2005-2008 Weston Schmidt, Harald Welte and OpenMoko Inc. -Copyright 2010-2012 Tormod Volden and Stefan Schmidt -This program is Free Software and has ABSOLUTELY NO WARRANTY -Please report bugs to dfu-util@lists.gnumonks.org - -Found DFU: [0483:df11] devnum=0, cfg=1, intf=0, alt=0, name="@Internal Flash /0x08000000/128*0002Kg" -Found DFU: [0483:df11] devnum=0, cfg=1, intf=0, alt=1, name="@Option Bytes /0x1FFFF800/01*016 e" -``` - -Then flash the binary as below. - -``` -dfu-util -D obj/inav_SPARKY.bin --alt 0 -R -s 0x08000000 -``` - -The output should be similar to this: - -``` -dfu-util 0.7 - -Copyright 2005-2008 Weston Schmidt, Harald Welte and OpenMoko Inc. -Copyright 2010-2012 Tormod Volden and Stefan Schmidt -This program is Free Software and has ABSOLUTELY NO WARRANTY -Please report bugs to dfu-util@lists.gnumonks.org - -Opening DFU capable USB device... ID 0483:df11 -Run-time device DFU version 011a -Found DFU: [0483:df11] devnum=0, cfg=1, intf=0, alt=0, name="@Internal Flash /0x08000000/128*0002Kg" -Claiming USB DFU Interface... -Setting Alternate Setting #0 ... -Determining device status: state = dfuERROR, status = 10 -dfuERROR, clearing status -Determining device status: state = dfuIDLE, status = 0 -dfuIDLE, continuing -DFU mode device DFU version 011a -Device returned transfer size 2048 -No valid DFU suffix signature -Warning: File has no DFU suffix -DfuSe interface name: "Internal Flash " -Downloading to address = 0x08000000, size = 76764 -...................................... -File downloaded successfully -can't detach -Resetting USB to switch back to runtime mode - -``` -On Linux you might want to take care that the modemmanager isn't trying to use your sparky as modem getting it into bootloader mode while doing so. In doubt you probably want to uninstall it. It could also be good idea to get udev fixed. It looks like teensy did just that -> http://www.pjrc.com/teensy/49-teensy.rules (untested) - -To make a full chip erase you can use a file created by -``` -dd if=/dev/zero of=zero.bin bs=1 count=262144 -``` -This can be used by dfu-util. - -## Via SWD - -On the bottom of the board there is an SWD header socket onto switch a JST-SH connector can be soldered. -Once you have SWD connected you can use the st-link or j-link tools to flash a binary. - -See Sparky schematic for CONN2 pinouts. - -## TauLabs bootloader - -Flashing INAV will erase the TauLabs bootloader, this is not a problem and can easily be restored using the st flashloader tool. - -# Serial Ports - -| Value | Identifier | RX | TX | Notes | -| ----- | ------------ | --------- | ---------- | -------------------------------------------------------------- | -| 1 | USB VCP | RX (USB) | TX (USB) | | -| 2 | USART1 | RX / PB7 | TX / PB6 | Conn1 / Flexi Port. | -| 3 | USART2 | RX / PA3 | PWM6 / PA2 | On RX is on INPUT header. Best port for Serial RX input | -| 4 | USART3 | RX / PB11 | TX / PB10 | RX/TX is on one end of the 6-pin header about the PWM outputs. | - -USB VCP *can* be used at the same time as other serial port. - -All USART ports all support automatic hardware inversion which allows direct connection of serial rx receivers like the FrSky X4RSB - no external inverter needed. - - -# Battery Monitoring Connections - -| Pin | Signal | Function | -| ---- | ------ | --------------- | -| PWM9 | PA4 | Battery Voltage | -| PWM8 | PA7 | Current Meter | - -## Voltage Monitoring - -The Sparky has no battery divider cricuit, PWM9 has an inline 10k resistor which has to be factored into the resistor calculations. -The divider circuit should eventally create a voltage between 0v and 3.3v (MAX) at the MCU input pin. - -WARNING: Double check the output of your voltage divider using a voltmeter *before* connecting to the FC. - -### Example Circuit - -For a 3Cell battery divider the following circuit works: - -`Battery (+) ---< R1 >--- PWM9 ---< R2 >--- Battery (-)` - -* R1 = 8k2 (Grey Red Red) -* R2 = 2k0 (Red Black Red) - -This gives a 2.2k for an 11.2v battery. The `vbat_scale` for this divider should be set around `52`. - -## Current Monitoring - -Connect a current sensor to PWM8/PA7 that gives a range between 0v and 3.3v out (MAX). diff --git a/docs/Safehomes.md b/docs/Safehomes.md index b3ed76403c..e805659518 100644 --- a/docs/Safehomes.md +++ b/docs/Safehomes.md @@ -12,14 +12,20 @@ One potential risk when landing is that there might be buildings, trees and othe ## Safehome -Safehomes are a list of GPS coordinates that identify safe landing points. When the flight controller is armed, it checks the list of safehomes. The nearest safehome that is enabled and within ```safehome_max_distance``` (default 200m) of the current position will be selected. Otherwise, it reverts to the old behaviour of using your current GPS position as home. +Safehomes are a list of GPS coordinates that identify safe landing points. You can define up to 8 safehomes for different locations you fly at. When the flight controller is armed, it checks the list of safehomes. The nearest safehome that is enabled and within ```safehome_max_distance``` (default 200m) of the current position is identified. The arming home location remains as home. -Be aware that the safehome replaces your arming position as home. When flying, RTH will return to the safehome and OSD elements such as distance to home and direction to home will refer to the selected safehome. +When RTH is activated, whether by radio failsafe, or using the RTH radio control mode, the safehome identified during arming will replace the home location. If RTH is turned off, either by regaining radio control or turning off the RTH radio control mode, the home location will return back to arming point. -You can define up to 8 safehomes for different locations you fly at. +The safehome operating mode is set using ```safehome_usage_mode```. If ```OFF```, safehomes will not be used. If ```RTH```, the safehome will replace the arming location when RTH is activated, either manually or because of RX failsafe. If ```RTH_FS```, the safehome will only be used for RX failsafe. This option can be changed using the OSD menu. + +If you frequently use RTH to return back to the arming point, you may not want the aircraft to fly to the safehome. Let it do this at least once to confirm safehomes is working as expected. Afterward, `set safehome_usage_mode = RTH_FS` and the safehome will only be used for failsafe. + +When using mode `RTH_FS`, you should confirm that your radio's failsafe configuration triggers the iNav failsafe mode. With many receivers, you have the ability to specify what signal to output during failsafe conditions. When you are choosing safehome locations, ensure that the location is clear of obstructions for a radius more than 50m (`nav_fw_loiter_radius`). As the plane descends, the circles aren't always symmetrical, as wind direction could result in some wider or tighter turns. Also, the direction and length of the final landing stage is also unknown. You want to choose a point that has provides a margin for variation and the final landing. +If your safehome is not visible from your current location, use extra caution. A visual check of the safehome is recommend prior to flying. If the safehome is in use, you can use the OSD menu to disable safehome usage prior to your flight. + ## OSD Message when Armed When the aircraft is armed, the OSD briefly shows `ARMED` and the current GPS position and current date and time. @@ -36,8 +42,14 @@ If a safehome is selected, an additional message appears: CURRENT DATE CURRENT TIME ``` -The GPS details are those of the selected safehome. -To draw your attention to "HOME" being replaced, the message flashes and stays visible longer. +The GPS details are those of the arming location, not the safehome. +To draw your attention to a safehome being selected, the message flashes and stays visible longer. + +If a safehome was found, but ``safehome_usage_mode``` is ```OFF```, the message ```SAFEHOME FOUND; MODE OFF``` will appear. + +## OSD Message during RTH + +If RTH is in progress to a safehome, the message "DIVERTING TO SAFEHOME" will be displayed. ## CLI command `safehome` to manage safehomes diff --git a/docs/Settings.md b/docs/Settings.md index b67551d1a3..2c461007a9 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1,536 +1,546 @@ # CLI Variable Reference -| Variable Name | Default Value | Description | -| ------------- | ------------- | ----------- | -| 3d_deadband_high | 1514 | High value of throttle deadband for 3D mode (when stick is in the deadband range, the value in 3d_neutral is used instead) | -| 3d_deadband_low | 1406 | Low value of throttle deadband for 3D mode (when stick is in the 3d_deadband_throttle range, the fixed values of 3d_deadband_low / _high are used instead) | -| 3d_deadband_throttle | 50 | Throttle signal will be held to a fixed value when throttle is centered with an error margin defined in this parameter. | -| 3d_neutral | 1460 | Neutral (stop) throttle value for 3D mode | -| acc_event_threshold_high | 0 | Acceleration threshold [cm/s/s] for impact / high g event text messages sent by SIM module. Acceleration values greater than 4 g can occur in fixed wing flight without an impact, so a setting of 4000 or greater is suggested. 0 = detection off. | -| acc_event_threshold_low | 0 | Acceleration threshold [cm/s/s] for low-g / freefall detection text messages sent by SIM module. A setting of less than 100 is suggested. Valid values: [0-900], 0 = detection off. | -| acc_event_threshold_neg_x | 0 | Acceleration threshold [cm/s/s] for backwards acceleration / fixed wing landing detection text messages sent by SIM module. Suggested value for fixed wing: 1100. 0 = detection off. | -| acc_hardware | AUTO | Selection of acc hardware. See Wiki Sensor auto detect and hardware failure detection for more info | -| acc_lpf_hz | 15 | Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | -| acc_lpf_type | BIQUAD | Specifies the type of the software LPF of the acc signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | -| acc_notch_cutoff | 1 | | -| acc_notch_hz | 0 | | -| accgain_x | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | -| accgain_y | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | -| accgain_z | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | -| acczero_x | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. | -| acczero_y | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. | -| acczero_z | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. | -| airmode_throttle_threshold | 1300 | Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used | -| airmode_type | STICK_CENTER | Defines the Airmode state handling type. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `airmode_throttle_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER_ONCE** or **STICK_CENTER** modes. | -| airspeed_adc_channel | _target default_ | ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't have a dedicated connector for analog airspeed sensor will default to 0 | -| align_acc | DEFAULT | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | -| align_board_pitch | 0 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | -| align_board_roll | 0 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | -| align_board_yaw | 0 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | -| align_gyro | DEFAULT | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | -| align_mag | DEFAULT | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | -| align_mag_pitch | 0 | Same as align_mag_roll, but for the pitch axis. | -| align_mag_roll | 0 | Set the external mag alignment on the roll axis (in 0.1 degree steps). If this value is non-zero, the compass is assumed to be externally mounted and both the board and on-board compass alignent (align_mag) are ignored. See also align_mag_pitch and align_mag_yaw. | -| align_mag_yaw | 0 | Same as align_mag_roll, but for the yaw axis. | -| align_opflow | CW0FLIP | Optical flow module alignment (default CW0_DEG_FLIP) | -| alt_hold_deadband | 50 | Defines the deadband of throttle during alt_hold [r/c points] | -| antigravity_accelerator | 1 | | -| antigravity_cutoff_lpf_hz | 15 | Antigravity cutoff frequenct for Throtte filter. Antigravity is based on the difference between actual and filtered throttle input. The bigger is the difference, the bigger Antigravity gain | -| antigravity_gain | 1 | Max Antigravity gain. `1` means Antigravity is disabled, `2` means Iterm is allowed to double during rapid throttle movements | -| applied_defaults | 0 | Internal (configurator) hint. Should not be changed manually | -| baro_cal_tolerance | 150 | Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. | -| baro_hardware | AUTO | Selection of baro hardware. See Wiki Sensor auto detect and hardware failure detection for more info | -| baro_median_filter | ON | 3-point median filtering for barometer readouts. No reason to change this setting | -| bat_cells | 0 | Number of cells of the battery (0 = auto-detect), see battery documentation. 7S, 9S and 11S batteries cannot be auto-detected. | -| bat_voltage_src | RAW | Chose between raw and sag compensated battery voltage to use for battery alarms and telemetry. Possible values are `RAW` and `SAG_COMP` | -| battery_capacity | 0 | Set the battery capacity in mAh or mWh (see `battery_capacity_unit`). Used to calculate the remaining battery capacity. | -| battery_capacity_critical | 0 | If the remaining battery capacity goes below this threshold the battery is considered empty and the beeper will emit long beeps. | -| battery_capacity_unit | MAH | Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour). | -| battery_capacity_warning | 0 | If the remaining battery capacity goes below this threshold the beeper will emit short beeps and the relevant OSD items will blink. | -| blackbox_device | _target default_ | Selection of where to write blackbox data | -| blackbox_rate_denom | 1 | Blackbox logging rate denominator. See blackbox_rate_num. | -| blackbox_rate_num | 1 | Blackbox logging rate numerator. Use num/denom settings to decide if a frame should be logged, allowing control of the portion of logged loop iterations | -| cpu_underclock | OFF | This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz | -| cruise_power | 0 | Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit | -| current_adc_channel | _target default_ | ADC channel to use for analog current sensor input. Defaults to board CURRENT sensor input (if available). 0 = disabled | -| current_meter_offset | _target default_ | This sets the output offset voltage of the current sensor in millivolts. | -| current_meter_scale | _target default_ | This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt. | -| current_meter_type | ADC | ADC , VIRTUAL, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position. | -| d_boost_factor | 1.25 | | -| d_boost_gyro_delta_lpf_hz | 80 | | -| d_boost_max_at_acceleration | 7500 | | -| deadband | 5 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. | -| debug_mode | NONE | Defines debug values exposed in debug variables (developer / debugging setting) | -| disarm_kill_switch | ON | Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. | -| display_force_sw_blink | OFF | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON | -| dji_esc_temp_source | ESC | Re-purpose the ESC temperature field for IMU/BARO temperature | -| dji_speed_source | GROUND | Sets the speed type displayed by the DJI OSD: GROUND, 3D, AIR | -| dji_use_name_for_messages | ON | Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance | -| dji_workarounds | 1 | Enables workarounds for different versions of MSP protocol used | -| dterm_lpf2_hz | 0 | Cutoff frequency for stage 2 D-term low pass filter | -| dterm_lpf2_type | BIQUAD | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | -| dterm_lpf_hz | 40 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value | -| dterm_lpf_type | BIQUAD | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | -| dynamic_gyro_notch_enabled | OFF | Enable/disable dynamic gyro notch also known as Matrix Filter | -| dynamic_gyro_notch_min_hz | 150 | Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` | -| dynamic_gyro_notch_q | 120 | Q factor for dynamic notches | -| dynamic_gyro_notch_range | MEDIUM | Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers | -| eleres_freq | 435 | | -| eleres_loc_delay | 240 | | -| eleres_loc_en | OFF | | -| eleres_loc_power | 7 | | -| eleres_signature | 0 | | -| eleres_telemetry_en | OFF | | -| eleres_telemetry_power | 7 | | -| esc_sensor_listen_only | OFF | Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case | -| failsafe_delay | 5 | Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). | -| failsafe_fw_pitch_angle | 100 | Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb | -| failsafe_fw_roll_angle | -200 | Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll | -| failsafe_fw_yaw_rate | -45 | Requested yaw rate to execute when `SET-THR` failsafe is active on a fixed-wing machine. In deg/s. Negative values = left turn | -| failsafe_lights | ON | Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF]. | -| failsafe_lights_flash_on_time | 100 | Flash lights ON time in milliseconds when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled. [20-65535]. | -| failsafe_lights_flash_period | 1000 | Time in milliseconds between two flashes when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled [40-65535]. | -| failsafe_min_distance | 0 | If failsafe happens when craft is closer than this distance in centimeters from home, failsafe will not execute regular failsafe_procedure, but will execute procedure specified in failsafe_min_distance_procedure instead. 0 = Normal failsafe_procedure always taken. | -| failsafe_min_distance_procedure | DROP | What failsafe procedure to initiate in Stage 2 when craft is closer to home than failsafe_min_distance. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | -| failsafe_mission | ON | If set to `OFF` the failsafe procedure won't be triggered and the mission will continue if the FC is in WP (automatic mission) mode | -| failsafe_off_delay | 200 | Time in deciseconds to wait before turning off motors when failsafe is activated. 0 = No timeout. See [Failsafe documentation](Failsafe.md#failsafe_off_delay). | -| failsafe_procedure | SET-THR | What failsafe procedure to initiate in Stage 2. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | -| failsafe_recovery_delay | 5 | Time in deciseconds to wait before aborting failsafe when signal is recovered. See [Failsafe documentation](Failsafe.md#failsafe_recovery_delay). | -| failsafe_stick_threshold | 50 | Threshold for stick motion to consider failsafe condition resolved. If non-zero failsafe won't clear even if RC link is restored - you have to move sticks to exit failsafe. | -| failsafe_throttle | 1000 | Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | -| failsafe_throttle_low_delay | 0 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout | -| fixed_wing_auto_arm | OFF | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. | -| flaperon_throw_offset | 200 | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. | -| flip_over_after_crash_power_factor | 65 | flip over after crash power factor | -| fpv_mix_degrees | 0 | | -| frsky_coordinates_format | 0 | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA | -| frsky_default_latitude | 0 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. | -| frsky_default_longitude | 0 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. | -| frsky_pitch_roll | OFF | S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data | -| frsky_unit | METRIC | Not used? [METRIC/IMPERIAL] | -| frsky_vfas_precision | 0 | D-Series telemetry only: Set to 1 to send raw VBat value in 0.1V resolution for receivers that can handle it, or 0 (default) to use the standard method | -| fw_autotune_ff_to_i_tc | 600 | FF to I time (defines time for I to reach the same level of response as FF) [ms] | -| fw_autotune_ff_to_p_gain | 10 | FF to P gain (strength relationship) [%] | -| fw_autotune_overshoot_time | 100 | Time [ms] to detect sustained overshoot | -| fw_autotune_threshold | 50 | Threshold [%] of max rate to consider overshoot/undershoot detection | -| fw_autotune_undershoot_time | 200 | Time [ms] to detect sustained undershoot | -| fw_d_level | 75 | Fixed-wing attitude stabilisation HORIZON transition point | -| fw_d_pitch | 0 | Fixed wing rate stabilisation D-gain for PITCH | -| fw_d_roll | 0 | Fixed wing rate stabilisation D-gain for ROLL | -| fw_d_yaw | 0 | Fixed wing rate stabilisation D-gain for YAW | -| fw_ff_pitch | 50 | Fixed-wing rate stabilisation FF-gain for PITCH | -| fw_ff_roll | 50 | Fixed-wing rate stabilisation FF-gain for ROLL | -| fw_ff_yaw | 60 | Fixed-wing rate stabilisation FF-gain for YAW | -| fw_i_level | 5 | Fixed-wing attitude stabilisation low-pass filter cutoff | -| fw_i_pitch | 7 | Fixed-wing rate stabilisation I-gain for PITCH | -| fw_i_roll | 7 | Fixed-wing rate stabilisation I-gain for ROLL | -| fw_i_yaw | 10 | Fixed-wing rate stabilisation I-gain for YAW | -| fw_iterm_limit_stick_position | 0.5 | Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side | -| fw_iterm_throw_limit | 165 | Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. | -| fw_level_pitch_trim | 0 | Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level | -| fw_loiter_direction | RIGHT | Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick. | -| fw_min_throttle_down_pitch | 0 | Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees) | -| fw_p_level | 20 | Fixed-wing attitude stabilisation P-gain | -| fw_p_pitch | 5 | Fixed-wing rate stabilisation P-gain for PITCH | -| fw_p_roll | 5 | Fixed-wing rate stabilisation P-gain for ROLL | -| fw_p_yaw | 6 | Fixed-wing rate stabilisation P-gain for YAW | -| fw_reference_airspeed | 1000 | Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present. | -| fw_tpa_time_constant | 0 | TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. Planes with low thrust/weight ratio generally need higher time constant. Default is zero for compatibility with old setups | -| fw_turn_assist_pitch_gain | 1 | Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | -| fw_turn_assist_yaw_gain | 1 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | -| fw_yaw_iterm_freeze_bank_angle | 0 | Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled. | -| gps_auto_baud | ON | Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS. When used with NAZA/DJI it will automatic detect GPS baudrate and change to it, ignoring the selected baudrate set in ports | -| gps_auto_config | ON | Enable automatic configuration of UBlox GPS receivers. | -| gps_dyn_model | AIR_1G | GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying. | -| gps_min_sats | 6 | Minimum number of GPS satellites in view to acquire GPS_FIX and consider GPS position valid. Some GPS receivers appeared to be very inaccurate with low satellite count. | -| gps_provider | UBLOX | Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N). | -| gps_sbas_mode | NONE | Which SBAS mode to be used | -| gps_ublox_use_galileo | OFF | Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. | -| gyro_dyn_lpf_curve_expo | 5 | Expo value for the throttle-to-frequency mapping for Dynamic LPF | -| gyro_dyn_lpf_max_hz | 500 | Maximum frequency of the gyro Dynamic LPF | -| gyro_dyn_lpf_min_hz | 200 | Minimum frequency of the gyro Dynamic LPF | -| gyro_hardware_lpf | 256HZ | Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first. | -| gyro_lpf_hz | 60 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | -| gyro_lpf_type | BIQUAD | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | -| gyro_notch_cutoff | 1 | | -| gyro_notch_hz | 0 | | -| gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz) | -| gyro_stage2_lowpass_type | BIQUAD | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | -| gyro_sync | ON | This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf | -| gyro_to_use | 0 | | -| gyro_use_dyn_lpf | OFF | Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. | -| has_flaps | OFF | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot | -| heading_hold_rate_limit | 90 | This setting limits yaw rotation rate that HEADING_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when HEADING_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. | -| hott_alarm_sound_interval | 5 | Battery alarm delay in seconds for Hott telemetry | -| i2c_speed | 400KHZ | This setting controls the clock speed of I2C bus. 400KHZ is the default that most setups are able to use. Some noise-free setups may be overclocked to 800KHZ. Some sensor chips or setups with long wires may work unreliably at 400KHZ - user can try lowering the clock speed to 200KHZ or even 100KHZ. User need to bear in mind that lower clock speeds might require higher looptimes (lower looptime rate) | -| ibus_telemetry_type | 0 | Type compatibility ibus telemetry for transmitters. See Telemetry.md label IBUS for details. | -| idle_power | 0 | Power draw at zero throttle used for remaining flight time/distance estimation in 0.01W unit | -| imu2_align_pitch | 0 | Pitch alignment for Secondary IMU. 1/10 of a degree | -| imu2_align_roll | 0 | Roll alignment for Secondary IMU. 1/10 of a degree | -| imu2_align_yaw | 0 | Yaw alignment for Secondary IMU. 1/10 of a degree | -| imu2_gain_acc_x | 0 | Secondary IMU ACC calibration data | -| imu2_gain_acc_y | 0 | Secondary IMU ACC calibration data | -| imu2_gain_acc_z | 0 | Secondary IMU ACC calibration data | -| imu2_gain_mag_x | 0 | Secondary IMU MAG calibration data | -| imu2_gain_mag_y | 0 | Secondary IMU MAG calibration data | -| imu2_gain_mag_z | 0 | Secondary IMU MAG calibration data | -| imu2_hardware | NONE | Selection of a Secondary IMU hardware type. NONE disables this functionality | -| imu2_radius_acc | 0 | Secondary IMU MAG calibration data | -| imu2_radius_mag | 0 | Secondary IMU MAG calibration data | -| imu2_use_for_osd_ahi | OFF | If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon | -| imu2_use_for_osd_heading | OFF | If set to ON, Secondary IMU data will be used for Analog OSD heading | -| imu2_use_for_stabilized | OFF | If set to ON, Secondary IMU data will be used for Angle, Horizon and all other modes that control attitude (PosHold, WP, RTH) | -| imu_acc_ignore_rate | 0 | Total gyro rotation rate threshold [deg/s] to consider accelerometer trustworthy on airplanes | -| imu_acc_ignore_slope | 0 | Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight) | -| imu_dcm_ki | 50 | Inertial Measurement Unit KI Gain for accelerometer measurements | -| imu_dcm_ki_mag | 0 | Inertial Measurement Unit KI Gain for compass measurements | -| imu_dcm_kp | 2500 | Inertial Measurement Unit KP Gain for accelerometer measurements | -| imu_dcm_kp_mag | 10000 | Inertial Measurement Unit KP Gain for compass measurements | -| inav_allow_dead_reckoning | OFF | Defines if inav will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation | -| inav_auto_mag_decl | ON | Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored. | -| inav_baro_epv | 100 | Uncertainty value for barometric sensor [cm] | -| inav_gravity_cal_tolerance | 5 | Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. | -| inav_max_eph_epv | 1000 | Maximum uncertainty value until estimated position is considered valid and is used for navigation [cm] | -| inav_max_surface_altitude | 200 | Max allowed altitude for surface following mode. [cm] | -| inav_reset_altitude | FIRST_ARM | Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming) | -| inav_reset_home | FIRST_ARM | Allows to chose when the home position is reset. Can help prevent resetting home position after accidental mid-air disarm. Possible values are: NEVER, FIRST_ARM and EACH_ARM | -| inav_use_gps_no_baro | OFF | | -| inav_use_gps_velned | ON | Defined if iNav should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF iNav will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance. | -| inav_w_acc_bias | 0.01 | Weight for accelerometer drift estimation | -| inav_w_xy_flow_p | 1.0 | | -| inav_w_xy_flow_v | 2.0 | | -| inav_w_xy_gps_p | 1.0 | Weight of GPS coordinates in estimated UAV position and speed. | -| inav_w_xy_gps_v | 2.0 | Weight of GPS velocity data in estimated UAV speed | -| inav_w_xy_res_v | 0.5 | Decay coefficient for estimated velocity when GPS reference for position is lost | -| inav_w_xyz_acc_p | 1.0 | | -| inav_w_z_baro_p | 0.35 | Weight of barometer measurements in estimated altitude and climb rate | -| inav_w_z_gps_p | 0.2 | Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes | -| inav_w_z_gps_v | 0.1 | Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors. If GPS doesn't support native climb rate reporting (i.e. NMEA GPS) you may consider setting this to zero | -| inav_w_z_res_v | 0.5 | Decay coefficient for estimated climb rate when baro/GPS reference for altitude is lost | -| inav_w_z_surface_p | 3.5 | | -| inav_w_z_surface_v | 6.1 | | -| iterm_windup | 50 | Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) | -| ledstrip_visual_beeper | OFF | | -| log_level | ERROR | Defines serial debugging log level. See `docs/development/serial_printf_debugging.md` for usage. | -| log_topics | 0 | Defines serial debugging log topic. See `docs/development/serial_printf_debugging.md` for usage. | -| looptime | 1000 | This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible. | -| ltm_update_rate | NORMAL | Defines the LTM update rate (use of bandwidth [NORMAL/MEDIUM/SLOW]). See Telemetry.md, LTM section for details. | -| mag_calibration_time | 30 | Adjust how long time the Calibration of mag will last. | -| mag_declination | 0 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you acquire valid GPS fix. | -| mag_hardware | AUTO | Selection of mag hardware. See Wiki Sensor auto detect and hardware failure detection for more info | -| mag_to_use | 0 | Allow to chose between built-in and external compass sensor if they are connected to separate buses. Currently only for REVO target | -| maggain_x | 1024 | Magnetometer calibration X gain. If 1024, no calibration or calibration failed | -| maggain_y | 1024 | Magnetometer calibration Y gain. If 1024, no calibration or calibration failed | -| maggain_z | 1024 | Magnetometer calibration Z gain. If 1024, no calibration or calibration failed | -| magzero_x | 0 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. | -| magzero_y | 0 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. | -| magzero_z | 0 | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. | -| manual_pitch_rate | 100 | Servo travel multiplier for the PITCH axis in `MANUAL` flight mode [0-100]% | -| manual_rc_expo | 70 | Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100] | -| manual_rc_yaw_expo | 20 | Exposition value used for the YAW axis by the `MANUAL` flight mode [0-100] | -| manual_roll_rate | 100 | Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]% | -| manual_yaw_rate | 100 | Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]% | -| mavlink_ext_status_rate | 2 | | -| mavlink_extra1_rate | 10 | | -| mavlink_extra2_rate | 2 | | -| mavlink_extra3_rate | 1 | | -| mavlink_pos_rate | 2 | | -| mavlink_rc_chan_rate | 5 | | -| max_angle_inclination_pit | 300 | Maximum inclination in level (angle) mode (PITCH axis). 100=10° | -| max_angle_inclination_rll | 300 | Maximum inclination in level (angle) mode (ROLL axis). 100=10° | -| max_check | 1900 | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | -| max_throttle | 1850 | This is the maximum value (in us) sent to esc when armed. Default of 1850 are OK for everyone (legacy). For modern ESCs, higher values (c. 2000) may be more appropriate. If you have brushed motors, the value should be set to 2000. | -| mc_cd_lpf_hz | 30 | Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky | -| mc_cd_pitch | 60 | Multicopter Control Derivative gain for PITCH | -| mc_cd_roll | 60 | Multicopter Control Derivative gain for ROLL | -| mc_cd_yaw | 60 | Multicopter Control Derivative gain for YAW | -| mc_d_level | 75 | Multicopter attitude stabilisation HORIZON transition point | -| mc_d_pitch | 23 | Multicopter rate stabilisation D-gain for PITCH | -| mc_d_roll | 23 | Multicopter rate stabilisation D-gain for ROLL | -| mc_d_yaw | 0 | Multicopter rate stabilisation D-gain for YAW | -| mc_i_level | 15 | Multicopter attitude stabilisation low-pass filter cutoff | -| mc_i_pitch | 30 | Multicopter rate stabilisation I-gain for PITCH | -| mc_i_roll | 30 | Multicopter rate stabilisation I-gain for ROLL | -| mc_i_yaw | 45 | Multicopter rate stabilisation I-gain for YAW | -| mc_iterm_relax | RP | | -| mc_iterm_relax_cutoff | 15 | | -| mc_p_level | 20 | Multicopter attitude stabilisation P-gain | -| mc_p_pitch | 40 | Multicopter rate stabilisation P-gain for PITCH | -| mc_p_roll | 40 | Multicopter rate stabilisation P-gain for ROLL | -| mc_p_yaw | 85 | Multicopter rate stabilisation P-gain for YAW | -| min_check | 1100 | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | -| min_command | 1000 | This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly when powered up, try decreasing this value. It can also be used for calibrating all ESCs at once. | -| mode_range_logic_operator | OR | Control how Mode selection works in flight modes. If you example have Angle mode configured on two different Aux channels, this controls if you need both activated ( AND ) or if you only need one activated ( OR ) to active angle mode. | -| model_preview_type | -1 | ID of mixer preset applied in a Configurator. **Do not modify manually**. Used only for backup/restore reasons. | -| moron_threshold | 32 | When powering up, gyro bias is calculated. If the model is shaking/moving during this initial calibration, offsets are calculated incorrectly, and could lead to poor flying performance. This threshold means how much average gyro reading could differ before re-calibration is triggered. | -| motor_accel_time | 0 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] | -| motor_decel_time | 0 | Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] | -| motor_direction_inverted | OFF | Use if you need to inverse yaw motor direction. | -| motor_poles | 14 | The number of motor poles. Required to compute motor RPM | -| motor_pwm_protocol | ONESHOT125 | Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, DSHOT150, DSHOT300, DSHOT600, DSHOT1200, BRUSHED | -| motor_pwm_rate | 400 | Output frequency (in Hz) for motor pins. Default is 400Hz for motor with motor_pwm_protocol set to STANDARD. For *SHOT (e.g. ONESHOT125) values of 1000 and 2000 have been tested by the development team and are supported. It may be possible to use higher values. For BRUSHED values of 8000 and above should be used. Setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported for brushed. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set max_throttle to 2000. | -| msp_override_channels | 0 | Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode. | -| name | _empty_ | Craft name | -| nav_auto_climb_rate | 500 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] | -| nav_auto_speed | 300 | Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only] | -| nav_disarm_on_landing | OFF | If set to ON, iNav disarms the FC after landing | -| nav_emerg_landing_speed | 500 | Rate of descent UAV will try to maintain when doing emergency descent sequence [cm/s] | -| nav_extra_arming_safety | ON | If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured. ALLOW_BYPASS allows the user to momentarily disable this check by holding yaw high (left stick held at the bottom right in mode 2) when switch arming is used | -| nav_fw_allow_manual_thr_increase | OFF | Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing | -| nav_fw_bank_angle | 35 | Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll | -| nav_fw_climb_angle | 20 | Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit | -| nav_fw_control_smoothness | 0 | How smoothly the autopilot controls the airplane to correct the navigation error | -| nav_fw_cruise_speed | 0 | Speed for the plane/wing at cruise throttle used for remaining flight time/distance estimation in cm/s | -| nav_fw_cruise_thr | 1400 | Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded ) | -| nav_fw_cruise_yaw_rate | 20 | Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps] | -| nav_fw_dive_angle | 15 | Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit | -| nav_fw_heading_p | 60 | P gain of Heading Hold controller (Fixedwing) | -| nav_fw_land_dive_angle | 2 | Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees | -| nav_fw_launch_accel | 1863 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s | -| nav_fw_launch_climb_angle | 18 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit | -| nav_fw_launch_detect_time | 40 | Time for which thresholds have to breached to consider launch happened [ms] | -| nav_fw_launch_end_time | 3000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] | -| nav_fw_launch_idle_thr | 1000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position) | -| nav_fw_launch_max_altitude | 0 | Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. | -| nav_fw_launch_max_angle | 45 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] | -| nav_fw_launch_min_time | 0 | Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]. | -| nav_fw_launch_motor_delay | 500 | Delay between detected launch and launch sequence start and throttling up (ms) | -| nav_fw_launch_spinup_time | 100 | Time to bring power from minimum throttle to nav_fw_launch_thr - to avoid big stress on ESC and large torque from propeller | -| nav_fw_launch_thr | 1700 | Launch throttle - throttle to be set during launch sequence (pwm units) | -| nav_fw_launch_timeout | 5000 | Maximum time for launch sequence to be executed. After this time LAUNCH mode will be turned off and regular flight mode will take over (ms) | -| nav_fw_launch_velocity | 300 | Forward velocity threshold for swing-launch detection [cm/s] | -| nav_fw_loiter_radius | 7500 | PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] | -| nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes | -| nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes | -| nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | -| nav_fw_pitch2thr_smoothing | 6 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | -| nav_fw_pitch2thr_threshold | 50 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | -| nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | -| nav_fw_pos_hdg_i | 2 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | -| nav_fw_pos_hdg_p | 30 | P gain of heading PID controller. (Fixedwing, rovers, boats) | -| nav_fw_pos_hdg_pidsum_limit | 350 | Output limit for heading trajectory PID controller. (Fixedwing, rovers, boats) | -| nav_fw_pos_xy_d | 8 | D gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero | -| nav_fw_pos_xy_i | 5 | I gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero | -| nav_fw_pos_xy_p | 75 | P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH | -| nav_fw_pos_z_d | 10 | D gain of altitude PID controller (Fixedwing) | -| nav_fw_pos_z_i | 5 | I gain of altitude PID controller (Fixedwing) | -| nav_fw_pos_z_p | 40 | P gain of altitude PID controller (Fixedwing) | -| nav_fw_yaw_deadband | 0 | Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course | -| nav_land_slowdown_maxalt | 2000 | Defines at what altitude the descent velocity should start to ramp down from 100% nav_landing_speed to 25% nav_landing_speed. [cm] | -| nav_land_slowdown_minalt | 500 | Defines at what altitude the descent velocity should start to be 25% of nav_landing_speed [cm] | -| nav_landing_speed | 200 | Vertical descent velocity during the RTH landing phase. [cm/s] | -| nav_manual_climb_rate | 200 | Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] | -| nav_manual_speed | 500 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] | -| nav_max_terrain_follow_alt | 100 | Max allowed above the ground altitude for terrain following mode | -| nav_mc_auto_disarm_delay | 2000 | Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s) | -| nav_mc_bank_angle | 30 | Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude | -| nav_mc_braking_bank_angle | 40 | max angle that MR is allowed to bank in BOOST mode | -| nav_mc_braking_boost_disengage_speed | 100 | BOOST will be disabled when speed goes below this value | -| nav_mc_braking_boost_factor | 100 | acceleration factor for BOOST phase | -| nav_mc_braking_boost_speed_threshold | 150 | BOOST can be enabled when speed is above this value | -| nav_mc_braking_boost_timeout | 750 | how long in ms BOOST phase can happen | -| nav_mc_braking_disengage_speed | 75 | braking is disabled when speed goes below this value | -| nav_mc_braking_speed_threshold | 100 | min speed in cm/s above which braking can happen | -| nav_mc_braking_timeout | 2000 | timeout in ms for braking | -| nav_mc_heading_p | 60 | P gain of Heading Hold controller (Multirotor) | -| nav_mc_hover_thr | 1500 | Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering. | -| nav_mc_pos_deceleration_time | 120 | Used for stoping distance calculation. Stop position is computed as _speed_ * _nav_mc_pos_deceleration_time_ from the place where sticks are released. Braking mode overrides this setting | -| nav_mc_pos_expo | 10 | Expo for PosHold control | -| nav_mc_pos_xy_p | 65 | Controls how fast the drone will fly towards the target position. This is a multiplier to convert displacement to target velocity | -| nav_mc_pos_z_p | 50 | P gain of altitude PID controller (Multirotor) | -| nav_mc_vel_xy_d | 100 | D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target. | -| nav_mc_vel_xy_dterm_attenuation | 90 | Maximum D-term attenution percentage for horizontal velocity PID controller (Multirotor). It allows to smooth the PosHold CRUISE, WP and RTH when Multirotor is traveling at full speed. Dterm is not attenuated at low speeds, breaking and accelerating. | -| nav_mc_vel_xy_dterm_attenuation_end | 60 | A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum | -| nav_mc_vel_xy_dterm_attenuation_start | 10 | A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins | -| nav_mc_vel_xy_dterm_lpf_hz | 2 | | -| nav_mc_vel_xy_ff | 40 | | -| nav_mc_vel_xy_i | 15 | I gain of Position-Rate (Velocity to Acceleration) PID controller. Used for drift compensation (caused by wind for example). Higher I means stronger response to drift. Too much I gain might cause target overshot | -| nav_mc_vel_xy_p | 40 | P gain of Position-Rate (Velocity to Acceleration) PID controller. Higher P means stronger response when position error occurs. Too much P might cause "nervous" behavior and oscillations | -| nav_mc_vel_z_d | 10 | D gain of velocity PID controller | -| nav_mc_vel_z_i | 50 | I gain of velocity PID controller | -| nav_mc_vel_z_p | 100 | P gain of velocity PID controller | -| nav_mc_wp_slowdown | ON | When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes. | -| nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. | -| nav_overrides_motor_stop | ALL_NAV | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | -| nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | -| nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] | -| nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. | -| nav_rth_alt_mode | AT_LEAST | Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details | -| nav_rth_altitude | 1000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) | -| nav_rth_climb_first | ON | If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way. | -| nav_rth_climb_ignore_emerg | OFF | If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. | -| nav_rth_home_altitude | 0 | Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm] | -| nav_rth_tail_first | OFF | If set to ON drone will return tail-first. Obviously meaningless for airplanes. | -| nav_use_fw_yaw_control | OFF | Enables or Disables the use of the heading PID controller on fixed wing. Heading PID controller is always enabled for rovers and boats | -| nav_use_midthr_for_althold | OFF | If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude | -| nav_user_control_mode | ATTI | Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - pitch/roll controls attitude like in ANGLE mode; CRUISE - pitch/roll controls velocity in forward and right direction. | -| nav_wp_radius | 100 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius | -| nav_wp_safe_distance | 10000 | First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check. | -| opflow_hardware | NONE | Selection of OPFLOW hardware. | -| opflow_scale | 10.5 | | -| osd_ahi_bordered | OFF | Shows a border/corners around the AHI region (pixel OSD only) | -| osd_ahi_height | 162 | AHI height in pixels (pixel OSD only) | -| osd_ahi_max_pitch | 20 | Max pitch, in degrees, for OSD artificial horizon | -| osd_ahi_reverse_roll | OFF | | -| osd_ahi_style | DEFAULT | Sets OSD Artificial Horizon style "DEFAULT" or "LINE" for the FrSky Graphical OSD. | -| osd_ahi_vertical_offset | -18 | AHI vertical offset from center (pixel OSD only) | -| osd_ahi_width | 132 | AHI width in pixels (pixel OSD only) | -| osd_alt_alarm | 100 | Value above which to make the OSD relative altitude indicator blink (meters) | -| osd_baro_temp_alarm_max | 600 | Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade) | -| osd_baro_temp_alarm_min | -200 | Temperature under which the baro temperature OSD element will start blinking (decidegrees centigrade) | -| osd_camera_fov_h | 135 | Horizontal field of view for the camera in degres | -| osd_camera_fov_v | 85 | Vertical field of view for the camera in degres | -| osd_camera_uptilt | 0 | Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal | -| osd_coordinate_digits | 9 | | -| osd_crosshairs_style | DEFAULT | To set the visual type for the crosshair | -| osd_crsf_lq_format | TYPE1 | To select LQ format | -| osd_current_alarm | 0 | Value above which the OSD current consumption element will start blinking. Measured in full Amperes. | -| osd_dist_alarm | 1000 | Value above which to make the OSD distance from home indicator blink (meters) | -| osd_esc_temp_alarm_max | 900 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | -| osd_esc_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) | -| osd_estimations_wind_compensation | ON | Use wind estimation for remaining flight time/distance estimation | -| osd_failsafe_switch_layout | OFF | If enabled the OSD automatically switches to the first layout during failsafe | -| osd_force_grid | OFF | Force OSD to work in grid mode even if the OSD device supports pixel level access (mainly used for development) | -| osd_gforce_alarm | 5 | Value above which the OSD g force indicator will blink (g) | -| osd_gforce_axis_alarm_max | 5 | Value above which the OSD axis g force indicators will blink (g) | -| osd_gforce_axis_alarm_min | -5 | Value under which the OSD axis g force indicators will blink (g) | -| osd_home_position_arm_screen | ON | Should home position coordinates be displayed on the arming screen. | -| osd_horizon_offset | 0 | To vertically adjust the whole OSD and AHI and scrolling bars | -| osd_hud_homepoint | OFF | To 3D-display the home point location in the hud | -| osd_hud_homing | OFF | To display little arrows around the crossair showing where the home point is in the hud | -| osd_hud_margin_h | 3 | Left and right margins for the hud area | -| osd_hud_margin_v | 3 | Top and bottom margins for the hud area | -| osd_hud_radar_disp | 0 | Maximum count of nearby aircrafts or points of interest to display in the hud, as sent from an ESP32 LoRa module. Set to 0 to disable (show nothing). The nearby aircrafts will appear as markers A, B, C, etc | -| osd_hud_radar_nearest | 0 | To display an extra bar of informations at the bottom of the hud area for the closest radar aircraft found, if closest than the set value, in meters. Shows relative altitude (meters or feet, with an up or down arrow to indicate if above or below), speed (in m/s or f/s), and absolute heading (in °, 0 is north, 90 is east, 180 is south, 270 is west). Set to 0 (zero) to disable. | -| osd_hud_radar_range_max | 4000 | In meters, radar aircrafts further away than this will not be displayed in the hud | -| osd_hud_radar_range_min | 3 | In meters, radar aircrafts closer than this will not be displayed in the hud | -| osd_hud_wp_disp | 0 | How many navigation waypoints are displayed, set to 0 (zero) to disable. As sample, if set to 2, and you just passed the 3rd waypoint of the mission, you'll see markers for the 4th waypoint (marked 1) and the 5th waypoint (marked 2) | -| osd_imu_temp_alarm_max | 600 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | -| osd_imu_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) | -| osd_left_sidebar_scroll | NONE | | -| osd_left_sidebar_scroll_step | 0 | How many units each sidebar step represents. 0 means the default value for the scroll type. | -| osd_link_quality_alarm | 70 | LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50% | -| osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. | -| osd_neg_alt_alarm | 5 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) | -| osd_pan_servo_index | 0 | Index of the pan servo to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos. | -| osd_pan_servo_pwm2centideg | 0 | Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. | -| osd_plus_code_digits | 11 | Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm. | -| osd_plus_code_short | 0 | Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requires a reference location within, respectively, ~800km, ~40 km and ~2km to recover the original coordinates. | -| osd_right_sidebar_scroll | NONE | | -| osd_right_sidebar_scroll_step | 0 | Same as left_sidebar_scroll_step, but for the right sidebar | -| osd_row_shiftdown | 0 | Number of rows to shift the OSD display (increase if top rows are cut off) | -| osd_rssi_alarm | 20 | Value below which to make the OSD RSSI indicator blink | -| osd_sidebar_horizontal_offset | 0 | Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. | -| osd_sidebar_scroll_arrows | OFF | | -| osd_snr_alarm | 4 | Value below which Crossfire SNR Alarm pops-up. (dB) | -| osd_stats_energy_unit | MAH | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) | -| osd_temp_label_align | LEFT | Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` | -| osd_time_alarm | 10 | Value above which to make the OSD flight time indicator blink (minutes) | -| osd_units | METRIC | IMPERIAL, METRIC, UK | -| osd_video_system | AUTO | Video system used. Possible values are `AUTO`, `PAL` and `NTSC` | -| pid_type | AUTO | Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` | -| pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | -| pidsum_limit_yaw | 350 | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | -| pinio_box1 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 | -| pinio_box2 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 | -| pinio_box3 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 | -| pinio_box4 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 | -| pitch_rate | 20 | Defines rotation rate on PITCH axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | -| pitot_hardware | NONE | Selection of pitot hardware. | -| pitot_lpf_milli_hz | 350 | | -| pitot_scale | 1.0 | | -| platform_type | MULTIROTOR | Defines UAV platform type. Allowed values: "MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented | -| pos_hold_deadband | 10 | Stick deadband in [r/c points], applied after r/c deadband and expo | -| prearm_timeout | 10000 | Duration (ms) for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout. | -| rangefinder_hardware | NONE | Selection of rangefinder hardware. | -| rangefinder_median_filter | OFF | 3-point median filtering for rangefinder readouts | -| rate_accel_limit_roll_pitch | 0 | Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting. | -| rate_accel_limit_yaw | 10000 | Limits acceleration of YAW rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 180 dps^2). When set correctly, it greatly improves stopping performance and general stability during yaw turns. Value of 0 disables limiting. | -| rc_expo | 70 | Exposition value used for the PITCH/ROLL axes by all the stabilized flights modes (all but `MANUAL`) | -| rc_filter_frequency | 50 | RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values | -| rc_yaw_expo | 20 | Exposition value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`) | -| reboot_character | 82 | Special character used to trigger reboot | -| receiver_type | _target default_ | Selection of receiver (RX) type. Additional configuration of a `serialrx_provider` and a UART will be needed for `SERIAL` | -| report_cell_voltage | OFF | S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON | -| roll_rate | 20 | Defines rotation rate on ROLL axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | -| rpm_gyro_filter_enabled | OFF | Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV | -| rpm_gyro_harmonics | 1 | Number of harmonic frequences to be covered by gyro RPM filter. Default value of `1` usually works just fine | -| rpm_gyro_min_hz | 100 | The lowest frequency for gyro RPM filtere. Default `150` is fine for 5" mini-quads. On 7-inch drones you can lower even down to `60`-`70` | -| rpm_gyro_q | 500 | Q factor for gyro RPM filter. Lower values give softer, wider attenuation. Usually there is no need to change this setting | -| rssi_adc_channel | _target default_ | ADC channel to use for analog RSSI input. Defaults to board RSSI input (if available). 0 = disabled | -| rssi_channel | 0 | RX channel containing the RSSI signal | -| rssi_max | 100 | The maximum RSSI value sent by the receiver, in %. For example, if your receiver's maximum RSSI value shows as 83% in the configurator/OSD set this parameter to 83. See also rssi_min. | -| rssi_min | 0 | The minimum RSSI value sent by the receiver, in %. For example, if your receiver's minimum RSSI value shows as 42% in the configurator/OSD set this parameter to 42. See also rssi_max. Note that rssi_min can be set to a value bigger than rssi_max to invert the RSSI calculation (i.e. bigger values mean lower RSSI). | -| rssi_source | AUTO | Source of RSSI input. Possible values: `NONE`, `AUTO`, `ADC`, `CHANNEL`, `PROTOCOL`, `MSP` | -| rth_energy_margin | 5 | Energy margin wanted after getting home (percent of battery energy capacity). Use for the remaining flight time/distance calculation | -| rx_max_usec | 2115 | Defines the longest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of mid_rc. | -| rx_min_usec | 885 | Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of mid_rc. | -| rx_spi_id | 0 | | -| rx_spi_protocol | _target default_ | | -| rx_spi_rf_channel_count | 0 | | -| safehome_max_distance | 20000 | In order for a safehome to be used, it must be less than this distance (in cm) from the arming point. | -| sbus_sync_interval | 3000 | | -| sdcard_detect_inverted | _target default_ | This setting drives the way SD card is detected in card slot. On some targets (AnyFC F7 clone) different card slot was used and depending of hardware revision ON or OFF setting might be required. If card is not detected, change this value. | -| serialrx_halfduplex | AUTO | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire. | -| serialrx_inverted | OFF | Reverse the serial inversion of the serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). | -| serialrx_provider | _target default_ | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. | -| servo_autotrim_rotation_limit | 15 | Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`. | -| servo_center_pulse | 1500 | Servo midpoint | -| servo_lpf_hz | 20 | Selects the servo PWM output cutoff frequency. Value is in [Hz] | -| servo_protocol | PWM | An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port) | -| servo_pwm_rate | 50 | Output frequency (in Hz) servo pins. When using tricopters or gimbal with digital servo, this rate can be increased. Max of 498Hz (for 500Hz pwm period), and min of 50Hz. Most digital servos will support for example 330Hz. | -| setpoint_kalman_enabled | OFF | Enable Kalman filter on the PID controller setpoint | -| setpoint_kalman_q | 100 | Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds | -| setpoint_kalman_sharpness | 100 | Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets | -| setpoint_kalman_w | 4 | Window size for the setpoint Kalman filter. Wider the window, more samples are used to compute variance. In general, wider window results in smoother filter response | -| sim_ground_station_number | _empty_ | Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module. | -| sim_low_altitude | -32767 | Threshold for low altitude warning messages sent by SIM module when the 'L' transmit flag is set in `sim_transmit_flags`. | -| sim_pin | 0000 | PIN code for the SIM module | -| sim_transmit_flags | `SIM_TX_FLAG_FAILSAFE` | Bitmask specifying text message transmit condition flags for the SIM module. 1: continuous transmission, 2: continuous transmission in failsafe mode, 4: continuous transmission when GPS signal quality is low, 8: acceleration events, 16: continuous transmission when altitude is below `sim_low_altitude` | -| sim_transmit_interval | 60 | Text message transmission interval in seconds for SIM module. Minimum value: 10 | -| small_angle | 25 | If the aircraft tilt angle exceed this value the copter will refuse to arm. | -| smartport_fuel_unit | MAH | S.Port telemetry only: Unit of the value sent with the `FUEL` ID (FrSky D-Series always sends percent). [PERCENT/MAH/MWH] | -| smartport_master_halfduplex | ON | | -| smartport_master_inverted | OFF | | -| spektrum_sat_bind | `SPEKTRUM_SAT_BIND_DISABLED` | 0 = disabled. Used to bind the spektrum satellite to RX | -| srxl2_baud_fast | ON | | -| srxl2_unit_id | 1 | | -| stats | OFF | General switch of the statistics recording feature (a.k.a. odometer) | -| stats_total_dist | 0 | Total flight distance [in meters]. The value is updated on every disarm when "stats" are enabled. | -| stats_total_energy | 0 | | -| stats_total_time | 0 | Total flight time [in seconds]. The value is updated on every disarm when "stats" are enabled. | -| switch_disarm_delay | 250 | Delay before disarming when requested by switch (ms) [0-1000] | -| telemetry_halfduplex | ON | S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details | -| telemetry_inverted | OFF | Determines if the telemetry protocol default signal inversion is reversed. This should be OFF in most cases unless a custom or hacked RX is used. | -| telemetry_switch | OFF | Which aux channel to use to change serial output & baud rate (MSP / Telemetry). It disables automatic switching to Telemetry when armed. | -| thr_comp_weight | 1 | Weight used for the throttle compensation based on battery voltage. See the [battery documentation](Battery.md#automatic-throttle-compensation-based-on-battery-voltage) | -| thr_expo | 0 | Throttle exposition value | -| thr_mid | 50 | Throttle value when the stick is set to mid-position. Used in the throttle curve calculation. | -| throttle_idle | 15 | The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle. | -| throttle_scale | 1.0 | Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50% | -| throttle_tilt_comp_str | 0 | Can be used in ANGLE and HORIZON mode and will automatically boost throttle when banking. Setting is in percentage, 0=disabled. | -| tpa_breakpoint | 1500 | See tpa_rate. | -| tpa_rate | 0 | Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. | -| tri_unarmed_servo | ON | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. | -| tz_automatic_dst | OFF | Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`. | -| tz_offset | 0 | Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs | -| vbat_adc_channel | _target default_ | ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled | -| vbat_cell_detect_voltage | 425 | Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units. | -| vbat_max_cell_voltage | 420 | Maximum voltage per cell in 0.01V units, default is 4.20V | -| vbat_meter_type | ADC | Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running | -| vbat_min_cell_voltage | 330 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) | -| vbat_scale | _target default_ | Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing "status" in cli. | -| vbat_warning_cell_voltage | 350 | Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) | -| vtx_band | 4 | Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. | -| vtx_channel | 1 | Channel to use within the configured `vtx_band`. Valid values are [1, 8]. | -| vtx_halfduplex | ON | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | -| vtx_low_power_disarm | OFF | When the craft is disarmed, set the VTX to its lowest power. `ON` will set the power to its minimum value on startup, increase it to `vtx_power` when arming and change it back to its lowest setting after disarming. `UNTIL_FIRST_ARM` will start with minimum power, but once the craft is armed it will increase to `vtx_power` and it will never decrease until the craft is power cycled. | -| vtx_max_power_override | 0 | Some VTXes may report max power incorrectly (i.e. 200mW for a 600mW VTX). Use this to override max supported power. 0 to disable and use whatever VTX reports as its capabilities | -| vtx_pit_mode_chan | 1 | | -| vtx_power | 1 | VTX RF power level to use. The exact number of mw depends on the VTX hardware. | -| vtx_smartaudio_early_akk_workaround | ON | Enable workaround for early AKK SAudio-enabled VTX bug. | -| yaw_deadband | 5 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. | -| yaw_lpf_hz | 0 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | -| yaw_rate | 20 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | +| Variable Name | Default Value | Min | Max | Description | +| ------------- | ------------- | --- | --- | ----------- | +| 3d_deadband_high | 1514 | PWM_RANGE_MIN | PWM_RANGE_MAX | High value of throttle deadband for 3D mode (when stick is in the deadband range, the value in 3d_neutral is used instead) | +| 3d_deadband_low | 1406 | PWM_RANGE_MIN | PWM_RANGE_MAX | Low value of throttle deadband for 3D mode (when stick is in the 3d_deadband_throttle range, the fixed values of 3d_deadband_low / _high are used instead) | +| 3d_deadband_throttle | 50 | 0 | 200 | Throttle signal will be held to a fixed value when throttle is centered with an error margin defined in this parameter. | +| 3d_neutral | 1460 | PWM_RANGE_MIN | PWM_RANGE_MAX | Neutral (stop) throttle value for 3D mode | +| acc_event_threshold_high | 0 | 0 | 65535 | Acceleration threshold [cm/s/s] for impact / high g event text messages sent by SIM module. Acceleration values greater than 4 g can occur in fixed wing flight without an impact, so a setting of 4000 or greater is suggested. 0 = detection off. | +| acc_event_threshold_low | 0 | 0 | 900 | Acceleration threshold [cm/s/s] for low-g / freefall detection text messages sent by SIM module. A setting of less than 100 is suggested. Valid values: [0-900], 0 = detection off. | +| acc_event_threshold_neg_x | 0 | 0 | 65535 | Acceleration threshold [cm/s/s] for backwards acceleration / fixed wing landing detection text messages sent by SIM module. Suggested value for fixed wing: 1100. 0 = detection off. | +| acc_hardware | AUTO | | | Selection of acc hardware. See Wiki Sensor auto detect and hardware failure detection for more info | +| acc_lpf_hz | 15 | 0 | 200 | Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | +| acc_lpf_type | BIQUAD | | | Specifies the type of the software LPF of the acc signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | +| acc_notch_cutoff | 1 | 1 | 255 | | +| acc_notch_hz | 0 | 0 | 255 | | +| accgain_x | 4096 | 1 | 8192 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | +| accgain_y | 4096 | 1 | 8192 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | +| accgain_z | 4096 | 1 | 8192 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | +| acczero_x | 0 | -32768 | 32767 | Calculated value after '6 position avanced calibration'. See Wiki page. | +| acczero_y | 0 | -32768 | 32767 | Calculated value after '6 position avanced calibration'. See Wiki page. | +| acczero_z | 0 | -32768 | 32767 | Calculated value after '6 position avanced calibration'. See Wiki page. | +| airmode_throttle_threshold | 1300 | 1000 | 2000 | Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used | +| airmode_type | STICK_CENTER | | | Defines the Airmode state handling type. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `airmode_throttle_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER_ONCE** or **STICK_CENTER** modes. | +| airspeed_adc_channel | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't have a dedicated connector for analog airspeed sensor will default to 0 | +| align_acc | DEFAULT | | | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | +| align_board_pitch | 0 | -1800 | 3600 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | +| align_board_roll | 0 | -1800 | 3600 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | +| align_board_yaw | 0 | -1800 | 3600 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | +| align_gyro | DEFAULT | | | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | +| align_mag | DEFAULT | | | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | +| align_mag_pitch | 0 | -1800 | 3600 | Same as align_mag_roll, but for the pitch axis. | +| align_mag_roll | 0 | -1800 | 3600 | Set the external mag alignment on the roll axis (in 0.1 degree steps). If this value is non-zero, the compass is assumed to be externally mounted and both the board and on-board compass alignent (align_mag) are ignored. See also align_mag_pitch and align_mag_yaw. | +| align_mag_yaw | 0 | -1800 | 3600 | Same as align_mag_roll, but for the yaw axis. | +| align_opflow | CW0FLIP | | | Optical flow module alignment (default CW0_DEG_FLIP) | +| alt_hold_deadband | 50 | 10 | 250 | Defines the deadband of throttle during alt_hold [r/c points] | +| antigravity_accelerator | 1 | 1 | 20 | | +| antigravity_cutoff_lpf_hz | 15 | 1 | 30 | Antigravity cutoff frequenct for Throtte filter. Antigravity is based on the difference between actual and filtered throttle input. The bigger is the difference, the bigger Antigravity gain | +| antigravity_gain | 1 | 1 | 20 | Max Antigravity gain. `1` means Antigravity is disabled, `2` means Iterm is allowed to double during rapid throttle movements | +| applied_defaults | 0 | 0 | 3 | Internal (configurator) hint. Should not be changed manually | +| baro_cal_tolerance | 150 | 0 | 1000 | Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. | +| baro_hardware | AUTO | | | Selection of baro hardware. See Wiki Sensor auto detect and hardware failure detection for more info | +| baro_median_filter | ON | | | 3-point median filtering for barometer readouts. No reason to change this setting | +| bat_cells | 0 | 0 | 12 | Number of cells of the battery (0 = auto-detect), see battery documentation. 7S, 9S and 11S batteries cannot be auto-detected. | +| bat_voltage_src | RAW | | | Chose between raw and sag compensated battery voltage to use for battery alarms and telemetry. Possible values are `RAW` and `SAG_COMP` | +| battery_capacity | 0 | 0 | 4294967295 | Set the battery capacity in mAh or mWh (see `battery_capacity_unit`). Used to calculate the remaining battery capacity. | +| battery_capacity_critical | 0 | 0 | 4294967295 | If the remaining battery capacity goes below this threshold the battery is considered empty and the beeper will emit long beeps. | +| battery_capacity_unit | MAH | | | Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour). | +| battery_capacity_warning | 0 | 0 | 4294967295 | If the remaining battery capacity goes below this threshold the beeper will emit short beeps and the relevant OSD items will blink. | +| blackbox_device | _target default_ | | | Selection of where to write blackbox data | +| blackbox_rate_denom | 1 | 1 | 65535 | Blackbox logging rate denominator. See blackbox_rate_num. | +| blackbox_rate_num | 1 | 1 | 65535 | Blackbox logging rate numerator. Use num/denom settings to decide if a frame should be logged, allowing control of the portion of logged loop iterations | +| cpu_underclock | OFF | | | This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz | +| cruise_power | 0 | 0 | 4294967295 | Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit | +| current_adc_channel | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for analog current sensor input. Defaults to board CURRENT sensor input (if available). 0 = disabled | +| current_meter_offset | _target default_ | -32768 | 32767 | This sets the output offset voltage of the current sensor in millivolts. | +| current_meter_scale | _target default_ | -10000 | 10000 | This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt. | +| current_meter_type | ADC | | | ADC , VIRTUAL, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position. | +| d_boost_factor | 1.25 | 1 | 3 | | +| d_boost_gyro_delta_lpf_hz | 80 | 10 | 250 | | +| d_boost_max_at_acceleration | 7500 | 1000 | 16000 | | +| deadband | 5 | 0 | 32 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. | +| debug_mode | NONE | | | Defines debug values exposed in debug variables (developer / debugging setting) | +| disarm_kill_switch | ON | | | Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. | +| display_force_sw_blink | OFF | | | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON | +| dji_esc_temp_source | ESC | | | Re-purpose the ESC temperature field for IMU/BARO temperature | +| dji_speed_source | GROUND | | | Sets the speed type displayed by the DJI OSD: GROUND, 3D, AIR | +| dji_use_name_for_messages | ON | | | Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance | +| dji_workarounds | 1 | 0 | 255 | Enables workarounds for different versions of MSP protocol used | +| dshot_beeper_enabled | ON | | | Whether using DShot motors as beepers is enabled | +| dshot_beeper_tone | 1 | 1 | 5 | Sets the DShot beeper tone | +| dterm_lpf2_hz | 0 | 0 | 500 | Cutoff frequency for stage 2 D-term low pass filter | +| dterm_lpf2_type | BIQUAD | | | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | +| dterm_lpf_hz | 40 | 0 | 500 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value | +| dterm_lpf_type | BIQUAD | | | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | +| dynamic_gyro_notch_enabled | OFF | | | Enable/disable dynamic gyro notch also known as Matrix Filter | +| dynamic_gyro_notch_min_hz | 150 | 30 | 1000 | Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` | +| dynamic_gyro_notch_q | 120 | 1 | 1000 | Q factor for dynamic notches | +| dynamic_gyro_notch_range | MEDIUM | | | Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers | +| eleres_freq | 435 | 415 | 450 | | +| eleres_loc_delay | 240 | 30 | 1800 | | +| eleres_loc_en | OFF | | | | +| eleres_loc_power | 7 | 0 | 7 | | +| eleres_signature | 0 | | 4294967295 | | +| eleres_telemetry_en | OFF | | | | +| eleres_telemetry_power | 7 | 0 | 7 | | +| esc_sensor_listen_only | OFF | | | Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case | +| failsafe_delay | 5 | 0 | 200 | Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). | +| failsafe_fw_pitch_angle | 100 | -800 | 800 | Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb | +| failsafe_fw_roll_angle | -200 | -800 | 800 | Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll | +| failsafe_fw_yaw_rate | -45 | -1000 | 1000 | Requested yaw rate to execute when `SET-THR` failsafe is active on a fixed-wing machine. In deg/s. Negative values = left turn | +| failsafe_lights | ON | | | Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF]. | +| failsafe_lights_flash_on_time | 100 | 20 | 65535 | Flash lights ON time in milliseconds when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled. [20-65535]. | +| failsafe_lights_flash_period | 1000 | 40 | 65535 | Time in milliseconds between two flashes when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled [40-65535]. | +| failsafe_min_distance | 0 | 0 | 65000 | If failsafe happens when craft is closer than this distance in centimeters from home, failsafe will not execute regular failsafe_procedure, but will execute procedure specified in failsafe_min_distance_procedure instead. 0 = Normal failsafe_procedure always taken. | +| failsafe_min_distance_procedure | DROP | | | What failsafe procedure to initiate in Stage 2 when craft is closer to home than failsafe_min_distance. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | +| failsafe_mission | ON | | | If set to `OFF` the failsafe procedure won't be triggered and the mission will continue if the FC is in WP (automatic mission) mode | +| failsafe_off_delay | 200 | 0 | 200 | Time in deciseconds to wait before turning off motors when failsafe is activated. 0 = No timeout. See [Failsafe documentation](Failsafe.md#failsafe_off_delay). | +| failsafe_procedure | SET-THR | | | What failsafe procedure to initiate in Stage 2. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | +| failsafe_recovery_delay | 5 | 0 | 200 | Time in deciseconds to wait before aborting failsafe when signal is recovered. See [Failsafe documentation](Failsafe.md#failsafe_recovery_delay). | +| failsafe_stick_threshold | 50 | 0 | 500 | Threshold for stick motion to consider failsafe condition resolved. If non-zero failsafe won't clear even if RC link is restored - you have to move sticks to exit failsafe. | +| failsafe_throttle | 1000 | PWM_RANGE_MIN | PWM_RANGE_MAX | Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | +| failsafe_throttle_low_delay | 0 | 0 | 300 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout | +| fixed_wing_auto_arm | OFF | | | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. | +| flaperon_throw_offset | 200 | FLAPERON_THROW_MIN | FLAPERON_THROW_MAX | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. | +| flip_over_after_crash_power_factor | 65 | 0 | 100 | flip over after crash power factor | +| fpv_mix_degrees | 0 | 0 | 50 | | +| frsky_coordinates_format | 0 | 0 | FRSKY_FORMAT_NMEA | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA | +| frsky_default_latitude | 0 | -90 | 90 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. | +| frsky_default_longitude | 0 | -180 | 180 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. | +| frsky_pitch_roll | OFF | | | S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data | +| frsky_unit | METRIC | | | Not used? [METRIC/IMPERIAL] | +| frsky_vfas_precision | 0 | FRSKY_VFAS_PRECISION_LOW | FRSKY_VFAS_PRECISION_HIGH | D-Series telemetry only: Set to 1 to send raw VBat value in 0.1V resolution for receivers that can handle it, or 0 (default) to use the standard method | +| fw_autotune_ff_to_i_tc | 600 | 100 | 5000 | FF to I time (defines time for I to reach the same level of response as FF) [ms] | +| fw_autotune_ff_to_p_gain | 10 | 0 | 100 | FF to P gain (strength relationship) [%] | +| fw_autotune_overshoot_time | 100 | 50 | 500 | Time [ms] to detect sustained overshoot | +| fw_autotune_threshold | 50 | 0 | 100 | Threshold [%] of max rate to consider overshoot/undershoot detection | +| fw_autotune_undershoot_time | 200 | 50 | 500 | Time [ms] to detect sustained undershoot | +| fw_d_level | 75 | 0 | 200 | Fixed-wing attitude stabilisation HORIZON transition point | +| fw_d_pitch | 0 | 0 | 200 | Fixed wing rate stabilisation D-gain for PITCH | +| fw_d_roll | 0 | 0 | 200 | Fixed wing rate stabilisation D-gain for ROLL | +| fw_d_yaw | 0 | 0 | 200 | Fixed wing rate stabilisation D-gain for YAW | +| fw_ff_pitch | 50 | 0 | 200 | Fixed-wing rate stabilisation FF-gain for PITCH | +| fw_ff_roll | 50 | 0 | 200 | Fixed-wing rate stabilisation FF-gain for ROLL | +| fw_ff_yaw | 60 | 0 | 200 | Fixed-wing rate stabilisation FF-gain for YAW | +| fw_i_level | 5 | 0 | 200 | Fixed-wing attitude stabilisation low-pass filter cutoff | +| fw_i_pitch | 7 | 0 | 200 | Fixed-wing rate stabilisation I-gain for PITCH | +| fw_i_roll | 7 | 0 | 200 | Fixed-wing rate stabilisation I-gain for ROLL | +| fw_i_yaw | 10 | 0 | 200 | Fixed-wing rate stabilisation I-gain for YAW | +| fw_iterm_limit_stick_position | 0.5 | 0 | 1 | Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side | +| fw_iterm_throw_limit | 165 | FW_ITERM_THROW_LIMIT_MIN | FW_ITERM_THROW_LIMIT_MAX | Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. | +| fw_level_pitch_trim | 0 | -10 | 10 | Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level | +| fw_loiter_direction | RIGHT | | | Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick. | +| fw_min_throttle_down_pitch | 0 | 0 | 450 | Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees) | +| fw_p_level | 20 | 0 | 200 | Fixed-wing attitude stabilisation P-gain | +| fw_p_pitch | 5 | 0 | 200 | Fixed-wing rate stabilisation P-gain for PITCH | +| fw_p_roll | 5 | 0 | 200 | Fixed-wing rate stabilisation P-gain for ROLL | +| fw_p_yaw | 6 | 0 | 200 | Fixed-wing rate stabilisation P-gain for YAW | +| fw_reference_airspeed | 1500 | 300 | 6000 | Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present. | +| fw_tpa_time_constant | 0 | 0 | 5000 | TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. Planes with low thrust/weight ratio generally need higher time constant. Default is zero for compatibility with old setups | +| fw_turn_assist_pitch_gain | 1 | 0 | 2 | Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | +| fw_turn_assist_yaw_gain | 1 | 0 | 2 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | +| fw_yaw_iterm_freeze_bank_angle | 0 | 0 | 90 | Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled. | +| gps_auto_baud | ON | | | Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS. When used with NAZA/DJI it will automatic detect GPS baudrate and change to it, ignoring the selected baudrate set in ports | +| gps_auto_config | ON | | | Enable automatic configuration of UBlox GPS receivers. | +| gps_dyn_model | AIR_1G | | | GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying. | +| gps_min_sats | 6 | 5 | 10 | Minimum number of GPS satellites in view to acquire GPS_FIX and consider GPS position valid. Some GPS receivers appeared to be very inaccurate with low satellite count. | +| gps_provider | UBLOX | | | Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N). | +| gps_sbas_mode | NONE | | | Which SBAS mode to be used | +| gps_ublox_use_galileo | OFF | | | Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. | +| gyro_dyn_lpf_curve_expo | 5 | 1 | 10 | Expo value for the throttle-to-frequency mapping for Dynamic LPF | +| gyro_dyn_lpf_max_hz | 500 | 40 | 1000 | Maximum frequency of the gyro Dynamic LPF | +| gyro_dyn_lpf_min_hz | 200 | 40 | 400 | Minimum frequency of the gyro Dynamic LPF | +| gyro_hardware_lpf | 256HZ | | | Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first. | +| gyro_lpf_hz | 60 | | 200 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | +| gyro_lpf_type | BIQUAD | | | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | +| gyro_notch_cutoff | 1 | 1 | 500 | | +| gyro_notch_hz | 0 | | 500 | | +| gyro_stage2_lowpass_hz | 0 | 0 | 500 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz) | +| gyro_stage2_lowpass_type | BIQUAD | | | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | +| gyro_to_use | 0 | 0 | 1 | | +| gyro_use_dyn_lpf | OFF | | | Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. | +| has_flaps | OFF | | | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot | +| heading_hold_rate_limit | 90 | HEADING_HOLD_RATE_LIMIT_MIN | HEADING_HOLD_RATE_LIMIT_MAX | This setting limits yaw rotation rate that HEADING_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when HEADING_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. | +| hott_alarm_sound_interval | 5 | 0 | 120 | Battery alarm delay in seconds for Hott telemetry | +| i2c_speed | 400KHZ | | | This setting controls the clock speed of I2C bus. 400KHZ is the default that most setups are able to use. Some noise-free setups may be overclocked to 800KHZ. Some sensor chips or setups with long wires may work unreliably at 400KHZ - user can try lowering the clock speed to 200KHZ or even 100KHZ. User need to bear in mind that lower clock speeds might require higher looptimes (lower looptime rate) | +| ibus_telemetry_type | 0 | 0 | 255 | Type compatibility ibus telemetry for transmitters. See Telemetry.md label IBUS for details. | +| idle_power | 0 | 0 | 65535 | Power draw at zero throttle used for remaining flight time/distance estimation in 0.01W unit | +| imu2_align_pitch | 0 | -1800 | 3600 | Pitch alignment for Secondary IMU. 1/10 of a degree | +| imu2_align_roll | 0 | -1800 | 3600 | Roll alignment for Secondary IMU. 1/10 of a degree | +| imu2_align_yaw | 0 | -1800 | 3600 | Yaw alignment for Secondary IMU. 1/10 of a degree | +| imu2_gain_acc_x | 0 | -32768 | 32767 | Secondary IMU ACC calibration data | +| imu2_gain_acc_y | 0 | -32768 | 32767 | Secondary IMU ACC calibration data | +| imu2_gain_acc_z | 0 | -32768 | 32767 | Secondary IMU ACC calibration data | +| imu2_gain_mag_x | 0 | -32768 | 32767 | Secondary IMU MAG calibration data | +| imu2_gain_mag_y | 0 | -32768 | 32767 | Secondary IMU MAG calibration data | +| imu2_gain_mag_z | 0 | -32768 | 32767 | Secondary IMU MAG calibration data | +| imu2_hardware | NONE | | | Selection of a Secondary IMU hardware type. NONE disables this functionality | +| imu2_radius_acc | 0 | -32768 | 32767 | Secondary IMU MAG calibration data | +| imu2_radius_mag | 0 | -32768 | 32767 | Secondary IMU MAG calibration data | +| imu2_use_for_osd_ahi | OFF | | | If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon | +| imu2_use_for_osd_heading | OFF | | | If set to ON, Secondary IMU data will be used for Analog OSD heading | +| imu2_use_for_stabilized | OFF | | | If set to ON, Secondary IMU data will be used for Angle, Horizon and all other modes that control attitude (PosHold, WP, RTH) | +| imu_acc_ignore_rate | 0 | 0 | 20 | Total gyro rotation rate threshold [deg/s] to consider accelerometer trustworthy on airplanes | +| imu_acc_ignore_slope | 0 | 0 | 5 | Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight) | +| imu_dcm_ki | 50 | | 65535 | Inertial Measurement Unit KI Gain for accelerometer measurements | +| imu_dcm_ki_mag | 0 | | 65535 | Inertial Measurement Unit KI Gain for compass measurements | +| imu_dcm_kp | 2500 | | 65535 | Inertial Measurement Unit KP Gain for accelerometer measurements | +| imu_dcm_kp_mag | 10000 | | 65535 | Inertial Measurement Unit KP Gain for compass measurements | +| inav_allow_dead_reckoning | OFF | | | Defines if inav will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation | +| inav_auto_mag_decl | ON | | | Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored. | +| inav_baro_epv | 100 | 0 | 9999 | Uncertainty value for barometric sensor [cm] | +| inav_gravity_cal_tolerance | 5 | 0 | 255 | Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. | +| inav_max_eph_epv | 1000 | 0 | 9999 | Maximum uncertainty value until estimated position is considered valid and is used for navigation [cm] | +| inav_max_surface_altitude | 200 | 0 | 1000 | Max allowed altitude for surface following mode. [cm] | +| inav_reset_altitude | FIRST_ARM | | | Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming) | +| inav_reset_home | FIRST_ARM | | | Allows to chose when the home position is reset. Can help prevent resetting home position after accidental mid-air disarm. Possible values are: NEVER, FIRST_ARM and EACH_ARM | +| inav_use_gps_no_baro | OFF | | | | +| inav_use_gps_velned | ON | | | Defined if iNav should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF iNav will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance. | +| inav_w_acc_bias | 0.01 | 0 | 1 | Weight for accelerometer drift estimation | +| inav_w_xy_flow_p | 1.0 | 0 | 100 | | +| inav_w_xy_flow_v | 2.0 | 0 | 100 | | +| inav_w_xy_gps_p | 1.0 | 0 | 10 | Weight of GPS coordinates in estimated UAV position and speed. | +| inav_w_xy_gps_v | 2.0 | 0 | 10 | Weight of GPS velocity data in estimated UAV speed | +| inav_w_xy_res_v | 0.5 | 0 | 10 | Decay coefficient for estimated velocity when GPS reference for position is lost | +| inav_w_xyz_acc_p | 1.0 | 0 | 1 | | +| inav_w_z_baro_p | 0.35 | 0 | 10 | Weight of barometer measurements in estimated altitude and climb rate | +| inav_w_z_gps_p | 0.2 | 0 | 10 | Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes | +| inav_w_z_gps_v | 0.1 | 0 | 10 | Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors. If GPS doesn't support native climb rate reporting (i.e. NMEA GPS) you may consider setting this to zero | +| inav_w_z_res_v | 0.5 | 0 | 10 | Decay coefficient for estimated climb rate when baro/GPS reference for altitude is lost | +| inav_w_z_surface_p | 3.5 | 0 | 100 | | +| inav_w_z_surface_v | 6.1 | 0 | 100 | | +| iterm_windup | 50 | 0 | 90 | Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) | +| ledstrip_visual_beeper | OFF | | | | +| log_level | ERROR | | | Defines serial debugging log level. See `docs/development/serial_printf_debugging.md` for usage. | +| log_topics | 0 | 0 | 4294967295 | Defines serial debugging log topic. See `docs/development/serial_printf_debugging.md` for usage. | +| looptime | 1000 | | 9000 | This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible. | +| ltm_update_rate | NORMAL | | | Defines the LTM update rate (use of bandwidth [NORMAL/MEDIUM/SLOW]). See Telemetry.md, LTM section for details. | +| mag_calibration_time | 30 | 20 | 120 | Adjust how long time the Calibration of mag will last. | +| mag_declination | 0 | -18000 | 18000 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you acquire valid GPS fix. | +| mag_hardware | AUTO | | | Selection of mag hardware. See Wiki Sensor auto detect and hardware failure detection for more info | +| mag_to_use | 0 | 0 | 1 | Allow to chose between built-in and external compass sensor if they are connected to separate buses. Currently only for REVO target | +| maggain_x | 1024 | -32768 | 32767 | Magnetometer calibration X gain. If 1024, no calibration or calibration failed | +| maggain_y | 1024 | -32768 | 32767 | Magnetometer calibration Y gain. If 1024, no calibration or calibration failed | +| maggain_z | 1024 | -32768 | 32767 | Magnetometer calibration Z gain. If 1024, no calibration or calibration failed | +| magzero_x | 0 | -32768 | 32767 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. | +| magzero_y | 0 | -32768 | 32767 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. | +| magzero_z | 0 | -32768 | 32767 | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. | +| manual_pitch_rate | 100 | 0 | 100 | Servo travel multiplier for the PITCH axis in `MANUAL` flight mode [0-100]% | +| manual_rc_expo | 70 | 0 | 100 | Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100] | +| manual_rc_yaw_expo | 20 | 0 | 100 | Exposition value used for the YAW axis by the `MANUAL` flight mode [0-100] | +| manual_roll_rate | 100 | 0 | 100 | Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]% | +| manual_yaw_rate | 100 | 0 | 100 | Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]% | +| mavlink_ext_status_rate | 2 | 0 | 255 | | +| mavlink_extra1_rate | 10 | 0 | 255 | | +| mavlink_extra2_rate | 2 | 0 | 255 | | +| mavlink_extra3_rate | 1 | 0 | 255 | | +| mavlink_pos_rate | 2 | 0 | 255 | | +| mavlink_rc_chan_rate | 5 | 0 | 255 | | +| mavlink_version | 2 | 1 | 2 | Version of MAVLink to use | +| max_angle_inclination_pit | 300 | 100 | 900 | Maximum inclination in level (angle) mode (PITCH axis). 100=10° | +| max_angle_inclination_rll | 300 | 100 | 900 | Maximum inclination in level (angle) mode (ROLL axis). 100=10° | +| max_check | 1900 | PWM_RANGE_MIN | PWM_RANGE_MAX | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | +| max_throttle | 1850 | PWM_RANGE_MIN | PWM_RANGE_MAX | This is the maximum value (in us) sent to esc when armed. Default of 1850 are OK for everyone (legacy). For modern ESCs, higher values (c. 2000) may be more appropriate. If you have brushed motors, the value should be set to 2000. | +| mc_cd_lpf_hz | 30 | 0 | 200 | Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky | +| mc_cd_pitch | 60 | 0 | 200 | Multicopter Control Derivative gain for PITCH | +| mc_cd_roll | 60 | 0 | 200 | Multicopter Control Derivative gain for ROLL | +| mc_cd_yaw | 60 | 0 | 200 | Multicopter Control Derivative gain for YAW | +| mc_d_level | 75 | 0 | 200 | Multicopter attitude stabilisation HORIZON transition point | +| mc_d_pitch | 23 | 0 | 200 | Multicopter rate stabilisation D-gain for PITCH | +| mc_d_roll | 23 | 0 | 200 | Multicopter rate stabilisation D-gain for ROLL | +| mc_d_yaw | 0 | 0 | 200 | Multicopter rate stabilisation D-gain for YAW | +| mc_i_level | 15 | 0 | 200 | Multicopter attitude stabilisation low-pass filter cutoff | +| mc_i_pitch | 30 | 0 | 200 | Multicopter rate stabilisation I-gain for PITCH | +| mc_i_roll | 30 | 0 | 200 | Multicopter rate stabilisation I-gain for ROLL | +| mc_i_yaw | 45 | 0 | 200 | Multicopter rate stabilisation I-gain for YAW | +| mc_iterm_relax | RP | | | | +| mc_iterm_relax_cutoff | 15 | 1 | 100 | | +| mc_p_level | 20 | 0 | 200 | Multicopter attitude stabilisation P-gain | +| mc_p_pitch | 40 | 0 | 200 | Multicopter rate stabilisation P-gain for PITCH | +| mc_p_roll | 40 | 0 | 200 | Multicopter rate stabilisation P-gain for ROLL | +| mc_p_yaw | 85 | 0 | 200 | Multicopter rate stabilisation P-gain for YAW | +| min_check | 1100 | PWM_RANGE_MIN | PWM_RANGE_MAX | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | +| min_command | 1000 | 0 | PWM_RANGE_MAX | This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly when powered up, try decreasing this value. It can also be used for calibrating all ESCs at once. | +| mode_range_logic_operator | OR | | | Control how Mode selection works in flight modes. If you example have Angle mode configured on two different Aux channels, this controls if you need both activated ( AND ) or if you only need one activated ( OR ) to active angle mode. | +| model_preview_type | -1 | -1 | 32767 | ID of mixer preset applied in a Configurator. **Do not modify manually**. Used only for backup/restore reasons. | +| moron_threshold | 32 | | 128 | When powering up, gyro bias is calculated. If the model is shaking/moving during this initial calibration, offsets are calculated incorrectly, and could lead to poor flying performance. This threshold means how much average gyro reading could differ before re-calibration is triggered. | +| motor_accel_time | 0 | 0 | 1000 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] | +| motor_decel_time | 0 | 0 | 1000 | Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] | +| motor_direction_inverted | OFF | | | Use if you need to inverse yaw motor direction. | +| motor_poles | 14 | 4 | 255 | The number of motor poles. Required to compute motor RPM | +| motor_pwm_protocol | ONESHOT125 | | | Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, DSHOT150, DSHOT300, DSHOT600, DSHOT1200, BRUSHED | +| motor_pwm_rate | 400 | 50 | 32000 | Output frequency (in Hz) for motor pins. Default is 400Hz for motor with motor_pwm_protocol set to STANDARD. For *SHOT (e.g. ONESHOT125) values of 1000 and 2000 have been tested by the development team and are supported. It may be possible to use higher values. For BRUSHED values of 8000 and above should be used. Setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported for brushed. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set max_throttle to 2000. | +| msp_override_channels | 0 | 0 | 65535 | Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode. | +| name | _empty_ | | | Craft name | +| nav_auto_climb_rate | 500 | 10 | 2000 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] | +| nav_auto_speed | 300 | 10 | 2000 | Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only] | +| nav_disarm_on_landing | OFF | | | If set to ON, iNav disarms the FC after landing | +| nav_emerg_landing_speed | 500 | 100 | 2000 | Rate of descent UAV will try to maintain when doing emergency descent sequence [cm/s] | +| nav_extra_arming_safety | ON | | | If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured. ALLOW_BYPASS allows the user to momentarily disable this check by holding yaw high (left stick held at the bottom right in mode 2) when switch arming is used | +| nav_fw_allow_manual_thr_increase | OFF | | | Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing | +| nav_fw_bank_angle | 35 | 5 | 80 | Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll | +| nav_fw_climb_angle | 20 | 5 | 80 | Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit | +| nav_fw_control_smoothness | 0 | 0 | 9 | How smoothly the autopilot controls the airplane to correct the navigation error | +| nav_fw_cruise_speed | 0 | 0 | 65535 | Speed for the plane/wing at cruise throttle used for remaining flight time/distance estimation in cm/s | +| nav_fw_cruise_thr | 1400 | 1000 | 2000 | Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded ) | +| nav_fw_cruise_yaw_rate | 20 | 0 | 60 | Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps] | +| nav_fw_dive_angle | 15 | 5 | 80 | Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit | +| nav_fw_heading_p | 60 | 0 | 255 | P gain of Heading Hold controller (Fixedwing) | +| nav_fw_land_dive_angle | 2 | -20 | 20 | Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees | +| nav_fw_launch_accel | 1863 | 1000 | 20000 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s | +| nav_fw_launch_climb_angle | 18 | 5 | 45 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit | +| nav_fw_launch_detect_time | 40 | 10 | 1000 | Time for which thresholds have to breached to consider launch happened [ms] | +| nav_fw_launch_end_time | 3000 | 0 | 5000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] | +| nav_fw_launch_idle_thr | 1000 | 1000 | 2000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position) | +| nav_fw_launch_max_altitude | 0 | 0 | 60000 | Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. | +| nav_fw_launch_max_angle | 45 | 5 | 180 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] | +| nav_fw_launch_min_time | 0 | 0 | 60000 | Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]. | +| nav_fw_launch_motor_delay | 500 | 0 | 5000 | Delay between detected launch and launch sequence start and throttling up (ms) | +| nav_fw_launch_spinup_time | 100 | 0 | 1000 | Time to bring power from minimum throttle to nav_fw_launch_thr - to avoid big stress on ESC and large torque from propeller | +| nav_fw_launch_thr | 1700 | 1000 | 2000 | Launch throttle - throttle to be set during launch sequence (pwm units) | +| nav_fw_launch_timeout | 5000 | | 60000 | Maximum time for launch sequence to be executed. After this time LAUNCH mode will be turned off and regular flight mode will take over (ms) | +| nav_fw_launch_velocity | 300 | 100 | 10000 | Forward velocity threshold for swing-launch detection [cm/s] | +| nav_fw_loiter_radius | 7500 | 0 | 30000 | PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] | +| nav_fw_max_thr | 1700 | 1000 | 2000 | Maximum throttle for flying wing in GPS assisted modes | +| nav_fw_min_thr | 1200 | 1000 | 2000 | Minimum throttle for flying wing in GPS assisted modes | +| nav_fw_pitch2thr | 10 | 0 | 100 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | +| nav_fw_pitch2thr_smoothing | 6 | 0 | 9 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | +| nav_fw_pitch2thr_threshold | 50 | 0 | 900 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | +| nav_fw_pos_hdg_d | 0 | 0 | 255 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | +| nav_fw_pos_hdg_i | 2 | 0 | 255 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | +| nav_fw_pos_hdg_p | 30 | 0 | 255 | P gain of heading PID controller. (Fixedwing, rovers, boats) | +| nav_fw_pos_hdg_pidsum_limit | 350 | PID_SUM_LIMIT_MIN | PID_SUM_LIMIT_MAX | Output limit for heading trajectory PID controller. (Fixedwing, rovers, boats) | +| nav_fw_pos_xy_d | 8 | 0 | 255 | D gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero | +| nav_fw_pos_xy_i | 5 | 0 | 255 | I gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero | +| nav_fw_pos_xy_p | 75 | 0 | 255 | P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH | +| nav_fw_pos_z_d | 10 | 0 | 255 | D gain of altitude PID controller (Fixedwing) | +| nav_fw_pos_z_i | 5 | 0 | 255 | I gain of altitude PID controller (Fixedwing) | +| nav_fw_pos_z_p | 40 | 0 | 255 | P gain of altitude PID controller (Fixedwing) | +| nav_fw_yaw_deadband | 0 | 0 | 90 | Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course | +| nav_land_maxalt_vspd | 200 | 100 | 2000 | Vertical descent velocity above nav_land_slowdown_maxalt during the RTH landing phase. [cm/s] | +| nav_land_minalt_vspd | 50 | 50 | 500 | Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s] | +| nav_land_slowdown_maxalt | 2000 | 500 | 4000 | Defines at what altitude the descent velocity should start to ramp down from `nav_land_maxalt_vspd` to `nav_land_minalt_vspd` during the RTH landing phase [cm] | +| nav_land_slowdown_minalt | 500 | 50 | 1000 | Defines at what altitude the descent velocity should start to be `nav_land_minalt_vspd` [cm] | +| nav_manual_climb_rate | 200 | 10 | 2000 | Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] | +| nav_manual_speed | 500 | 10 | 2000 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] | +| nav_max_terrain_follow_alt | 100 | | 1000 | Max allowed above the ground altitude for terrain following mode | +| nav_mc_auto_disarm_delay | 2000 | 100 | 10000 | Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s) | +| nav_mc_bank_angle | 30 | 15 | 45 | Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude | +| nav_mc_braking_bank_angle | 40 | 15 | 60 | max angle that MR is allowed to bank in BOOST mode | +| nav_mc_braking_boost_disengage_speed | 100 | 0 | 1000 | BOOST will be disabled when speed goes below this value | +| nav_mc_braking_boost_factor | 100 | 0 | 200 | acceleration factor for BOOST phase | +| nav_mc_braking_boost_speed_threshold | 150 | 100 | 1000 | BOOST can be enabled when speed is above this value | +| nav_mc_braking_boost_timeout | 750 | 0 | 5000 | how long in ms BOOST phase can happen | +| nav_mc_braking_disengage_speed | 75 | 0 | 1000 | braking is disabled when speed goes below this value | +| nav_mc_braking_speed_threshold | 100 | 0 | 1000 | min speed in cm/s above which braking can happen | +| nav_mc_braking_timeout | 2000 | 100 | 5000 | timeout in ms for braking | +| nav_mc_heading_p | 60 | 0 | 255 | P gain of Heading Hold controller (Multirotor) | +| nav_mc_hover_thr | 1500 | 1000 | 2000 | Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering. | +| nav_mc_pos_deceleration_time | 120 | 0 | 255 | Used for stoping distance calculation. Stop position is computed as _speed_ * _nav_mc_pos_deceleration_time_ from the place where sticks are released. Braking mode overrides this setting | +| nav_mc_pos_expo | 10 | 0 | 255 | Expo for PosHold control | +| nav_mc_pos_xy_p | 65 | 0 | 255 | Controls how fast the drone will fly towards the target position. This is a multiplier to convert displacement to target velocity | +| nav_mc_pos_z_p | 50 | 0 | 255 | P gain of altitude PID controller (Multirotor) | +| nav_mc_vel_xy_d | 100 | 0 | 255 | D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target. | +| nav_mc_vel_xy_dterm_attenuation | 90 | 0 | 100 | Maximum D-term attenution percentage for horizontal velocity PID controller (Multirotor). It allows to smooth the PosHold CRUISE, WP and RTH when Multirotor is traveling at full speed. Dterm is not attenuated at low speeds, breaking and accelerating. | +| nav_mc_vel_xy_dterm_attenuation_end | 60 | 0 | 100 | A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum | +| nav_mc_vel_xy_dterm_attenuation_start | 10 | 0 | 100 | A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins | +| nav_mc_vel_xy_dterm_lpf_hz | 2 | 0 | 100 | | +| nav_mc_vel_xy_ff | 40 | 0 | 255 | | +| nav_mc_vel_xy_i | 15 | 0 | 255 | I gain of Position-Rate (Velocity to Acceleration) PID controller. Used for drift compensation (caused by wind for example). Higher I means stronger response to drift. Too much I gain might cause target overshot | +| nav_mc_vel_xy_p | 40 | 0 | 255 | P gain of Position-Rate (Velocity to Acceleration) PID controller. Higher P means stronger response when position error occurs. Too much P might cause "nervous" behavior and oscillations | +| nav_mc_vel_z_d | 10 | 0 | 255 | D gain of velocity PID controller | +| nav_mc_vel_z_i | 50 | 0 | 255 | I gain of velocity PID controller | +| nav_mc_vel_z_p | 100 | 0 | 255 | P gain of velocity PID controller | +| nav_mc_wp_slowdown | ON | | | When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes. | +| nav_min_rth_distance | 500 | 0 | 5000 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. | +| nav_overrides_motor_stop | ALL_NAV | | | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | +| nav_position_timeout | 5 | 0 | 10 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | +| nav_rth_abort_threshold | 50000 | | 65000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] | +| nav_rth_allow_landing | ALWAYS | | | If set to ON drone will land as a last phase of RTH. | +| nav_rth_alt_control_override | OFF | | | If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the RTH climb phase using full pitch or roll stick held for > 1 second. RTH altitude is reset to the current altitude using pitch down stick. RTH CLIMB FIRST is overridden using right roll stick so craft turns and heads directly to home (CLIMB FIRST override only works for fixed wing) | +| nav_rth_alt_mode | AT_LEAST | | | Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details | +| nav_rth_altitude | 1000 | | 65000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) | +| nav_rth_climb_first | ON | | | If set to ON or ON_FW_SPIRAL aircraft will climb to nav_rth_altitude first before turning to head home. If set to OFF aircraft will turn and head home immediately climbing on the way. For a fixed wing ON will use a linear climb, ON_FW_SPIRAL will use a loiter turning climb with climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius (ON_FW_SPIRAL is a fixed wing setting and behaves the same as ON for a multirotor) | +| nav_rth_climb_ignore_emerg | OFF | | | If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. | +| nav_rth_home_altitude | 0 | | 65000 | Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm] | +| nav_rth_tail_first | OFF | | | If set to ON drone will return tail-first. Obviously meaningless for airplanes. | +| nav_use_fw_yaw_control | OFF | | | Enables or Disables the use of the heading PID controller on fixed wing. Heading PID controller is always enabled for rovers and boats | +| nav_use_midthr_for_althold | OFF | | | If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude | +| nav_user_control_mode | ATTI | | | Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - pitch/roll controls attitude like in ANGLE mode; CRUISE - pitch/roll controls velocity in forward and right direction. | +| nav_wp_radius | 100 | 10 | 10000 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius | +| nav_wp_safe_distance | 10000 | | 65000 | First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check. | +| opflow_hardware | NONE | | | Selection of OPFLOW hardware. | +| opflow_scale | 10.5 | 0 | 10000 | | +| osd_ahi_bordered | OFF | | | Shows a border/corners around the AHI region (pixel OSD only) | +| osd_ahi_height | 162 | | 255 | AHI height in pixels (pixel OSD only) | +| osd_ahi_max_pitch | 20 | 10 | 90 | Max pitch, in degrees, for OSD artificial horizon | +| osd_ahi_reverse_roll | OFF | | | | +| osd_ahi_style | DEFAULT | | | Sets OSD Artificial Horizon style "DEFAULT" or "LINE" for the FrSky Graphical OSD. | +| osd_ahi_vertical_offset | -18 | -128 | 127 | AHI vertical offset from center (pixel OSD only) | +| osd_ahi_width | 132 | | 255 | AHI width in pixels (pixel OSD only) | +| osd_alt_alarm | 100 | 0 | 10000 | Value above which to make the OSD relative altitude indicator blink (meters) | +| osd_baro_temp_alarm_max | 600 | -550 | 1250 | Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade) | +| osd_baro_temp_alarm_min | -200 | -550 | 1250 | Temperature under which the baro temperature OSD element will start blinking (decidegrees centigrade) | +| osd_camera_fov_h | 135 | 60 | 150 | Horizontal field of view for the camera in degres | +| osd_camera_fov_v | 85 | 30 | 120 | Vertical field of view for the camera in degres | +| osd_camera_uptilt | 0 | -40 | 80 | Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal | +| osd_coordinate_digits | 9 | 8 | 11 | | +| osd_crosshairs_style | DEFAULT | | | To set the visual type for the crosshair | +| osd_crsf_lq_format | TYPE1 | | | To select LQ format | +| osd_current_alarm | 0 | 0 | 255 | Value above which the OSD current consumption element will start blinking. Measured in full Amperes. | +| osd_dist_alarm | 1000 | 0 | 50000 | Value above which to make the OSD distance from home indicator blink (meters) | +| osd_esc_temp_alarm_max | 900 | -550 | 1500 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | +| osd_esc_temp_alarm_min | -200 | -550 | 1500 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) | +| osd_estimations_wind_compensation | ON | | | Use wind estimation for remaining flight time/distance estimation | +| osd_failsafe_switch_layout | OFF | | | If enabled the OSD automatically switches to the first layout during failsafe | +| osd_force_grid | OFF | | | Force OSD to work in grid mode even if the OSD device supports pixel level access (mainly used for development) | +| osd_gforce_alarm | 5 | 0 | 20 | Value above which the OSD g force indicator will blink (g) | +| osd_gforce_axis_alarm_max | 5 | -20 | 20 | Value above which the OSD axis g force indicators will blink (g) | +| osd_gforce_axis_alarm_min | -5 | -20 | 20 | Value under which the OSD axis g force indicators will blink (g) | +| osd_home_position_arm_screen | ON | | | Should home position coordinates be displayed on the arming screen. | +| osd_horizon_offset | 0 | -2 | 2 | To vertically adjust the whole OSD and AHI and scrolling bars | +| osd_hud_homepoint | OFF | | | To 3D-display the home point location in the hud | +| osd_hud_homing | OFF | | | To display little arrows around the crossair showing where the home point is in the hud | +| osd_hud_margin_h | 3 | 0 | 4 | Left and right margins for the hud area | +| osd_hud_margin_v | 3 | 1 | 3 | Top and bottom margins for the hud area | +| osd_hud_radar_disp | 0 | 0 | 4 | Maximum count of nearby aircrafts or points of interest to display in the hud, as sent from an ESP32 LoRa module. Set to 0 to disable (show nothing). The nearby aircrafts will appear as markers A, B, C, etc | +| osd_hud_radar_nearest | 0 | 0 | 4000 | To display an extra bar of informations at the bottom of the hud area for the closest radar aircraft found, if closest than the set value, in meters. Shows relative altitude (meters or feet, with an up or down arrow to indicate if above or below), speed (in m/s or f/s), and absolute heading (in °, 0 is north, 90 is east, 180 is south, 270 is west). Set to 0 (zero) to disable. | +| osd_hud_radar_range_max | 4000 | 100 | 9990 | In meters, radar aircrafts further away than this will not be displayed in the hud | +| osd_hud_radar_range_min | 3 | 1 | 30 | In meters, radar aircrafts closer than this will not be displayed in the hud | +| osd_hud_wp_disp | 0 | 0 | 3 | How many navigation waypoints are displayed, set to 0 (zero) to disable. As sample, if set to 2, and you just passed the 3rd waypoint of the mission, you'll see markers for the 4th waypoint (marked 1) and the 5th waypoint (marked 2) | +| osd_imu_temp_alarm_max | 600 | -550 | 1250 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | +| osd_imu_temp_alarm_min | -200 | -550 | 1250 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) | +| osd_left_sidebar_scroll | NONE | | | | +| osd_left_sidebar_scroll_step | 0 | | 255 | How many units each sidebar step represents. 0 means the default value for the scroll type. | +| osd_link_quality_alarm | 70 | 0 | 100 | LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50% | +| osd_main_voltage_decimals | 1 | 1 | 2 | Number of decimals for the battery voltages displayed in the OSD [1-2]. | +| osd_neg_alt_alarm | 5 | 0 | 10000 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) | +| osd_pan_servo_index | 0 | 0 | 10 | Index of the pan servo to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos. | +| osd_pan_servo_pwm2centideg | 0 | -36 | 36 | Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. | +| osd_plus_code_digits | 11 | 10 | 13 | Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm. | +| osd_plus_code_short | 0 | | | Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requires a reference location within, respectively, ~800km, ~40 km and ~2km to recover the original coordinates. | +| osd_right_sidebar_scroll | NONE | | | | +| osd_right_sidebar_scroll_step | 0 | | 255 | Same as left_sidebar_scroll_step, but for the right sidebar | +| osd_row_shiftdown | 0 | 0 | 1 | Number of rows to shift the OSD display (increase if top rows are cut off) | +| osd_rssi_alarm | 20 | 0 | 100 | Value below which to make the OSD RSSI indicator blink | +| osd_sidebar_horizontal_offset | 0 | -128 | 127 | Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. | +| osd_sidebar_scroll_arrows | OFF | | | | +| osd_snr_alarm | 4 | -20 | 10 | Value below which Crossfire SNR Alarm pops-up. (dB) | +| osd_stats_energy_unit | MAH | | | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) | +| osd_stats_min_voltage_unit | BATTERY | | | Display minimum voltage of the `BATTERY` or the average per `CELL` in the OSD stats. | +| osd_telemetry | OFF | | | To enable OSD telemetry for antenna tracker. Possible values are `OFF`, `ON` and `TEST` | +| osd_temp_label_align | LEFT | | | Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` | +| osd_time_alarm | 10 | 0 | 600 | Value above which to make the OSD flight time indicator blink (minutes) | +| osd_units | METRIC | | | IMPERIAL, METRIC, UK | +| osd_video_system | AUTO | | | Video system used. Possible values are `AUTO`, `PAL` and `NTSC` | +| pid_type | AUTO | | | Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` | +| pidsum_limit | 500 | PID_SUM_LIMIT_MIN | PID_SUM_LIMIT_MAX | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | +| pidsum_limit_yaw | 350 | PID_SUM_LIMIT_MIN | PID_SUM_LIMIT_MAX | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | +| pinio_box1 | `BOX_PERMANENT_ID_NONE` | 0 | 255 | Mode assignment for PINIO#1 | +| pinio_box2 | `BOX_PERMANENT_ID_NONE` | 0 | 255 | Mode assignment for PINIO#1 | +| pinio_box3 | `BOX_PERMANENT_ID_NONE` | 0 | 255 | Mode assignment for PINIO#1 | +| pinio_box4 | `BOX_PERMANENT_ID_NONE` | 0 | 255 | Mode assignment for PINIO#1 | +| pitch_rate | 20 | 6 | 180 | Defines rotation rate on PITCH axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | +| pitot_hardware | NONE | | | Selection of pitot hardware. | +| pitot_lpf_milli_hz | 350 | 0 | 10000 | | +| pitot_scale | 1.0 | 0 | 100 | | +| platform_type | MULTIROTOR | | | Defines UAV platform type. Allowed values: "MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented | +| pos_hold_deadband | 10 | 2 | 250 | Stick deadband in [r/c points], applied after r/c deadband and expo | +| prearm_timeout | 10000 | 0 | 10000 | Duration (ms) for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout. | +| rangefinder_hardware | NONE | | | Selection of rangefinder hardware. | +| rangefinder_median_filter | OFF | | | 3-point median filtering for rangefinder readouts | +| rate_accel_limit_roll_pitch | 0 | | 500000 | Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting. | +| rate_accel_limit_yaw | 10000 | | 500000 | Limits acceleration of YAW rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 180 dps^2). When set correctly, it greatly improves stopping performance and general stability during yaw turns. Value of 0 disables limiting. | +| rc_expo | 70 | 0 | 100 | Exposition value used for the PITCH/ROLL axes by all the stabilized flights modes (all but `MANUAL`) | +| rc_filter_frequency | 50 | 0 | 100 | RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values | +| rc_yaw_expo | 20 | 0 | 100 | Exposition value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`) | +| reboot_character | 82 | 48 | 126 | Special character used to trigger reboot | +| receiver_type | _target default_ | | | Selection of receiver (RX) type. Additional configuration of a `serialrx_provider` and a UART will be needed for `SERIAL` | +| report_cell_voltage | OFF | | | S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON | +| roll_rate | 20 | 6 | 180 | Defines rotation rate on ROLL axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | +| rpm_gyro_filter_enabled | OFF | | | Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV | +| rpm_gyro_harmonics | 1 | 1 | 3 | Number of harmonic frequences to be covered by gyro RPM filter. Default value of `1` usually works just fine | +| rpm_gyro_min_hz | 100 | 30 | 200 | The lowest frequency for gyro RPM filtere. Default `150` is fine for 5" mini-quads. On 7-inch drones you can lower even down to `60`-`70` | +| rpm_gyro_q | 500 | 1 | 3000 | Q factor for gyro RPM filter. Lower values give softer, wider attenuation. Usually there is no need to change this setting | +| rssi_adc_channel | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for analog RSSI input. Defaults to board RSSI input (if available). 0 = disabled | +| rssi_channel | 0 | 0 | MAX_SUPPORTED_RC_CHANNEL_COUNT | RX channel containing the RSSI signal | +| rssi_max | 100 | RSSI_VISIBLE_VALUE_MIN | RSSI_VISIBLE_VALUE_MAX | The maximum RSSI value sent by the receiver, in %. For example, if your receiver's maximum RSSI value shows as 83% in the configurator/OSD set this parameter to 83. See also rssi_min. | +| rssi_min | 0 | RSSI_VISIBLE_VALUE_MIN | RSSI_VISIBLE_VALUE_MAX | The minimum RSSI value sent by the receiver, in %. For example, if your receiver's minimum RSSI value shows as 42% in the configurator/OSD set this parameter to 42. See also rssi_max. Note that rssi_min can be set to a value bigger than rssi_max to invert the RSSI calculation (i.e. bigger values mean lower RSSI). | +| rssi_source | AUTO | | | Source of RSSI input. Possible values: `NONE`, `AUTO`, `ADC`, `CHANNEL`, `PROTOCOL`, `MSP` | +| rth_energy_margin | 5 | 0 | 100 | Energy margin wanted after getting home (percent of battery energy capacity). Use for the remaining flight time/distance calculation | +| rx_max_usec | 2115 | PWM_PULSE_MIN | PWM_PULSE_MAX | Defines the longest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of mid_rc. | +| rx_min_usec | 885 | PWM_PULSE_MIN | PWM_PULSE_MAX | Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of mid_rc. | +| rx_spi_id | 0 | 0 | 0 | | +| rx_spi_protocol | _target default_ | | | | +| rx_spi_rf_channel_count | 0 | 0 | 8 | | +| safehome_max_distance | 20000 | 0 | 65000 | In order for a safehome to be used, it must be less than this distance (in cm) from the arming point. | +| safehome_usage_mode | RTH | | | Used to control when safehomes will be used. Possible values are `OFF`, `RTH` and `RTH_FS`. See [Safehome documentation](Safehomes.md#Safehome) for more information. | +| sbus_sync_interval | 3000 | 500 | 10000 | | +| sdcard_detect_inverted | _target default_ | | | This setting drives the way SD card is detected in card slot. On some targets (AnyFC F7 clone) different card slot was used and depending of hardware revision ON or OFF setting might be required. If card is not detected, change this value. | +| serialrx_halfduplex | AUTO | | | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire. | +| serialrx_inverted | OFF | | | Reverse the serial inversion of the serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). | +| serialrx_provider | _target default_ | | | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. | +| servo_autotrim_rotation_limit | 15 | 1 | 60 | Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`. | +| servo_center_pulse | 1500 | PWM_RANGE_MIN | PWM_RANGE_MAX | Servo midpoint | +| servo_lpf_hz | 20 | 0 | 400 | Selects the servo PWM output cutoff frequency. Value is in [Hz] | +| servo_protocol | PWM | | | An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port) | +| servo_pwm_rate | 50 | 50 | 498 | Output frequency (in Hz) servo pins. When using tricopters or gimbal with digital servo, this rate can be increased. Max of 498Hz (for 500Hz pwm period), and min of 50Hz. Most digital servos will support for example 330Hz. | +| setpoint_kalman_enabled | OFF | | | Enable Kalman filter on the PID controller setpoint | +| setpoint_kalman_q | 100 | 1 | 16000 | Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds | +| setpoint_kalman_sharpness | 100 | 1 | 16000 | Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets | +| setpoint_kalman_w | 4 | 1 | 40 | Window size for the setpoint Kalman filter. Wider the window, more samples are used to compute variance. In general, wider window results in smoother filter response | +| sim_ground_station_number | _empty_ | | | Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module. | +| sim_low_altitude | -32767 | -32768 | 32767 | Threshold for low altitude warning messages sent by SIM module when the 'L' transmit flag is set in `sim_transmit_flags`. | +| sim_pin | 0000 | | | PIN code for the SIM module | +| sim_transmit_flags | `SIM_TX_FLAG_FAILSAFE` | | 63 | Bitmask specifying text message transmit condition flags for the SIM module. 1: continuous transmission, 2: continuous transmission in failsafe mode, 4: continuous transmission when GPS signal quality is low, 8: acceleration events, 16: continuous transmission when altitude is below `sim_low_altitude` | +| sim_transmit_interval | 60 | SIM_MIN_TRANSMIT_INTERVAL | 65535 | Text message transmission interval in seconds for SIM module. Minimum value: 10 | +| small_angle | 25 | 0 | 180 | If the aircraft tilt angle exceed this value the copter will refuse to arm. | +| smartport_fuel_unit | MAH | | | S.Port telemetry only: Unit of the value sent with the `FUEL` ID (FrSky D-Series always sends percent). [PERCENT/MAH/MWH] | +| smartport_master_halfduplex | ON | | | | +| smartport_master_inverted | OFF | | | | +| smith_predictor_delay | 0 | 0 | 8 | Expected delay of the gyro signal. In milliseconds | +| smith_predictor_lpf_hz | 50 | 1 | 500 | Cutoff frequency for the Smith Predictor Low Pass Filter | +| smith_predictor_strength | 0.5 | 0 | 1 | The strength factor of a Smith Predictor of PID measurement. In percents | +| spektrum_sat_bind | `SPEKTRUM_SAT_BIND_DISABLED` | SPEKTRUM_SAT_BIND_DISABLED | SPEKTRUM_SAT_BIND_MAX | 0 = disabled. Used to bind the spektrum satellite to RX | +| srxl2_baud_fast | ON | | | | +| srxl2_unit_id | 1 | 0 | 15 | | +| stats | OFF | | | General switch of the statistics recording feature (a.k.a. odometer) | +| stats_total_dist | 0 | | 2147483647 | Total flight distance [in meters]. The value is updated on every disarm when "stats" are enabled. | +| stats_total_energy | 0 | | 2147483647 | | +| stats_total_time | 0 | | 2147483647 | Total flight time [in seconds]. The value is updated on every disarm when "stats" are enabled. | +| switch_disarm_delay | 250 | 0 | 1000 | Delay before disarming when requested by switch (ms) [0-1000] | +| telemetry_halfduplex | ON | | | S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details | +| telemetry_inverted | OFF | | | Determines if the telemetry protocol default signal inversion is reversed. This should be OFF in most cases unless a custom or hacked RX is used. | +| telemetry_switch | OFF | | | Which aux channel to use to change serial output & baud rate (MSP / Telemetry). It disables automatic switching to Telemetry when armed. | +| thr_comp_weight | 1 | 0 | 2 | Weight used for the throttle compensation based on battery voltage. See the [battery documentation](Battery.md#automatic-throttle-compensation-based-on-battery-voltage) | +| thr_expo | 0 | 0 | 100 | Throttle exposition value | +| thr_mid | 50 | 0 | 100 | Throttle value when the stick is set to mid-position. Used in the throttle curve calculation. | +| throttle_idle | 15 | 0 | 30 | The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle. | +| throttle_scale | 1.0 | 0 | 1 | Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50% | +| throttle_tilt_comp_str | 0 | 0 | 100 | Can be used in ANGLE and HORIZON mode and will automatically boost throttle when banking. Setting is in percentage, 0=disabled. | +| tpa_breakpoint | 1500 | PWM_RANGE_MIN | PWM_RANGE_MAX | See tpa_rate. | +| tpa_rate | 0 | 0 | 100 | Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. | +| tri_unarmed_servo | ON | | | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. | +| tz_automatic_dst | OFF | | | Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`. | +| tz_offset | 0 | -1440 | 1440 | Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs | +| vbat_adc_channel | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled | +| vbat_cell_detect_voltage | 425 | 100 | 500 | Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units. | +| vbat_max_cell_voltage | 420 | 100 | 500 | Maximum voltage per cell in 0.01V units, default is 4.20V | +| vbat_meter_type | ADC | | | Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running | +| vbat_min_cell_voltage | 330 | 100 | 500 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) | +| vbat_scale | _target default_ | VBAT_SCALE_MIN | VBAT_SCALE_MAX | Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing "status" in cli. | +| vbat_warning_cell_voltage | 350 | 100 | 500 | Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) | +| vtx_band | 4 | VTX_SETTINGS_NO_BAND | VTX_SETTINGS_MAX_BAND | Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. | +| vtx_channel | 1 | VTX_SETTINGS_MIN_CHANNEL | VTX_SETTINGS_MAX_CHANNEL | Channel to use within the configured `vtx_band`. Valid values are [1, 8]. | +| vtx_halfduplex | ON | | | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | +| vtx_low_power_disarm | OFF | | | When the craft is disarmed, set the VTX to its lowest power. `ON` will set the power to its minimum value on startup, increase it to `vtx_power` when arming and change it back to its lowest setting after disarming. `UNTIL_FIRST_ARM` will start with minimum power, but once the craft is armed it will increase to `vtx_power` and it will never decrease until the craft is power cycled. | +| vtx_max_power_override | 0 | 0 | 10000 | Some VTXes may report max power incorrectly (i.e. 200mW for a 600mW VTX). Use this to override max supported power. 0 to disable and use whatever VTX reports as its capabilities | +| vtx_pit_mode_chan | 1 | VTX_SETTINGS_MIN_CHANNEL | VTX_SETTINGS_MAX_CHANNEL | | +| vtx_power | 1 | VTX_SETTINGS_MIN_POWER | VTX_SETTINGS_MAX_POWER | VTX RF power level to use. The exact number of mw depends on the VTX hardware. | +| vtx_smartaudio_early_akk_workaround | ON | | | Enable workaround for early AKK SAudio-enabled VTX bug. | +| yaw_deadband | 5 | 0 | 100 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. | +| yaw_lpf_hz | 0 | 0 | 200 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | +| yaw_rate | 20 | 2 | 180 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | > Note: this table is autogenerated. Do not edit it manually. \ No newline at end of file diff --git a/docs/assets/images/StickPositions.png b/docs/assets/images/StickPositions.png index 2d45981c2a..98281caeef 100644 Binary files a/docs/assets/images/StickPositions.png and b/docs/assets/images/StickPositions.png differ diff --git a/docs/assets/images/StickPositions_trans.png b/docs/assets/images/StickPositions_trans.png new file mode 100644 index 0000000000..2d45981c2a Binary files /dev/null and b/docs/assets/images/StickPositions_trans.png differ diff --git a/lib/main/MAVLink/.gitignore b/lib/main/MAVLink/.gitignore new file mode 100644 index 0000000000..debff4b898 --- /dev/null +++ b/lib/main/MAVLink/.gitignore @@ -0,0 +1 @@ +mavlink-src/ diff --git a/lib/main/MAVLink/README.md b/lib/main/MAVLink/README.md new file mode 100644 index 0000000000..51e912f99c --- /dev/null +++ b/lib/main/MAVLink/README.md @@ -0,0 +1,13 @@ +# MAVLink Generator (`mavgen`) + +This directory contains the MAVLink library and scripts to automatically generate it from MAVLink definitions. + +In order to run it, you will need [Python and some other libraries installed](https://mavlink.io/en/getting_started/installation.html). + +Then, run the appropriate script (`generate.sh` for Linux, `generate.bat` for Windows) to automatically re-generate the library. + +## IMPORTANT NOTE + +By default, the MAVLink library declares all of its functions as `inline`, which results in common functions being duplicated many times. +After generating the library, `protocol.h` must be modified, and all `inline` keywords removed. +This is performed automatically by `generate.sh`, but not by `generate.bat` as Windows batch files do not have an equivalent to `sed`. So, this must be done manually on Windows. diff --git a/lib/main/MAVLink/checksum.h b/lib/main/MAVLink/checksum.h index 0f30b659fa..0a899a6077 100755 --- a/lib/main/MAVLink/checksum.h +++ b/lib/main/MAVLink/checksum.h @@ -1,10 +1,11 @@ -#ifdef __cplusplus +#pragma once + +#if defined(MAVLINK_USE_CXX_NAMESPACE) +namespace mavlink { +#elif defined(__cplusplus) extern "C" { #endif -#ifndef _CHECKSUM_H_ -#define _CHECKSUM_H_ - // Visual Studio versions before 2010 don't have stdint.h, so we just error out. #if (defined _MSC_VER) && (_MSC_VER < 1600) #error "The C-MAVLink implementation requires Visual Studio 2010 or greater" @@ -23,7 +24,7 @@ extern "C" { #ifndef HAVE_CRC_ACCUMULATE /** - * @brief Accumulate the X.25 CRC by adding one char at a time. + * @brief Accumulate the CRC16_MCRF4XX checksum by adding one char at a time. * * The checksum function adds the hash of one char at a time to the * 16 bit checksum (uint16_t). @@ -44,9 +45,9 @@ static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) /** - * @brief Initiliaze the buffer for the X.25 CRC + * @brief Initiliaze the buffer for the MCRF4XX CRC16 * - * @param crcAccum the 16 bit X.25 CRC + * @param crcAccum the 16 bit MCRF4XX CRC16 */ static inline void crc_init(uint16_t* crcAccum) { @@ -55,7 +56,7 @@ static inline void crc_init(uint16_t* crcAccum) /** - * @brief Calculates the X.25 checksum on a byte buffer + * @brief Calculates the CRC16_MCRF4XX checksum on a byte buffer * * @param pBuffer buffer containing the byte array to hash * @param length length of the byte array @@ -73,7 +74,7 @@ static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length) /** - * @brief Accumulate the X.25 CRC by adding an array of bytes + * @brief Accumulate the MCRF4XX CRC16 by adding an array of bytes * * The checksum function adds the hash of one char at a time to the * 16 bit checksum (uint16_t). @@ -89,8 +90,6 @@ static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer } } -#endif /* _CHECKSUM_H_ */ - -#ifdef __cplusplus +#if defined(MAVLINK_USE_CXX_NAMESPACE) || defined(__cplusplus) } #endif diff --git a/lib/main/MAVLink/common/common.h b/lib/main/MAVLink/common/common.h index 1014966516..55200c5a84 100755 --- a/lib/main/MAVLink/common/common.h +++ b/lib/main/MAVLink/common/common.h @@ -20,11 +20,11 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 36, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 24, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 37, 4, 7, 0, 27, 25, 0, 0, 0, 0, 0, 72, 26, 181, 225, 42, 6, 4, 0, 11, 18, 0, 0, 37, 20, 35, 33, 3, 0, 0, 4, 22, 39, 37, 53, 51, 53, 51, 0, 28, 56, 42, 33, 81, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 44, 64, 84, 9, 254, 16, 12, 36, 44, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 35, 35, 22, 13, 255, 14, 18, 43, 8, 22, 14, 36, 43, 41, 32, 243, 14, 93, 0, 100, 36, 60, 30, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 40, 63, 182, 40, 42, 0, 0, 0, 0, 0, 32, 52, 53, 6, 2, 38, 19, 254, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 117, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 137, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 78, 196, 132, 0, 15, 3, 0, 0, 0, 0, 0, 167, 183, 119, 191, 118, 148, 21, 0, 243, 124, 0, 0, 38, 20, 158, 152, 143, 0, 0, 14, 106, 49, 22, 143, 140, 5, 150, 0, 231, 183, 63, 54, 47, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 138, 108, 32, 185, 84, 34, 174, 124, 237, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 25, 226, 46, 29, 223, 85, 6, 229, 203, 1, 195, 109, 168, 181, 47, 72, 131, 127, 0, 103, 154, 178, 200, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 189, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 163, 105, 151, 35, 150, 179, 0, 0, 0, 0, 0, 90, 104, 85, 95, 130, 184, 81, 8, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {{0, 50, 9, 9, 0, 0, 0}, {1, 124, 31, 31, 0, 0, 0}, {2, 137, 12, 12, 0, 0, 0}, {4, 237, 14, 14, 3, 12, 13}, {5, 217, 28, 28, 1, 0, 0}, {6, 104, 3, 3, 0, 0, 0}, {7, 119, 32, 32, 0, 0, 0}, {8, 117, 36, 36, 0, 0, 0}, {11, 89, 6, 6, 1, 4, 0}, {19, 137, 24, 24, 3, 4, 5}, {20, 214, 20, 20, 3, 2, 3}, {21, 159, 2, 2, 3, 0, 1}, {22, 220, 25, 25, 0, 0, 0}, {23, 168, 23, 23, 3, 4, 5}, {24, 24, 30, 52, 0, 0, 0}, {25, 23, 101, 101, 0, 0, 0}, {26, 170, 22, 24, 0, 0, 0}, {27, 144, 26, 29, 0, 0, 0}, {28, 67, 16, 16, 0, 0, 0}, {29, 115, 14, 16, 0, 0, 0}, {30, 39, 28, 28, 0, 0, 0}, {31, 246, 32, 48, 0, 0, 0}, {32, 185, 28, 28, 0, 0, 0}, {33, 104, 28, 28, 0, 0, 0}, {34, 237, 22, 22, 0, 0, 0}, {35, 244, 22, 22, 0, 0, 0}, {36, 222, 21, 37, 0, 0, 0}, {37, 212, 6, 7, 3, 4, 5}, {38, 9, 6, 7, 3, 4, 5}, {39, 254, 37, 38, 3, 32, 33}, {40, 230, 4, 5, 3, 2, 3}, {41, 28, 4, 4, 3, 2, 3}, {42, 28, 2, 2, 0, 0, 0}, {43, 132, 2, 3, 3, 0, 1}, {44, 221, 4, 5, 3, 2, 3}, {45, 232, 2, 3, 3, 0, 1}, {46, 11, 2, 2, 0, 0, 0}, {47, 153, 3, 4, 3, 0, 1}, {48, 41, 13, 21, 1, 12, 0}, {49, 39, 12, 20, 0, 0, 0}, {50, 78, 37, 37, 3, 18, 19}, {51, 196, 4, 5, 3, 2, 3}, {52, 132, 7, 7, 0, 0, 0}, {54, 15, 27, 27, 3, 24, 25}, {55, 3, 25, 25, 0, 0, 0}, {61, 167, 72, 72, 0, 0, 0}, {62, 183, 26, 26, 0, 0, 0}, {63, 119, 181, 181, 0, 0, 0}, {64, 191, 225, 225, 0, 0, 0}, {65, 118, 42, 42, 0, 0, 0}, {66, 148, 6, 6, 3, 2, 3}, {67, 21, 4, 4, 0, 0, 0}, {69, 243, 11, 11, 1, 10, 0}, {70, 124, 18, 38, 3, 16, 17}, {73, 38, 37, 38, 3, 32, 33}, {74, 20, 20, 20, 0, 0, 0}, {75, 158, 35, 35, 3, 30, 31}, {76, 152, 33, 33, 3, 30, 31}, {77, 143, 3, 10, 3, 8, 9}, {80, 14, 4, 4, 3, 2, 3}, {81, 106, 22, 22, 0, 0, 0}, {82, 49, 39, 39, 3, 36, 37}, {83, 22, 37, 37, 0, 0, 0}, {84, 143, 53, 53, 3, 50, 51}, {85, 140, 51, 51, 0, 0, 0}, {86, 5, 53, 53, 3, 50, 51}, {87, 150, 51, 51, 0, 0, 0}, {89, 231, 28, 28, 0, 0, 0}, {90, 183, 56, 56, 0, 0, 0}, {91, 63, 42, 42, 0, 0, 0}, {92, 54, 33, 33, 0, 0, 0}, {93, 47, 81, 81, 0, 0, 0}, {100, 175, 26, 34, 0, 0, 0}, {101, 102, 32, 117, 0, 0, 0}, {102, 158, 32, 117, 0, 0, 0}, {103, 208, 20, 57, 0, 0, 0}, {104, 56, 32, 116, 0, 0, 0}, {105, 93, 62, 63, 0, 0, 0}, {106, 138, 44, 44, 0, 0, 0}, {107, 108, 64, 65, 0, 0, 0}, {108, 32, 84, 84, 0, 0, 0}, {109, 185, 9, 9, 0, 0, 0}, {110, 84, 254, 254, 3, 1, 2}, {111, 34, 16, 16, 0, 0, 0}, {112, 174, 12, 12, 0, 0, 0}, {113, 124, 36, 39, 0, 0, 0}, {114, 237, 44, 44, 0, 0, 0}, {115, 4, 64, 64, 0, 0, 0}, {116, 76, 22, 24, 0, 0, 0}, {117, 128, 6, 6, 3, 4, 5}, {118, 56, 14, 14, 0, 0, 0}, {119, 116, 12, 12, 3, 10, 11}, {120, 134, 97, 97, 0, 0, 0}, {121, 237, 2, 2, 3, 0, 1}, {122, 203, 2, 2, 3, 0, 1}, {123, 250, 113, 113, 3, 0, 1}, {124, 87, 35, 37, 0, 0, 0}, {125, 203, 6, 6, 0, 0, 0}, {126, 220, 79, 79, 0, 0, 0}, {127, 25, 35, 35, 0, 0, 0}, {128, 226, 35, 35, 0, 0, 0}, {129, 46, 22, 24, 0, 0, 0}, {130, 29, 13, 13, 0, 0, 0}, {131, 223, 255, 255, 0, 0, 0}, {132, 85, 14, 39, 0, 0, 0}, {133, 6, 18, 18, 0, 0, 0}, {134, 229, 43, 43, 0, 0, 0}, {135, 203, 8, 8, 0, 0, 0}, {136, 1, 22, 22, 0, 0, 0}, {137, 195, 14, 16, 0, 0, 0}, {138, 109, 36, 120, 0, 0, 0}, {139, 168, 43, 43, 3, 41, 42}, {140, 181, 41, 41, 0, 0, 0}, {141, 47, 32, 32, 0, 0, 0}, {142, 72, 243, 243, 0, 0, 0}, {143, 131, 14, 16, 0, 0, 0}, {144, 127, 93, 93, 0, 0, 0}, {146, 103, 100, 100, 0, 0, 0}, {147, 154, 36, 54, 0, 0, 0}, {148, 178, 60, 78, 0, 0, 0}, {149, 200, 30, 60, 0, 0, 0}, {162, 189, 8, 9, 0, 0, 0}, {192, 36, 44, 54, 0, 0, 0}, {225, 208, 65, 65, 0, 0, 0}, {230, 163, 42, 42, 0, 0, 0}, {231, 105, 40, 40, 0, 0, 0}, {232, 151, 63, 65, 0, 0, 0}, {233, 35, 182, 182, 0, 0, 0}, {234, 150, 40, 40, 0, 0, 0}, {235, 179, 42, 42, 0, 0, 0}, {241, 90, 32, 32, 0, 0, 0}, {242, 104, 52, 60, 0, 0, 0}, {243, 85, 53, 61, 1, 52, 0}, {244, 95, 6, 6, 0, 0, 0}, {245, 130, 2, 2, 0, 0, 0}, {246, 184, 38, 38, 0, 0, 0}, {247, 81, 19, 19, 0, 0, 0}, {248, 8, 254, 254, 3, 3, 4}, {249, 204, 36, 36, 0, 0, 0}, {250, 49, 30, 30, 0, 0, 0}, {251, 170, 18, 18, 0, 0, 0}, {252, 44, 18, 18, 0, 0, 0}, {253, 83, 51, 54, 0, 0, 0}, {254, 46, 9, 9, 0, 0, 0}, {256, 71, 42, 42, 3, 8, 9}, {257, 131, 9, 9, 0, 0, 0}, {258, 187, 32, 232, 3, 0, 1}, {259, 92, 235, 235, 0, 0, 0}, {260, 146, 5, 13, 0, 0, 0}, {261, 179, 27, 60, 0, 0, 0}, {262, 12, 18, 22, 0, 0, 0}, {263, 133, 255, 255, 0, 0, 0}, {264, 49, 28, 28, 0, 0, 0}, {265, 26, 16, 20, 0, 0, 0}, {266, 193, 255, 255, 3, 2, 3}, {267, 35, 255, 255, 3, 2, 3}, {268, 14, 4, 4, 3, 2, 3}, {269, 109, 213, 213, 0, 0, 0}, {270, 59, 19, 19, 0, 0, 0}, {271, 22, 52, 52, 0, 0, 0}, {275, 126, 31, 31, 0, 0, 0}, {276, 18, 49, 49, 0, 0, 0}, {280, 70, 33, 33, 0, 0, 0}, {281, 48, 13, 13, 0, 0, 0}, {282, 123, 35, 35, 3, 32, 33}, {283, 74, 144, 144, 0, 0, 0}, {284, 99, 32, 32, 3, 30, 31}, {285, 137, 40, 40, 3, 38, 39}, {286, 210, 53, 53, 3, 50, 51}, {287, 1, 23, 23, 3, 20, 21}, {288, 20, 23, 23, 3, 20, 21}, {290, 221, 42, 42, 0, 0, 0}, {291, 10, 57, 57, 0, 0, 0}, {299, 19, 96, 98, 0, 0, 0}, {300, 217, 22, 22, 0, 0, 0}, {301, 243, 58, 58, 0, 0, 0}, {310, 28, 17, 17, 0, 0, 0}, {311, 95, 116, 116, 0, 0, 0}, {320, 243, 20, 20, 3, 2, 3}, {321, 88, 2, 2, 3, 0, 1}, {322, 243, 149, 149, 0, 0, 0}, {323, 78, 147, 147, 3, 0, 1}, {324, 132, 146, 146, 0, 0, 0}, {330, 23, 158, 167, 0, 0, 0}, {331, 91, 230, 232, 0, 0, 0}, {332, 236, 239, 239, 0, 0, 0}, {333, 231, 109, 109, 0, 0, 0}, {334, 72, 10, 10, 0, 0, 0}, {335, 225, 24, 24, 0, 0, 0}, {336, 245, 84, 84, 0, 0, 0}, {339, 199, 5, 5, 0, 0, 0}, {340, 99, 70, 70, 0, 0, 0}, {350, 232, 20, 252, 0, 0, 0}, {360, 11, 25, 25, 0, 0, 0}, {370, 75, 87, 87, 0, 0, 0}, {373, 117, 42, 42, 0, 0, 0}, {375, 251, 140, 140, 0, 0, 0}, {380, 232, 20, 20, 0, 0, 0}, {385, 147, 133, 133, 3, 2, 3}, {390, 156, 238, 238, 0, 0, 0}, {395, 163, 156, 156, 0, 0, 0}, {400, 110, 254, 254, 3, 4, 5}, {401, 183, 6, 6, 3, 4, 5}, {9000, 113, 137, 137, 0, 0, 0}, {9005, 117, 34, 34, 0, 0, 0}, {12900, 114, 44, 44, 3, 0, 1}, {12901, 254, 59, 59, 3, 30, 31}, {12902, 49, 53, 53, 3, 4, 5}, {12903, 249, 46, 46, 3, 0, 1}, {12904, 203, 46, 46, 3, 20, 21}, {12905, 49, 43, 43, 3, 0, 1}, {12915, 62, 254, 254, 3, 0, 1}} #endif #include "../protocol.h" @@ -34,79 +34,6 @@ extern "C" { // ENUM DEFINITIONS -/** @brief Micro air vehicle / autopilot classes. This identifies the individual model. */ -#ifndef HAVE_ENUM_MAV_AUTOPILOT -#define HAVE_ENUM_MAV_AUTOPILOT -typedef enum MAV_AUTOPILOT -{ - MAV_AUTOPILOT_GENERIC=0, /* Generic autopilot, full support for everything | */ - MAV_AUTOPILOT_RESERVED=1, /* Reserved for future use. | */ - MAV_AUTOPILOT_SLUGS=2, /* SLUGS autopilot, http://slugsuav.soe.ucsc.edu | */ - MAV_AUTOPILOT_ARDUPILOTMEGA=3, /* ArduPilot - Plane/Copter/Rover/Sub/Tracker, https://ardupilot.org | */ - MAV_AUTOPILOT_OPENPILOT=4, /* OpenPilot, http://openpilot.org | */ - MAV_AUTOPILOT_GENERIC_WAYPOINTS_ONLY=5, /* Generic autopilot only supporting simple waypoints | */ - MAV_AUTOPILOT_GENERIC_WAYPOINTS_AND_SIMPLE_NAVIGATION_ONLY=6, /* Generic autopilot supporting waypoints and other simple navigation commands | */ - MAV_AUTOPILOT_GENERIC_MISSION_FULL=7, /* Generic autopilot supporting the full mission command set | */ - MAV_AUTOPILOT_INVALID=8, /* No valid autopilot, e.g. a GCS or other MAVLink component | */ - MAV_AUTOPILOT_PPZ=9, /* PPZ UAV - http://nongnu.org/paparazzi | */ - MAV_AUTOPILOT_UDB=10, /* UAV Dev Board | */ - MAV_AUTOPILOT_FP=11, /* FlexiPilot | */ - MAV_AUTOPILOT_PX4=12, /* PX4 Autopilot - http://px4.io/ | */ - MAV_AUTOPILOT_SMACCMPILOT=13, /* SMACCMPilot - http://smaccmpilot.org | */ - MAV_AUTOPILOT_AUTOQUAD=14, /* AutoQuad -- http://autoquad.org | */ - MAV_AUTOPILOT_ARMAZILA=15, /* Armazila -- http://armazila.com | */ - MAV_AUTOPILOT_AEROB=16, /* Aerob -- http://aerob.ru | */ - MAV_AUTOPILOT_ASLUAV=17, /* ASLUAV autopilot -- http://www.asl.ethz.ch | */ - MAV_AUTOPILOT_SMARTAP=18, /* SmartAP Autopilot - http://sky-drones.com | */ - MAV_AUTOPILOT_AIRRAILS=19, /* AirRails - http://uaventure.com | */ - MAV_AUTOPILOT_ENUM_END=20, /* | */ -} MAV_AUTOPILOT; -#endif - -/** @brief MAVLINK component type reported in HEARTBEAT message. Flight controllers must report the type of the vehicle on which they are mounted (e.g. MAV_TYPE_OCTOROTOR). All other components must report a value appropriate for their type (e.g. a camera must use MAV_TYPE_CAMERA). */ -#ifndef HAVE_ENUM_MAV_TYPE -#define HAVE_ENUM_MAV_TYPE -typedef enum MAV_TYPE -{ - MAV_TYPE_GENERIC=0, /* Generic micro air vehicle | */ - MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ - MAV_TYPE_QUADROTOR=2, /* Quadrotor | */ - MAV_TYPE_COAXIAL=3, /* Coaxial helicopter | */ - MAV_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ - MAV_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */ - MAV_TYPE_GCS=6, /* Operator control unit / ground control station | */ - MAV_TYPE_AIRSHIP=7, /* Airship, controlled | */ - MAV_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ - MAV_TYPE_ROCKET=9, /* Rocket | */ - MAV_TYPE_GROUND_ROVER=10, /* Ground rover | */ - MAV_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ - MAV_TYPE_SUBMARINE=12, /* Submarine | */ - MAV_TYPE_HEXAROTOR=13, /* Hexarotor | */ - MAV_TYPE_OCTOROTOR=14, /* Octorotor | */ - MAV_TYPE_TRICOPTER=15, /* Tricopter | */ - MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */ - MAV_TYPE_KITE=17, /* Kite | */ - MAV_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */ - MAV_TYPE_VTOL_DUOROTOR=19, /* Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter. | */ - MAV_TYPE_VTOL_QUADROTOR=20, /* Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter. | */ - MAV_TYPE_VTOL_TILTROTOR=21, /* Tiltrotor VTOL | */ - MAV_TYPE_VTOL_RESERVED2=22, /* VTOL reserved 2 | */ - MAV_TYPE_VTOL_RESERVED3=23, /* VTOL reserved 3 | */ - MAV_TYPE_VTOL_RESERVED4=24, /* VTOL reserved 4 | */ - MAV_TYPE_VTOL_RESERVED5=25, /* VTOL reserved 5 | */ - MAV_TYPE_GIMBAL=26, /* Gimbal | */ - MAV_TYPE_ADSB=27, /* ADSB system | */ - MAV_TYPE_PARAFOIL=28, /* Steerable, nonrigid airfoil | */ - MAV_TYPE_DODECAROTOR=29, /* Dodecarotor | */ - MAV_TYPE_CAMERA=30, /* Camera | */ - MAV_TYPE_CHARGING_STATION=31, /* Charging station | */ - MAV_TYPE_FLARM=32, /* FLARM collision avoidance system | */ - MAV_TYPE_SERVO=33, /* Servo | */ - MAV_TYPE_ODID=34, /* Open Drone ID. See https://mavlink.io/en/services/opendroneid.html. | */ - MAV_TYPE_ENUM_END=35, /* | */ -} MAV_TYPE; -#endif - /** @brief These values define the type of firmware release. These values indicate the first version or release of this type. For example the first alpha release would be 64, the second would be 65. */ #ifndef HAVE_ENUM_FIRMWARE_VERSION_TYPE #define HAVE_ENUM_FIRMWARE_VERSION_TYPE @@ -144,40 +71,6 @@ typedef enum HL_FAILURE_FLAG } HL_FAILURE_FLAG; #endif -/** @brief These flags encode the MAV mode. */ -#ifndef HAVE_ENUM_MAV_MODE_FLAG -#define HAVE_ENUM_MAV_MODE_FLAG -typedef enum MAV_MODE_FLAG -{ - MAV_MODE_FLAG_CUSTOM_MODE_ENABLED=1, /* 0b00000001 Reserved for future use. | */ - MAV_MODE_FLAG_TEST_ENABLED=2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ - MAV_MODE_FLAG_AUTO_ENABLED=4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ - MAV_MODE_FLAG_GUIDED_ENABLED=8, /* 0b00001000 guided mode enabled, system flies waypoints / mission items. | */ - MAV_MODE_FLAG_STABILIZE_ENABLED=16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ - MAV_MODE_FLAG_HIL_ENABLED=32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ - MAV_MODE_FLAG_MANUAL_INPUT_ENABLED=64, /* 0b01000000 remote control input is enabled. | */ - MAV_MODE_FLAG_SAFETY_ARMED=128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */ - MAV_MODE_FLAG_ENUM_END=129, /* | */ -} MAV_MODE_FLAG; -#endif - -/** @brief These values encode the bit positions of the decode position. These values can be used to read the value of a flag bit by combining the base_mode variable with AND with the flag position value. The result will be either 0 or 1, depending on if the flag is set or not. */ -#ifndef HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION -#define HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION -typedef enum MAV_MODE_FLAG_DECODE_POSITION -{ - MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE=1, /* Eighth bit: 00000001 | */ - MAV_MODE_FLAG_DECODE_POSITION_TEST=2, /* Seventh bit: 00000010 | */ - MAV_MODE_FLAG_DECODE_POSITION_AUTO=4, /* Sixth bit: 00000100 | */ - MAV_MODE_FLAG_DECODE_POSITION_GUIDED=8, /* Fifth bit: 00001000 | */ - MAV_MODE_FLAG_DECODE_POSITION_STABILIZE=16, /* Fourth bit: 00010000 | */ - MAV_MODE_FLAG_DECODE_POSITION_HIL=32, /* Third bit: 00100000 | */ - MAV_MODE_FLAG_DECODE_POSITION_MANUAL=64, /* Second bit: 01000000 | */ - MAV_MODE_FLAG_DECODE_POSITION_SAFETY=128, /* First bit: 10000000 | */ - MAV_MODE_FLAG_DECODE_POSITION_ENUM_END=129, /* | */ -} MAV_MODE_FLAG_DECODE_POSITION; -#endif - /** @brief Actions that may be specified in MAV_CMD_OVERRIDE_GOTO to override mission execution. */ #ifndef HAVE_ENUM_MAV_GOTO #define HAVE_ENUM_MAV_GOTO @@ -212,162 +105,6 @@ typedef enum MAV_MODE } MAV_MODE; #endif -/** @brief */ -#ifndef HAVE_ENUM_MAV_STATE -#define HAVE_ENUM_MAV_STATE -typedef enum MAV_STATE -{ - MAV_STATE_UNINIT=0, /* Uninitialized system, state is unknown. | */ - MAV_STATE_BOOT=1, /* System is booting up. | */ - MAV_STATE_CALIBRATING=2, /* System is calibrating and not flight-ready. | */ - MAV_STATE_STANDBY=3, /* System is grounded and on standby. It can be launched any time. | */ - MAV_STATE_ACTIVE=4, /* System is active and might be already airborne. Motors are engaged. | */ - MAV_STATE_CRITICAL=5, /* System is in a non-normal flight mode. It can however still navigate. | */ - MAV_STATE_EMERGENCY=6, /* System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. | */ - MAV_STATE_POWEROFF=7, /* System just initialized its power-down sequence, will shut down now. | */ - MAV_STATE_FLIGHT_TERMINATION=8, /* System is terminating itself. | */ - MAV_STATE_ENUM_END=9, /* | */ -} MAV_STATE; -#endif - -/** @brief Component ids (values) for the different types and instances of onboard hardware/software that might make up a MAVLink system (autopilot, cameras, servos, GPS systems, avoidance systems etc.). - Components must use the appropriate ID in their source address when sending messages. Components can also use IDs to determine if they are the intended recipient of an incoming message. The MAV_COMP_ID_ALL value is used to indicate messages that must be processed by all components. - When creating new entries, components that can have multiple instances (e.g. cameras, servos etc.) should be allocated sequential values. An appropriate number of values should be left free after these components to allow the number of instances to be expanded. */ -#ifndef HAVE_ENUM_MAV_COMPONENT -#define HAVE_ENUM_MAV_COMPONENT -typedef enum MAV_COMPONENT -{ - MAV_COMP_ID_ALL=0, /* Target id (target_component) used to broadcast messages to all components of the receiving system. Components should attempt to process messages with this component ID and forward to components on any other interfaces. Note: This is not a valid *source* component id for a message. | */ - MAV_COMP_ID_AUTOPILOT1=1, /* System flight controller component ("autopilot"). Only one autopilot is expected in a particular system. | */ - MAV_COMP_ID_USER1=25, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER2=26, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER3=27, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER4=28, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER5=29, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER6=30, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER7=31, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER8=32, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER9=33, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER10=34, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER11=35, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER12=36, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER13=37, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER14=38, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER15=39, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER16=40, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER17=41, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER18=42, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER19=43, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER20=44, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER21=45, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER22=46, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER23=47, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER24=48, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER25=49, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER26=50, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER27=51, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER28=52, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER29=53, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER30=54, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER31=55, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER32=56, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER33=57, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER34=58, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER35=59, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER36=60, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER37=61, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER38=62, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER39=63, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER40=64, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER41=65, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER42=66, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER43=67, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_TELEMETRY_RADIO=68, /* Telemetry radio (e.g. SiK radio, or other component that emits RADIO_STATUS messages). | */ - MAV_COMP_ID_USER45=69, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER46=70, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER47=71, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER48=72, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER49=73, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER50=74, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER51=75, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER52=76, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER53=77, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER54=78, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER55=79, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER56=80, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER57=81, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER58=82, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER59=83, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER60=84, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER61=85, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER62=86, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER63=87, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER64=88, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER65=89, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER66=90, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER67=91, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER68=92, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER69=93, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER70=94, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER71=95, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER72=96, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER73=97, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER74=98, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER75=99, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_CAMERA=100, /* Camera #1. | */ - MAV_COMP_ID_CAMERA2=101, /* Camera #2. | */ - MAV_COMP_ID_CAMERA3=102, /* Camera #3. | */ - MAV_COMP_ID_CAMERA4=103, /* Camera #4. | */ - MAV_COMP_ID_CAMERA5=104, /* Camera #5. | */ - MAV_COMP_ID_CAMERA6=105, /* Camera #6. | */ - MAV_COMP_ID_SERVO1=140, /* Servo #1. | */ - MAV_COMP_ID_SERVO2=141, /* Servo #2. | */ - MAV_COMP_ID_SERVO3=142, /* Servo #3. | */ - MAV_COMP_ID_SERVO4=143, /* Servo #4. | */ - MAV_COMP_ID_SERVO5=144, /* Servo #5. | */ - MAV_COMP_ID_SERVO6=145, /* Servo #6. | */ - MAV_COMP_ID_SERVO7=146, /* Servo #7. | */ - MAV_COMP_ID_SERVO8=147, /* Servo #8. | */ - MAV_COMP_ID_SERVO9=148, /* Servo #9. | */ - MAV_COMP_ID_SERVO10=149, /* Servo #10. | */ - MAV_COMP_ID_SERVO11=150, /* Servo #11. | */ - MAV_COMP_ID_SERVO12=151, /* Servo #12. | */ - MAV_COMP_ID_SERVO13=152, /* Servo #13. | */ - MAV_COMP_ID_SERVO14=153, /* Servo #14. | */ - MAV_COMP_ID_GIMBAL=154, /* Gimbal #1. | */ - MAV_COMP_ID_LOG=155, /* Logging component. | */ - MAV_COMP_ID_ADSB=156, /* Automatic Dependent Surveillance-Broadcast (ADS-B) component. | */ - MAV_COMP_ID_OSD=157, /* On Screen Display (OSD) devices for video links. | */ - MAV_COMP_ID_PERIPHERAL=158, /* Generic autopilot peripheral component ID. Meant for devices that do not implement the parameter microservice. | */ - MAV_COMP_ID_QX1_GIMBAL=159, /* Gimbal ID for QX1. | */ - MAV_COMP_ID_FLARM=160, /* FLARM collision alert component. | */ - MAV_COMP_ID_GIMBAL2=171, /* Gimbal #2. | */ - MAV_COMP_ID_GIMBAL3=172, /* Gimbal #3. | */ - MAV_COMP_ID_GIMBAL4=173, /* Gimbal #4 | */ - MAV_COMP_ID_GIMBAL5=174, /* Gimbal #5. | */ - MAV_COMP_ID_GIMBAL6=175, /* Gimbal #6. | */ - MAV_COMP_ID_MISSIONPLANNER=190, /* Component that can generate/supply a mission flight plan (e.g. GCS or developer API). | */ - MAV_COMP_ID_ONBOARD_COMPUTER=191, /* Component that lives on the onboard computer (companion computer) and has some generic functionalities, such as settings system parameters and monitoring the status of some processes that don't directly speak mavlink and so on. | */ - MAV_COMP_ID_PATHPLANNER=195, /* Component that finds an optimal path between points based on a certain constraint (e.g. minimum snap, shortest path, cost, etc.). | */ - MAV_COMP_ID_OBSTACLE_AVOIDANCE=196, /* Component that plans a collision free path between two points. | */ - MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY=197, /* Component that provides position estimates using VIO techniques. | */ - MAV_COMP_ID_PAIRING_MANAGER=198, /* Component that manages pairing of vehicle and GCS. | */ - MAV_COMP_ID_IMU=200, /* Inertial Measurement Unit (IMU) #1. | */ - MAV_COMP_ID_IMU_2=201, /* Inertial Measurement Unit (IMU) #2. | */ - MAV_COMP_ID_IMU_3=202, /* Inertial Measurement Unit (IMU) #3. | */ - MAV_COMP_ID_GPS=220, /* GPS #1. | */ - MAV_COMP_ID_GPS2=221, /* GPS #2. | */ - MAV_COMP_ID_ODID_TXRX_1=236, /* Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). | */ - MAV_COMP_ID_ODID_TXRX_2=237, /* Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). | */ - MAV_COMP_ID_ODID_TXRX_3=238, /* Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). | */ - MAV_COMP_ID_UDP_BRIDGE=240, /* Component to bridge MAVLink to UDP (i.e. from a UART). | */ - MAV_COMP_ID_UART_BRIDGE=241, /* Component to bridge to UART (i.e. from UDP). | */ - MAV_COMP_ID_TUNNEL_NODE=242, /* Component handling TUNNEL messages (e.g. vendor specific GUI of a component). | */ - MAV_COMP_ID_SYSTEM_CONTROL=250, /* Component for handling system messages (e.g. to ARM, takeoff, etc.). | */ - MAV_COMPONENT_ENUM_END=251, /* | */ -} MAV_COMPONENT; -#endif - /** @brief These encode the sensors whose status is sent as part of the SYS_STATUS message. */ #ifndef HAVE_ENUM_MAV_SYS_STATUS_SENSOR #define HAVE_ENUM_MAV_SYS_STATUS_SENSOR @@ -403,7 +140,8 @@ typedef enum MAV_SYS_STATUS_SENSOR MAV_SYS_STATUS_SENSOR_SATCOM=134217728, /* 0x8000000 Satellite Communication | */ MAV_SYS_STATUS_PREARM_CHECK=268435456, /* 0x10000000 pre-arm check status. Always healthy when armed | */ MAV_SYS_STATUS_OBSTACLE_AVOIDANCE=536870912, /* 0x20000000 Avoidance/collision prevention | */ - MAV_SYS_STATUS_SENSOR_ENUM_END=536870913, /* | */ + MAV_SYS_STATUS_SENSOR_PROPULSION=1073741824, /* 0x40000000 propulsion (actuator, esc, motor or propellor) | */ + MAV_SYS_STATUS_SENSOR_ENUM_END=1073741825, /* | */ } MAV_SYS_STATUS_SENSOR; #endif @@ -443,13 +181,13 @@ typedef enum MAV_FRAME #define HAVE_ENUM_MAVLINK_DATA_STREAM_TYPE typedef enum MAVLINK_DATA_STREAM_TYPE { - MAVLINK_DATA_STREAM_IMG_JPEG=1, /* | */ - MAVLINK_DATA_STREAM_IMG_BMP=2, /* | */ - MAVLINK_DATA_STREAM_IMG_RAW8U=3, /* | */ - MAVLINK_DATA_STREAM_IMG_RAW32U=4, /* | */ - MAVLINK_DATA_STREAM_IMG_PGM=5, /* | */ - MAVLINK_DATA_STREAM_IMG_PNG=6, /* | */ - MAVLINK_DATA_STREAM_TYPE_ENUM_END=7, /* | */ + MAVLINK_DATA_STREAM_IMG_JPEG=0, /* | */ + MAVLINK_DATA_STREAM_IMG_BMP=1, /* | */ + MAVLINK_DATA_STREAM_IMG_RAW8U=2, /* | */ + MAVLINK_DATA_STREAM_IMG_RAW32U=3, /* | */ + MAVLINK_DATA_STREAM_IMG_PGM=4, /* | */ + MAVLINK_DATA_STREAM_IMG_PNG=5, /* | */ + MAVLINK_DATA_STREAM_TYPE_ENUM_END=6, /* | */ } MAVLINK_DATA_STREAM_TYPE; #endif @@ -503,7 +241,8 @@ typedef enum MAV_MOUNT_MODE MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */ MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */ MAV_MOUNT_MODE_SYSID_TARGET=5, /* Gimbal tracks system with specified system ID | */ - MAV_MOUNT_MODE_ENUM_END=6, /* | */ + MAV_MOUNT_MODE_HOME_LOCATION=6, /* Gimbal tracks home location | */ + MAV_MOUNT_MODE_ENUM_END=7, /* | */ } MAV_MOUNT_MODE; #endif @@ -547,10 +286,7 @@ typedef enum GIMBAL_MANAGER_CAP_FLAGS GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_INFINITE_YAW=2048, /* Based on GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW. | */ GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_LOCAL=65536, /* Gimbal manager supports to point to a local position. | */ GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL=131072, /* Gimbal manager supports to point to a global latitude, longitude, altitude position. | */ - GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_FOCAL_LENGTH_SCALE=1048576, /* Gimbal manager supports pitching and yawing at an angular velocity scaled by focal length (the more zoomed in, the slower the movement). | */ - GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_NUDGING=2097152, /* Gimbal manager supports nudging when pointing to a location or tracking. | */ - GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_OVERRIDE=4194304, /* Gimbal manager supports overriding when pointing to a location or tracking. | */ - GIMBAL_MANAGER_CAP_FLAGS_ENUM_END=4194305, /* | */ + GIMBAL_MANAGER_CAP_FLAGS_ENUM_END=131073, /* | */ } GIMBAL_MANAGER_CAP_FLAGS; #endif @@ -578,11 +314,7 @@ typedef enum GIMBAL_MANAGER_FLAGS GIMBAL_MANAGER_FLAGS_ROLL_LOCK=4, /* Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK | */ GIMBAL_MANAGER_FLAGS_PITCH_LOCK=8, /* Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK | */ GIMBAL_MANAGER_FLAGS_YAW_LOCK=16, /* Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK | */ - GIMBAL_MANAGER_FLAGS_ANGULAR_VELOCITY_RELATIVE_TO_FOCAL_LENGTH=1048576, /* Scale angular velocity relative to focal length. This means the gimbal moves slower if it is zoomed in. | */ - GIMBAL_MANAGER_FLAGS_NUDGE=2097152, /* Interpret attitude control on top of pointing to a location or tracking. If this flag is set, the quaternion is relative to the existing tracking angle. | */ - GIMBAL_MANAGER_FLAGS_OVERRIDE=4194304, /* Completely override pointing to a location or tracking. If this flag is set, the quaternion is (as usual) according to GIMBAL_MANAGER_FLAGS_YAW_LOCK. | */ - GIMBAL_MANAGER_FLAGS_NONE=8388608, /* This flag can be set to give up control previously set using MAV_CMD_DO_GIMBAL_MANAGER_ATTITUDE. This flag must not be combined with other flags. | */ - GIMBAL_MANAGER_FLAGS_ENUM_END=8388609, /* | */ + GIMBAL_MANAGER_FLAGS_ENUM_END=17, /* | */ } GIMBAL_MANAGER_FLAGS; #endif @@ -604,6 +336,29 @@ typedef enum GIMBAL_DEVICE_ERROR_FLAGS } GIMBAL_DEVICE_ERROR_FLAGS; #endif +/** @brief Gripper actions. */ +#ifndef HAVE_ENUM_GRIPPER_ACTIONS +#define HAVE_ENUM_GRIPPER_ACTIONS +typedef enum GRIPPER_ACTIONS +{ + GRIPPER_ACTION_RELEASE=0, /* Gripper release cargo. | */ + GRIPPER_ACTION_GRAB=1, /* Gripper grab onto cargo. | */ + GRIPPER_ACTIONS_ENUM_END=2, /* | */ +} GRIPPER_ACTIONS; +#endif + +/** @brief Winch actions. */ +#ifndef HAVE_ENUM_WINCH_ACTIONS +#define HAVE_ENUM_WINCH_ACTIONS +typedef enum WINCH_ACTIONS +{ + WINCH_RELAXED=0, /* Relax winch. | */ + WINCH_RELATIVE_LENGTH_CONTROL=1, /* Wind or unwind specified length of cable, optionally using specified rate. | */ + WINCH_RATE_CONTROL=2, /* Wind or unwind cable at specified rate. | */ + WINCH_ACTIONS_ENUM_END=3, /* | */ +} WINCH_ACTIONS; +#endif + /** @brief Generalized UAVCAN node health */ #ifndef HAVE_ENUM_UAVCAN_NODE_HEALTH #define HAVE_ENUM_UAVCAN_NODE_HEALTH @@ -676,6 +431,24 @@ typedef enum STORAGE_STATUS } STORAGE_STATUS; #endif +/** @brief Flags to indicate the type of storage. */ +#ifndef HAVE_ENUM_STORAGE_TYPE +#define HAVE_ENUM_STORAGE_TYPE +typedef enum STORAGE_TYPE +{ + STORAGE_TYPE_UNKNOWN=0, /* Storage type is not known. | */ + STORAGE_TYPE_USB_STICK=1, /* Storage type is USB device. | */ + STORAGE_TYPE_SD=2, /* Storage type is SD card. | */ + STORAGE_TYPE_MICROSD=3, /* Storage type is microSD card. | */ + STORAGE_TYPE_CF=4, /* Storage type is CFast. | */ + STORAGE_TYPE_CFE=5, /* Storage type is CFexpress. | */ + STORAGE_TYPE_XQD=6, /* Storage type is XQD. | */ + STORAGE_TYPE_HD=7, /* Storage type is HD mass storage type. | */ + STORAGE_TYPE_OTHER=254, /* Storage type is other, not listed type. | */ + STORAGE_TYPE_ENUM_END=255, /* | */ +} STORAGE_TYPE; +#endif + /** @brief Yaw behaviour during orbit flight. */ #ifndef HAVE_ENUM_ORBIT_YAW_BEHAVIOUR #define HAVE_ENUM_ORBIT_YAW_BEHAVIOUR @@ -739,23 +512,11 @@ typedef enum COMP_METADATA_TYPE { COMP_METADATA_TYPE_VERSION=0, /* Version information which also includes information on other optional supported COMP_METADATA_TYPE's. Must be supported. Only downloadable from vehicle. | */ COMP_METADATA_TYPE_PARAMETER=1, /* Parameter meta data. | */ - COMP_METADATA_TYPE_ENUM_END=2, /* | */ + COMP_METADATA_TYPE_COMMANDS=2, /* Meta data which specifies the commands the vehicle supports. (WIP) | */ + COMP_METADATA_TYPE_ENUM_END=3, /* | */ } COMP_METADATA_TYPE; #endif -/** @brief Possible responses from a PARAM_START_TRANSACTION and PARAM_COMMIT_TRANSACTION messages. */ -#ifndef HAVE_ENUM_PARAM_TRANSACTION_RESPONSE -#define HAVE_ENUM_PARAM_TRANSACTION_RESPONSE -typedef enum PARAM_TRANSACTION_RESPONSE -{ - PARAM_TRANSACTION_RESPONSE_ACCEPTED=0, /* Transaction accepted. | */ - PARAM_TRANSACTION_RESPONSE_FAILED=1, /* Transaction failed. | */ - PARAM_TRANSACTION_RESPONSE_UNSUPPORTED=2, /* Transaction unsupported. | */ - PARAM_TRANSACTION_RESPONSE_INPROGRESS=3, /* Transaction in progress. | */ - PARAM_TRANSACTION_RESPONSE_ENUM_END=4, /* | */ -} PARAM_TRANSACTION_RESPONSE; -#endif - /** @brief Possible transport layers to set and get parameters via mavlink during a parameter transaction. */ #ifndef HAVE_ENUM_PARAM_TRANSACTION_TRANSPORT #define HAVE_ENUM_PARAM_TRANSACTION_TRANSPORT @@ -767,14 +528,15 @@ typedef enum PARAM_TRANSACTION_TRANSPORT } PARAM_TRANSACTION_TRANSPORT; #endif -/** @brief Possible parameter transaction action during a commit. */ +/** @brief Possible parameter transaction actions. */ #ifndef HAVE_ENUM_PARAM_TRANSACTION_ACTION #define HAVE_ENUM_PARAM_TRANSACTION_ACTION typedef enum PARAM_TRANSACTION_ACTION { - PARAM_TRANSACTION_ACTION_COMMIT=0, /* Commit the current parameter transaction. | */ - PARAM_TRANSACTION_ACTION_CANCEL=1, /* Cancel the current parameter transaction. | */ - PARAM_TRANSACTION_ACTION_ENUM_END=2, /* | */ + PARAM_TRANSACTION_ACTION_START=0, /* Commit the current parameter transaction. | */ + PARAM_TRANSACTION_ACTION_COMMIT=1, /* Commit the current parameter transaction. | */ + PARAM_TRANSACTION_ACTION_CANCEL=2, /* Cancel the current parameter transaction. | */ + PARAM_TRANSACTION_ACTION_ENUM_END=3, /* | */ } PARAM_TRANSACTION_ACTION; #endif @@ -788,7 +550,7 @@ typedef enum MAV_CMD MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this waypoint for X turns |Number of turns.| Leave loiter circle only once heading towards the next waypoint (0 = False)| Loiter radius around waypoint for forward-only moving vehicles (not multicopters). If positive loiter clockwise, else counter-clockwise| Loiter circle exit location and/or path to next waypoint ("xtrack") for forward-only moving vehicles (not multicopters). 0 for the vehicle to converge towards the center xtrack when it leaves the loiter (the line between the centers of the current and next waypoint), 1 to converge to the direct line between the location that the vehicle exits the loiter radius and the next waypoint. Otherwise the angle (in degrees) between the tangent of the loiter circle and the center xtrack at which the vehicle must leave the loiter (and converge to the center xtrack). NaN to use the current system default xtrack behaviour.| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_LOITER_TIME=19, /* Loiter at the specified latitude, longitude and altitude for a certain amount of time. Multicopter vehicles stop at the point (within a vehicle-specific acceptance radius). Forward-only moving vehicles (e.g. fixed-wing) circle the point with the specified radius/direction. If the Heading Required parameter (2) is non-zero forward moving aircraft will only leave the loiter circle once heading towards the next waypoint. |Loiter time (only starts once Lat, Lon and Alt is reached).| Leave loiter circle only once heading towards the next waypoint (0 = False)| Loiter radius around waypoint for forward-only moving vehicles (not multicopters). If positive loiter clockwise, else counter-clockwise.| Loiter circle exit location and/or path to next waypoint ("xtrack") for forward-only moving vehicles (not multicopters). 0 for the vehicle to converge towards the center xtrack when it leaves the loiter (the line between the centers of the current and next waypoint), 1 to converge to the direct line between the location that the vehicle exits the loiter radius and the next waypoint. Otherwise the angle (in degrees) between the tangent of the loiter circle and the center xtrack at which the vehicle must leave the loiter (and converge to the center xtrack). NaN to use the current system default xtrack behaviour.| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_LAND=21, /* Land at location. |Minimum target altitude if landing is aborted (0 = undefined/use system default).| Precision land mode.| Empty| Desired yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude.| Longitude.| Landing altitude (ground level in current frame).| */ + MAV_CMD_NAV_LAND=21, /* Land at location. |Minimum target altitude if landing is aborted (0 = undefined/use system default).| Precision land mode.| Empty.| Desired yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude.| Longitude.| Landing altitude (ground level in current frame).| */ MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand. Vehicles that support multiple takeoff modes (e.g. VTOL quadplane) should take off using the currently configured mode. |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_LAND_LOCAL=23, /* Land at local position (local frame only) |Landing target number (if available)| Maximum accepted offset from desired landing position - computed magnitude from spherical coordinates: d = sqrt(x^2 + y^2 + z^2), which gives the maximum accepted distance between the desired landing position and the position where the vehicle is about to land| Landing descend rate| Desired yaw angle| Y-axis position| X-axis position| Z-axis / ground level position| */ MAV_CMD_NAV_TAKEOFF_LOCAL=24, /* Takeoff from local position (local frame only) |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Takeoff ascend rate| Yaw angle (if magnetometer or another yaw estimation source present), ignored without one of these| Y-axis position| X-axis position| Z-axis position| */ @@ -804,7 +566,7 @@ typedef enum MAV_CMD MAV_CMD_NAV_VTOL_TAKEOFF=84, /* Takeoff from ground using VTOL mode, and transition to forward flight with specified heading. The command should be ignored by vehicles that dont support both VTOL and fixed-wing flight (multicopters, boats,etc.). |Empty| Front transition heading.| Empty| Yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_VTOL_LAND=85, /* Land using VTOL mode |Empty| Empty| Approach altitude (with the same reference as the Altitude field). NaN if unspecified.| Yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude| Longitude| Altitude (ground level)| */ MAV_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_DELAY=93, /* Delay the next navigation command a number of seconds or until a specified time |Delay (-1 to enable time-of-day fields)| hour (24h format, UTC, -1 to ignore)| minute (24h format, UTC, -1 to ignore)| second (24h format, UTC)| Empty| Empty| Empty| */ + MAV_CMD_NAV_DELAY=93, /* Delay the next navigation command a number of seconds or until a specified time |Delay (-1 to enable time-of-day fields)| hour (24h format, UTC, -1 to ignore)| minute (24h format, UTC, -1 to ignore)| second (24h format, UTC, -1 to ignore)| Empty| Empty| Empty| */ MAV_CMD_NAV_PAYLOAD_PLACE=94, /* Descend and place payload. Vehicle moves to specified location, descends until it detects a hanging payload has reached the ground, and then releases the payload. If ground is not detected before the reaching the maximum descent value (param1), the command will complete without releasing the payload. |Maximum distance to descend.| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay| Empty| Empty| Empty| Empty| Empty| Empty| */ @@ -822,7 +584,7 @@ typedef enum MAV_CMD MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo instance number.| Pulse Width Modulation.| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo instance number.| Pulse Width Modulation.| Cycle count.| Cycle time.| Empty| Empty| Empty| */ MAV_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CHANGE_ALTITUDE=186, /* Change altitude set point. |Altitude| Frame of new altitude.| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_CHANGE_ALTITUDE=186, /* Change altitude set point. |Altitude.| Frame of new altitude.| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_SET_ACTUATOR=187, /* Sets actuators (e.g. servos) to a desired value. The actuator numbers are mapped to specific outputs (e.g. on any MAIN or AUX PWM or UAVCAN) using a flight-stack specific mechanism (i.e. a parameter). |Actuator 1 value, scaled from [-1 to 1]. NaN to ignore.| Actuator 2 value, scaled from [-1 to 1]. NaN to ignore.| Actuator 3 value, scaled from [-1 to 1]. NaN to ignore.| Actuator 4 value, scaled from [-1 to 1]. NaN to ignore.| Actuator 5 value, scaled from [-1 to 1]. NaN to ignore.| Actuator 6 value, scaled from [-1 to 1]. NaN to ignore.| Index of actuator set (i.e if set to 1, Actuator 1 becomes Actuator 7)| */ MAV_CMD_DO_LAND_START=189, /* Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty| */ MAV_CMD_DO_RALLY_LAND=190, /* Mission command to perform a landing from a rally point. |Break altitude| Landing speed| Empty| Empty| Empty| Empty| Empty| */ @@ -830,10 +592,10 @@ typedef enum MAV_CMD MAV_CMD_DO_REPOSITION=192, /* Reposition the vehicle to a specific WGS84 global position. |Ground speed, less than 0 (-1) for default| Bitmask of option flags.| Reserved| Yaw heading. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). For planes indicates loiter direction (0: clockwise, 1: counter clockwise)| Latitude| Longitude| Altitude| */ MAV_CMD_DO_PAUSE_CONTINUE=193, /* If in a GPS controlled position mode, hold the current position or continue. |0: Pause current mission or reposition command, hold current position. 1: Continue mission. A VTOL capable vehicle should enter hover mode (multicopter and VTOL planes). A plane should loiter with the default loiter radius.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ MAV_CMD_DO_SET_REVERSE=194, /* Set moving direction to forward or reverse. |Direction (0=Forward, 1=Reverse)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_ROI_LOCATION=195, /* Sets the region of interest (ROI) to a location. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal is not to react to this message. |Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. (Send command multiple times for more than one but not all gimbals.)| Empty| Empty| Empty| Latitude of ROI location| Longitude of ROI location| Altitude of ROI location| */ - MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET=196, /* Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal device is not to react to this message. |Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. (Send command multiple times for more than one but not all gimbals.)| Empty| Empty| Empty| Pitch offset from next waypoint, positive tilting up| roll offset from next waypoint, positive banking to the right| yaw offset from next waypoint, positive panning to the right| */ - MAV_CMD_DO_SET_ROI_NONE=197, /* Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal device is not to react to this message. After this command the gimbal manager should go back to manual input if available, and otherwise assume a neutral position. |Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. (Send command multiple times for more than one but not all gimbals.)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_ROI_SYSID=198, /* Mount tracks system with specified system ID. Determination of target vehicle position may be done with GLOBAL_POSITION_INT or any other means. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal device is not to react to this message. |System ID| Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. (Send command multiple times for more than one but not all gimbals.)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_DO_SET_ROI_LOCATION=195, /* Sets the region of interest (ROI) to a location. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal is not to react to this message. |Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| Empty| Empty| Empty| Latitude of ROI location| Longitude of ROI location| Altitude of ROI location| */ + MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET=196, /* Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal device is not to react to this message. |Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| Empty| Empty| Empty| Pitch offset from next waypoint, positive pitching up| Roll offset from next waypoint, positive rolling to the right| Yaw offset from next waypoint, positive yawing to the right| */ + MAV_CMD_DO_SET_ROI_NONE=197, /* Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal device is not to react to this message. After this command the gimbal manager should go back to manual input if available, and otherwise assume a neutral position. |Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI_SYSID=198, /* Mount tracks system with specified system ID. Determination of target vehicle position may be done with GLOBAL_POSITION_INT or any other means. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal device is not to react to this message. |System ID| Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode.| Waypoint index/ target ID (depends on param 1).| Region of interest index. (allows a vehicle to manage multiple ROI's)| Empty| MAV_ROI_WPNEXT: pitch offset from next waypoint, MAV_ROI_LOCATION: latitude| MAV_ROI_WPNEXT: roll offset from next waypoint, MAV_ROI_LOCATION: longitude| MAV_ROI_WPNEXT: yaw offset from next waypoint, MAV_ROI_LOCATION: altitude| */ MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Configure digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ). |Modes: P, TV, AV, M, Etc.| Shutter speed: Divisor number for one second.| Aperture: F stop number.| ISO number e.g. 80, 100, 200, Etc.| Exposure type enumerator.| Command Identity.| Main engine cut-off time before camera trigger. (0 means no cut-off)| */ @@ -845,6 +607,8 @@ typedef enum MAV_CMD MAV_CMD_DO_PARACHUTE=208, /* Mission item/command to release a parachute or enable/disable auto release. |Action| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_MOTOR_TEST=209, /* Mission command to perform motor test. |Motor instance number. (from 1 to max number of motors on the vehicle)| Throttle type.| Throttle.| Timeout.| Motor count. (number of motors to test to test in sequence, waiting for the timeout above between them; 0=1 motor, 1=1 motor, 2=2 motors...)| Motor test order.| Empty| */ MAV_CMD_DO_INVERTED_FLIGHT=210, /* Change to/from inverted flight. |Inverted flight. (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_GRIPPER=211, /* Mission command to operate a gripper. |Gripper instance number.| Gripper action to perform.| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_AUTOTUNE_ENABLE=212, /* Enable/disable autotune. |Enable (1: enable, 0:disable).| Empty.| Empty.| Empty.| Empty.| Empty.| Empty.| */ MAV_CMD_NAV_SET_YAW_SPEED=213, /* Sets a desired vehicle turn angle and speed change. |Yaw angle to adjust steering by.| Speed.| Final angle. (0=absolute, 1=relative)| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL=214, /* Mission command to set camera trigger interval for this flight. If triggering is enabled, the camera is triggered each time this interval expires. This command can also be used to set the shutter integration time for the camera. |Camera trigger cycle time. -1 or 0 to ignore.| Camera shutter integration time. Should be less than trigger cycle time. -1 or 0 to ignore.| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |quaternion param q1, w (1 in null-rotation)| quaternion param q2, x (0 in null-rotation)| quaternion param q3, y (0 in null-rotation)| quaternion param q4, z (0 in null-rotation)| Empty| Empty| Empty| */ @@ -855,11 +619,12 @@ typedef enum MAV_CMD MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. Except for Temperature Calibration, only one sensor should be set in a single message and all others should be zero. |1: gyro calibration, 3: gyro temperature calibration| 1: magnetometer calibration| 1: ground pressure calibration| 1: radio RC calibration, 2: RC trim calibration| 1: accelerometer calibration, 2: board level calibration, 3: accelerometer temperature calibration, 4: simple accelerometer calibration| 1: APM: compass/motor interference calibration (PX4: airspeed calibration, deprecated), 2: airspeed calibration| 1: ESC calibration, 3: barometer temperature calibration| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow, 5: second magnetometer, 6: third magnetometer| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - MAV_CMD_PREFLIGHT_UAVCAN=243, /* Trigger UAVCAN config. This command will be only accepted if in pre-flight mode. |1: Trigger actuator ID assignment and direction mapping.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ + MAV_CMD_PREFLIGHT_UAVCAN=243, /* Trigger UAVCAN configuration (actuator ID assignment and direction mapping). Note that this maps to the legacy UAVCAN v0 function UAVCAN_ENUMERATE, which is intended to be executed just once during initial vehicle configuration (it is not a normal pre-flight command and has been poorly named). |1: Trigger actuator ID assignment and direction mapping. 0: Cancel command.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM, 2: Reset to defaults| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM, 2: Reset to defaults| Onboard logging: 0: Ignore, 1: Start default rate logging, -1: Stop logging, > 1: logging rate (e.g. set to 1000 for 1000 Hz logging)| Reserved| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot, 3: Reboot autopilot and keep it in the bootloader until upgraded.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer, 3: Reboot onboard computer and keep it in the bootloader until upgraded.| WIP: 0: Do nothing for camera, 1: Reboot onboard camera, 2: Shutdown onboard camera, 3: Reboot onboard camera and keep it in the bootloader until upgraded| WIP: 0: Do nothing for mount (e.g. gimbal), 1: Reboot mount, 2: Shutdown mount, 3: Reboot mount and keep it in the bootloader until upgraded| Reserved (set to 0)| Reserved (set to 0)| WIP: ID (e.g. camera ID -1 for all IDs)| */ MAV_CMD_DO_UPGRADE=247, /* Request a target system to start an upgrade of one (or all) of its components. For example, the command might be sent to a companion computer to cause it to upgrade a connected flight controller. The system doing the upgrade will report progress using the normal command protocol sequence for a long running operation. Command protocol information: https://mavlink.io/en/services/command.html. |Component id of the component to be upgraded. If set to 0, all components should be upgraded.| 0: Do not reboot component after the action is executed, 1: Reboot component after the action is executed.| Reserved| Reserved| Reserved| Reserved| WIP: upgrade progress report rate (can be used for more granular control).| */ MAV_CMD_OVERRIDE_GOTO=252, /* Override current mission with command to pause mission, pause mission and move to position, continue/resume mission. When param 1 indicates that the mission is paused (MAV_GOTO_DO_HOLD), param 2 defines whether it holds in place or moves to another position. |MAV_GOTO_DO_HOLD: pause mission and either hold or move to specified position (depending on param2), MAV_GOTO_DO_CONTINUE: resume mission.| MAV_GOTO_HOLD_AT_CURRENT_POSITION: hold at current position, MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position.| Coordinate frame of hold point.| Desired yaw angle.| Latitude/X position.| Longitude/Y position.| Altitude/Z position.| */ + MAV_CMD_OBLIQUE_SURVEY=260, /* Mission command to set a Camera Auto Mount Pivoting Oblique Survey (Replaces CAM_TRIGG_DIST for this purpose). The camera is triggered each time this distance is exceeded, then the mount moves to the next position. Params 4~6 set-up the angle limits and number of positions for oblique survey, where mount-enabled vehicles automatically roll the camera between shots to emulate an oblique camera setup (providing an increased HFOV). This command can also be used to set the shutter integration time for the camera. |Camera trigger distance. 0 to stop triggering.| Camera shutter integration time. 0 to ignore| The minimum interval in which the camera is capable of taking subsequent pictures repeatedly. 0 to ignore.| Total number of roll positions at which the camera will capture photos (images captures spread evenly across the limits defined by param5).| Angle limits that the camera can be rolled to left and right of center.| Fixed pitch angle that the camera will hold in oblique mode if the mount is actuated in the pitch axis.| Empty| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |0: disarm, 1: arm| 0: arm-disarm unless prevented by safety checks (i.e. when landed), 21196: force arming/disarming (e.g. allow arming to override preflight checks and disarming in flight)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ MAV_CMD_ILLUMINATOR_ON_OFF=405, /* Turns illuminators ON/OFF. An illuminator is a light source that is used for lighting up dark areas external to the sytstem: e.g. a torch or searchlight (as opposed to a light source for illuminating the system itself, e.g. an indicator light). |0: Illuminators OFF, 1: Illuminators ON| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ @@ -883,8 +648,10 @@ typedef enum MAV_CMD MAV_CMD_SET_CAMERA_FOCUS=532, /* Set camera focus. Camera must respond with a CAMERA_SETTINGS message (on success). |Focus type| Focus value| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ MAV_CMD_JUMP_TAG=600, /* Tagged jump target. Can be jumped to with MAV_CMD_DO_JUMP_TAG. |Tag.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ MAV_CMD_DO_JUMP_TAG=601, /* Jump to the matching tag in the mission list. Repeat this action for the specified number of times. A mission should contain a single matching tag for each jump. If this is not the case then a jump to a missing tag should complete the mission, and a jump where there are multiple matching tags should always select the one with the lowest mission sequence number. |Target tag to jump to.| Repeat count.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_DO_GIMBAL_MANAGER_TILTPAN=1000, /* High level setpoint to be sent to a gimbal manager to set a gimbal attitude. It is possible to set combinations of the values below. E.g. an angle as well as a desired angular rate can be used to get to this angle at a certain angular rate, or an angular rate only will result in continuous turning. NaN is to be used to signal unset. Note: a gimbal is never to react to this command but only the gimbal manager. |Tilt/pitch rate (positive to tilt up).| Pan/yaw rate (positive to pan to the right).| Tilt/pitch angle (positive to tilt up, relative to vehicle for PAN mode, relative to world horizon for HOLD mode).| Pan/yaw angle (positive to pan to the right, relative to vehicle for PAN mode, absolute to North for HOLD mode).| Gimbal manager flags to use.| Reserved (default:0)| Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. (Send command multiple times for more than one but not all gimbals.)| */ - MAV_CMD_IMAGE_START_CAPTURE=2000, /* Start image capture sequence. Sends CAMERA_IMAGE_CAPTURED after each capture. Use NaN for reserved values. |Reserved (Set to 0)| Desired elapsed time between two consecutive pictures (in seconds). Minimum values depend on hardware (typically greater than 2 seconds).| Total number of images to capture. 0 to capture forever/until MAV_CMD_IMAGE_STOP_CAPTURE.| Capture sequence number starting from 1. This is only valid for single-capture (param3 == 1). Increment the capture ID for each capture command to prevent double captures when a command is re-transmitted. Use 0 to ignore it.| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_PARAM_TRANSACTION=900, /* Request to start or end a parameter transaction. Multiple kinds of transport layers can be used to exchange parameters in the transaction (param, param_ext and mavftp). The command response can either be a success/failure or an in progress in case the receiving side takes some time to apply the parameters. |Action to be performed (start, commit, cancel, etc.)| Possible transport layers to set and get parameters via mavlink during a parameter transaction.| Identifier for a specific transaction.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW=1000, /* High level setpoint to be sent to a gimbal manager to set a gimbal attitude. It is possible to set combinations of the values below. E.g. an angle as well as a desired angular rate can be used to get to this angle at a certain angular rate, or an angular rate only will result in continuous turning. NaN is to be used to signal unset. Note: a gimbal is never to react to this command but only the gimbal manager. |Pitch angle (positive to pitch up, relative to vehicle for FOLLOW mode, relative to world horizon for LOCK mode).| Yaw angle (positive to yaw to the right, relative to vehicle for FOLLOW mode, absolute to North for LOCK mode).| Pitch rate (positive to pitch up).| Yaw rate (positive to yaw to the right).| Gimbal manager flags to use.| Reserved (default:0)| Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| */ + MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE=1001, /* Gimbal configuration to set which sysid/compid is in primary and secondary control. |Sysid for primary control (0: no one in control, -1: leave unchanged, -2: set itself in control (for missions where the own sysid is still unknown), -3: remove control if currently in control).| Compid for primary control (0: no one in control, -1: leave unchanged, -2: set itself in control (for missions where the own sysid is still unknown), -3: remove control if currently in control).| Sysid for secondary control (0: no one in control, -1: leave unchanged, -2: set itself in control (for missions where the own sysid is still unknown), -3: remove control if currently in control).| Compid for secondary control (0: no one in control, -1: leave unchanged, -2: set itself in control (for missions where the own sysid is still unknown), -3: remove control if currently in control).| Reserved (default:0)| Reserved (default:0)| Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| */ + MAV_CMD_IMAGE_START_CAPTURE=2000, /* Start image capture sequence. Sends CAMERA_IMAGE_CAPTURED after each capture. Use NaN for reserved values. |Reserved (Set to 0)| Desired elapsed time between two consecutive pictures (in seconds). Minimum values depend on hardware (typically greater than 2 seconds).| Total number of images to capture. 0 to capture forever/until MAV_CMD_IMAGE_STOP_CAPTURE.| Capture sequence number starting from 1. This is only valid for single-capture (param3 == 1), otherwise set to 0. Increment the capture ID for each capture command to prevent double captures when a command is re-transmitted.| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| */ MAV_CMD_IMAGE_STOP_CAPTURE=2001, /* Stop image capture sequence Use NaN for reserved values. |Reserved (Set to 0)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ MAV_CMD_REQUEST_CAMERA_IMAGE_CAPTURE=2002, /* Re-request a CAMERA_IMAGE_CAPTURED message. |Sequence number for missing CAMERA_IMAGE_CAPTURED message| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ MAV_CMD_DO_TRIGGER_CONTROL=2003, /* Enable or disable on-board camera triggering system. |Trigger enable/disable (0 for disable, 1 for start), -1 to ignore| 1 to reset the trigger sequence, -1 or 0 to ignore| 1 to pause triggering, but without switching the camera off or retracting it. -1 to ignore| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ @@ -910,14 +677,13 @@ typedef enum MAV_CMD MAV_CMD_SET_GUIDED_SUBMODE_CIRCLE=4001, /* This command sets submode circle when vehicle is in guided mode. Vehicle flies along a circle facing the center of the circle. The user can input the velocity along the circle and change the radius. If no input is given the vehicle will hold position. |Radius of desired circle in CIRCLE_MODE| User defined| User defined| User defined| Target latitude of center of circle in CIRCLE_MODE| Target longitude of center of circle in CIRCLE_MODE| Reserved (default:0)| */ MAV_CMD_CONDITION_GATE=4501, /* Delay mission state machine until gate has been reached. |Geometry: 0: orthogonal to path between previous and next waypoint.| Altitude: 0: ignore altitude| Empty| Empty| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_FENCE_RETURN_POINT=5000, /* Fence return point. There can only be one fence return point. - |Reserved| Reserved| Reserved| Reserved| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_FENCE_RETURN_POINT=5000, /* Fence return point (there can only be one such point in a geofence definition). If rally points are supported they should be used instead. |Reserved| Reserved| Reserved| Reserved| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION=5001, /* Fence vertex for an inclusion polygon (the polygon must not be self-intersecting). The vehicle must stay within this area. Minimum of 3 vertices required. - |Polygon vertex count| Reserved| Reserved| Reserved| Latitude| Longitude| Reserved| */ + |Polygon vertex count| Vehicle must be inside ALL inclusion zones in a single group, vehicle must be inside at least one group, must be the same for all points in each polygon| Reserved| Reserved| Latitude| Longitude| Reserved| */ MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION=5002, /* Fence vertex for an exclusion polygon (the polygon must not be self-intersecting). The vehicle must stay outside this area. Minimum of 3 vertices required. |Polygon vertex count| Reserved| Reserved| Reserved| Latitude| Longitude| Reserved| */ MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION=5003, /* Circular fence area. The vehicle must stay inside this area. - |Radius.| Reserved| Reserved| Reserved| Latitude| Longitude| Reserved| */ + |Radius.| Vehicle must be inside ALL inclusion zones in a single group, vehicle must be inside at least one group| Reserved| Reserved| Latitude| Longitude| Reserved| */ MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION=5004, /* Circular fence area. The vehicle must stay outside this area. |Radius.| Reserved| Reserved| Reserved| Latitude| Longitude| Reserved| */ MAV_CMD_NAV_RALLY_POINT=5100, /* Rally point. You can have multiple rally points defined. @@ -940,7 +706,9 @@ typedef enum MAV_CMD MAV_CMD_USER_3=31012, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ MAV_CMD_USER_4=31013, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ MAV_CMD_USER_5=31014, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ - MAV_CMD_ENUM_END=31015, /* | */ + MAV_CMD_FIXED_MAG_CAL_YAW=42006, /* Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location. |Yaw of vehicle in earth frame.| CompassMask, 0 for all.| Latitude.| Longitude.| Empty.| Empty.| Empty.| */ + MAV_CMD_DO_WINCH=42600, /* Command to operate winch. |Winch instance number.| Action to perform.| Length of cable to release (negative to wind).| Release rate (negative to wind).| Empty.| Empty.| Empty.| */ + MAV_CMD_ENUM_END=42601, /* | */ } MAV_CMD; #endif @@ -985,16 +753,16 @@ typedef enum MAV_ROI #define HAVE_ENUM_MAV_CMD_ACK typedef enum MAV_CMD_ACK { - MAV_CMD_ACK_OK=1, /* Command / mission item is ok. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_FAIL=2, /* Generic error message if none of the other reasons fails or if no detailed error reporting is implemented. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_ACCESS_DENIED=3, /* The system is refusing to accept this command from this source / communication partner. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_NOT_SUPPORTED=4, /* Command or mission item is not supported, other commands would be accepted. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_COORDINATE_FRAME_NOT_SUPPORTED=5, /* The coordinate frame of this command / mission item is not supported. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_COORDINATES_OUT_OF_RANGE=6, /* The coordinate frame of this command is ok, but he coordinate values exceed the safety limits of this system. This is a generic error, please use the more specific error messages below if possible. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_X_LAT_OUT_OF_RANGE=7, /* The X or latitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_Y_LON_OUT_OF_RANGE=8, /* The Y or longitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_Z_ALT_OUT_OF_RANGE=9, /* The Z or altitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ENUM_END=10, /* | */ + MAV_CMD_ACK_OK=0, /* Command / mission item is ok. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_FAIL=1, /* Generic error message if none of the other reasons fails or if no detailed error reporting is implemented. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_ACCESS_DENIED=2, /* The system is refusing to accept this command from this source / communication partner. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_NOT_SUPPORTED=3, /* Command or mission item is not supported, other commands would be accepted. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_COORDINATE_FRAME_NOT_SUPPORTED=4, /* The coordinate frame of this command / mission item is not supported. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_COORDINATES_OUT_OF_RANGE=5, /* The coordinate frame of this command is ok, but he coordinate values exceed the safety limits of this system. This is a generic error, please use the more specific error messages below if possible. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_X_LAT_OUT_OF_RANGE=6, /* The X or latitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_Y_LON_OUT_OF_RANGE=7, /* The Y or longitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_Z_ALT_OUT_OF_RANGE=8, /* The Z or altitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ENUM_END=9, /* | */ } MAV_CMD_ACK; #endif @@ -1208,7 +976,6 @@ typedef enum MAV_SENSOR_ORIENTATION MAV_SENSOR_ROTATION_ROLL_90_PITCH_68_YAW_293=38, /* Roll: 90, Pitch: 68, Yaw: 293 | */ MAV_SENSOR_ROTATION_PITCH_315=39, /* Pitch: 315 | */ MAV_SENSOR_ROTATION_ROLL_90_PITCH_315=40, /* Roll: 90, Pitch: 315 | */ - MAV_SENSOR_ROTATION_ROLL_270_YAW_180=41, /* Roll: 270, Yaw: 180 | */ MAV_SENSOR_ROTATION_CUSTOM=100, /* Custom orientation | */ MAV_SENSOR_ORIENTATION_ENUM_END=101, /* | */ } MAV_SENSOR_ORIENTATION; @@ -1309,26 +1076,39 @@ typedef enum MAV_BATTERY_CHARGE_STATE MAV_BATTERY_CHARGE_STATE_LOW=2, /* Battery state is low, warn and monitor close. | */ MAV_BATTERY_CHARGE_STATE_CRITICAL=3, /* Battery state is critical, return or abort immediately. | */ MAV_BATTERY_CHARGE_STATE_EMERGENCY=4, /* Battery state is too low for ordinary abort sequence. Perform fastest possible emergency stop to prevent damage. | */ - MAV_BATTERY_CHARGE_STATE_FAILED=5, /* Battery failed, damage unavoidable. | */ - MAV_BATTERY_CHARGE_STATE_UNHEALTHY=6, /* Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. | */ + MAV_BATTERY_CHARGE_STATE_FAILED=5, /* Battery failed, damage unavoidable. Possible causes (faults) are listed in MAV_BATTERY_FAULT. | */ + MAV_BATTERY_CHARGE_STATE_UNHEALTHY=6, /* Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in MAV_BATTERY_FAULT. | */ MAV_BATTERY_CHARGE_STATE_CHARGING=7, /* Battery is charging. | */ MAV_BATTERY_CHARGE_STATE_ENUM_END=8, /* | */ } MAV_BATTERY_CHARGE_STATE; #endif -/** @brief Smart battery supply status/fault flags (bitmask) for health indication. */ -#ifndef HAVE_ENUM_MAV_SMART_BATTERY_FAULT -#define HAVE_ENUM_MAV_SMART_BATTERY_FAULT -typedef enum MAV_SMART_BATTERY_FAULT +/** @brief Battery mode. Note, the normal operation mode (i.e. when flying) should be reported as MAV_BATTERY_MODE_UNKNOWN to allow message trimming in normal flight. */ +#ifndef HAVE_ENUM_MAV_BATTERY_MODE +#define HAVE_ENUM_MAV_BATTERY_MODE +typedef enum MAV_BATTERY_MODE { - MAV_SMART_BATTERY_FAULT_DEEP_DISCHARGE=1, /* Battery has deep discharged. | */ - MAV_SMART_BATTERY_FAULT_SPIKES=2, /* Voltage spikes. | */ - MAV_SMART_BATTERY_FAULT_SINGLE_CELL_FAIL=4, /* Single cell has failed. | */ - MAV_SMART_BATTERY_FAULT_OVER_CURRENT=8, /* Over-current fault. | */ - MAV_SMART_BATTERY_FAULT_OVER_TEMPERATURE=16, /* Over-temperature fault. | */ - MAV_SMART_BATTERY_FAULT_UNDER_TEMPERATURE=32, /* Under-temperature fault. | */ - MAV_SMART_BATTERY_FAULT_ENUM_END=33, /* | */ -} MAV_SMART_BATTERY_FAULT; + MAV_BATTERY_MODE_UNKNOWN=0, /* Battery mode not supported/unknown battery mode/normal operation. | */ + MAV_BATTERY_MODE_AUTO_DISCHARGING=1, /* Battery is auto discharging (towards storage level). | */ + MAV_BATTERY_MODE_HOT_SWAP=2, /* Battery in hot-swap mode (current limited to prevent spikes that might damage sensitive electrical circuits). | */ + MAV_BATTERY_MODE_ENUM_END=3, /* | */ +} MAV_BATTERY_MODE; +#endif + +/** @brief Smart battery supply status/fault flags (bitmask) for health indication. The battery must also report either MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY if any of these are set. */ +#ifndef HAVE_ENUM_MAV_BATTERY_FAULT +#define HAVE_ENUM_MAV_BATTERY_FAULT +typedef enum MAV_BATTERY_FAULT +{ + MAV_BATTERY_FAULT_DEEP_DISCHARGE=1, /* Battery has deep discharged. | */ + MAV_BATTERY_FAULT_SPIKES=2, /* Voltage spikes. | */ + MAV_BATTERY_FAULT_CELL_FAIL=4, /* One or more cells have failed. Battery should also report MAV_BATTERY_CHARGE_STATE_FAILE (and should not be used). | */ + MAV_BATTERY_FAULT_OVER_CURRENT=8, /* Over-current fault. | */ + MAV_BATTERY_FAULT_OVER_TEMPERATURE=16, /* Over-temperature fault. | */ + MAV_BATTERY_FAULT_UNDER_TEMPERATURE=32, /* Under-temperature fault. | */ + MAV_BATTERY_FAULT_INCOMPATIBLE_VOLTAGE=64, /* Vehicle voltage is not compatible with this battery (batteries on same power rail should have similar voltage). | */ + MAV_BATTERY_FAULT_ENUM_END=65, /* | */ +} MAV_BATTERY_FAULT; #endif /** @brief Flags to report status/failure cases for a power generator (used in GENERATOR_STATUS). Note that FAULTS are conditions that cause the generator to fail. Warnings are conditions that require attention before the next use (they indicate the system is not operating properly). */ @@ -1680,9 +1460,9 @@ typedef enum CAMERA_TRACKING_STATUS_FLAGS #define HAVE_ENUM_CAMERA_TRACKING_MODE typedef enum CAMERA_TRACKING_MODE { - CAMERA_TRACKING_NONE=0, /* Not tracking | */ - CAMERA_TRACKING_POINT=1, /* Target is a point | */ - CAMERA_TRACKING_RECTANGLE=2, /* Target is a rectangle | */ + CAMERA_TRACKING_MODE_NONE=0, /* Not tracking | */ + CAMERA_TRACKING_MODE_POINT=1, /* Target is a point | */ + CAMERA_TRACKING_MODE_RECTANGLE=2, /* Target is a rectangle | */ CAMERA_TRACKING_MODE_ENUM_END=3, /* | */ } CAMERA_TRACKING_MODE; #endif @@ -1692,10 +1472,10 @@ typedef enum CAMERA_TRACKING_MODE #define HAVE_ENUM_CAMERA_TRACKING_TARGET_DATA typedef enum CAMERA_TRACKING_TARGET_DATA { - CAMERA_TRACKING_TARGET_NONE=0, /* No target data | */ - CAMERA_TRACKING_TARGET_EMBEDDED=1, /* Target data embedded in image data (proprietary) | */ - CAMERA_TRACKING_TARGET_RENDERED=2, /* Target data rendered in image | */ - CAMERA_TRACKING_TARGET_IN_STATUS=4, /* Target data within status message (Point or Rectangle) | */ + CAMERA_TRACKING_TARGET_DATA_NONE=0, /* No target data | */ + CAMERA_TRACKING_TARGET_DATA_EMBEDDED=1, /* Target data embedded in image data (proprietary) | */ + CAMERA_TRACKING_TARGET_DATA_RENDERED=2, /* Target data rendered in image | */ + CAMERA_TRACKING_TARGET_DATA_IN_STATUS=4, /* Target data within status message (Point or Rectangle) | */ CAMERA_TRACKING_TARGET_DATA_ENUM_END=5, /* | */ } CAMERA_TRACKING_TARGET_DATA; #endif @@ -1798,6 +1578,20 @@ typedef enum POSITION_TARGET_TYPEMASK } POSITION_TARGET_TYPEMASK; #endif +/** @brief Bitmap to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates that none of the setpoint dimensions should be ignored. */ +#ifndef HAVE_ENUM_ATTITUDE_TARGET_TYPEMASK +#define HAVE_ENUM_ATTITUDE_TARGET_TYPEMASK +typedef enum ATTITUDE_TARGET_TYPEMASK +{ + ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE=1, /* Ignore body roll rate | */ + ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE=2, /* Ignore body pitch rate | */ + ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE=4, /* Ignore body yaw rate | */ + ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE=64, /* Ignore throttle | */ + ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE=128, /* Ignore attitude | */ + ATTITUDE_TARGET_TYPEMASK_ENUM_END=129, /* | */ +} ATTITUDE_TARGET_TYPEMASK; +#endif + /** @brief Airborne status of UAS. */ #ifndef HAVE_ENUM_UTM_FLIGHT_STATE #define HAVE_ENUM_UTM_FLIGHT_STATE @@ -2380,6 +2174,23 @@ typedef enum MAV_WINCH_STATUS_FLAG } MAV_WINCH_STATUS_FLAG; #endif +/** @brief */ +#ifndef HAVE_ENUM_MAG_CAL_STATUS +#define HAVE_ENUM_MAG_CAL_STATUS +typedef enum MAG_CAL_STATUS +{ + MAG_CAL_NOT_STARTED=0, /* | */ + MAG_CAL_WAITING_TO_START=1, /* | */ + MAG_CAL_RUNNING_STEP_ONE=2, /* | */ + MAG_CAL_RUNNING_STEP_TWO=3, /* | */ + MAG_CAL_SUCCESS=4, /* | */ + MAG_CAL_FAILED=5, /* | */ + MAG_CAL_BAD_ORIENTATION=6, /* | */ + MAG_CAL_BAD_RADIUS=7, /* | */ + MAG_CAL_STATUS_ENUM_END=8, /* | */ +} MAG_CAL_STATUS; +#endif + // MAVLINK VERSION #ifndef MAVLINK_VERSION @@ -2392,7 +2203,6 @@ typedef enum MAV_WINCH_STATUS_FLAG #endif // MESSAGE DEFINITIONS -#include "./mavlink_msg_heartbeat.h" #include "./mavlink_msg_sys_status.h" #include "./mavlink_msg_system_time.h" #include "./mavlink_msg_ping.h" @@ -2514,6 +2324,8 @@ typedef enum MAV_WINCH_STATUS_FLAG #include "./mavlink_msg_autopilot_version.h" #include "./mavlink_msg_landing_target.h" #include "./mavlink_msg_fence_status.h" +#include "./mavlink_msg_mag_cal_report.h" +#include "./mavlink_msg_efi_status.h" #include "./mavlink_msg_estimator_status.h" #include "./mavlink_msg_wind_cov.h" #include "./mavlink_msg_gps_input.h" @@ -2534,16 +2346,83 @@ typedef enum MAV_WINCH_STATUS_FLAG #include "./mavlink_msg_named_value_int.h" #include "./mavlink_msg_statustext.h" #include "./mavlink_msg_debug.h" +#include "./mavlink_msg_setup_signing.h" +#include "./mavlink_msg_button_change.h" +#include "./mavlink_msg_play_tune.h" +#include "./mavlink_msg_camera_information.h" +#include "./mavlink_msg_camera_settings.h" +#include "./mavlink_msg_storage_information.h" +#include "./mavlink_msg_camera_capture_status.h" +#include "./mavlink_msg_camera_image_captured.h" +#include "./mavlink_msg_flight_information.h" +#include "./mavlink_msg_mount_orientation.h" +#include "./mavlink_msg_logging_data.h" +#include "./mavlink_msg_logging_data_acked.h" +#include "./mavlink_msg_logging_ack.h" +#include "./mavlink_msg_video_stream_information.h" +#include "./mavlink_msg_video_stream_status.h" +#include "./mavlink_msg_camera_fov_status.h" +#include "./mavlink_msg_camera_tracking_image_status.h" +#include "./mavlink_msg_camera_tracking_geo_status.h" +#include "./mavlink_msg_gimbal_manager_information.h" +#include "./mavlink_msg_gimbal_manager_status.h" +#include "./mavlink_msg_gimbal_manager_set_attitude.h" +#include "./mavlink_msg_gimbal_device_information.h" +#include "./mavlink_msg_gimbal_device_set_attitude.h" +#include "./mavlink_msg_gimbal_device_attitude_status.h" +#include "./mavlink_msg_autopilot_state_for_gimbal_device.h" +#include "./mavlink_msg_gimbal_manager_set_pitchyaw.h" +#include "./mavlink_msg_gimbal_manager_set_manual_control.h" +#include "./mavlink_msg_esc_info.h" +#include "./mavlink_msg_esc_status.h" +#include "./mavlink_msg_wifi_config_ap.h" +#include "./mavlink_msg_ais_vessel.h" +#include "./mavlink_msg_uavcan_node_status.h" +#include "./mavlink_msg_uavcan_node_info.h" +#include "./mavlink_msg_param_ext_request_read.h" +#include "./mavlink_msg_param_ext_request_list.h" +#include "./mavlink_msg_param_ext_value.h" +#include "./mavlink_msg_param_ext_set.h" +#include "./mavlink_msg_param_ext_ack.h" +#include "./mavlink_msg_obstacle_distance.h" +#include "./mavlink_msg_odometry.h" +#include "./mavlink_msg_trajectory_representation_waypoints.h" +#include "./mavlink_msg_trajectory_representation_bezier.h" +#include "./mavlink_msg_cellular_status.h" +#include "./mavlink_msg_isbd_link_status.h" +#include "./mavlink_msg_cellular_config.h" +#include "./mavlink_msg_raw_rpm.h" +#include "./mavlink_msg_utm_global_position.h" +#include "./mavlink_msg_debug_float_array.h" +#include "./mavlink_msg_orbit_execution_status.h" +#include "./mavlink_msg_smart_battery_info.h" +#include "./mavlink_msg_generator_status.h" +#include "./mavlink_msg_actuator_output_status.h" +#include "./mavlink_msg_time_estimate_to_target.h" +#include "./mavlink_msg_tunnel.h" +#include "./mavlink_msg_onboard_computer_status.h" +#include "./mavlink_msg_component_information.h" +#include "./mavlink_msg_play_tune_v2.h" +#include "./mavlink_msg_supported_tunes.h" +#include "./mavlink_msg_wheel_distance.h" +#include "./mavlink_msg_winch_status.h" +#include "./mavlink_msg_open_drone_id_basic_id.h" +#include "./mavlink_msg_open_drone_id_location.h" +#include "./mavlink_msg_open_drone_id_authentication.h" +#include "./mavlink_msg_open_drone_id_self_id.h" +#include "./mavlink_msg_open_drone_id_system.h" +#include "./mavlink_msg_open_drone_id_operator_id.h" +#include "./mavlink_msg_open_drone_id_message_pack.h" // base include - +#include "../minimal/minimal.h" #undef MAVLINK_THIS_XML_IDX #define MAVLINK_THIS_XML_IDX 0 #if MAVLINK_THIS_XML_IDX == MAVLINK_PRIMARY_XML_IDX -# define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, MAVLINK_MESSAGE_INFO_LINK_NODE_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_ACK_TRANSACTION, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_PARAM_MAP_RC, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_INT, MAVLINK_MESSAGE_INFO_MISSION_CHANGED, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION_COV, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT_COV, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_COV, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_INT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_CANCEL, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_POSITION_TARGET_GLOBAL_INT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, MAVLINK_MESSAGE_INFO_HIL_ACTUATOR_CONTROLS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW_RAD, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_PROTOCOL, MAVLINK_MESSAGE_INFO_TIMESYNC, MAVLINK_MESSAGE_INFO_CAMERA_TRIGGER, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, MAVLINK_MESSAGE_INFO_GPS_RTK, MAVLINK_MESSAGE_INFO_GPS2_RTK, MAVLINK_MESSAGE_INFO_SCALED_IMU3, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, MAVLINK_MESSAGE_INFO_TERRAIN_REQUEST, MAVLINK_MESSAGE_INFO_TERRAIN_DATA, MAVLINK_MESSAGE_INFO_TERRAIN_CHECK, MAVLINK_MESSAGE_INFO_TERRAIN_REPORT, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE2, MAVLINK_MESSAGE_INFO_ATT_POS_MOCAP, MAVLINK_MESSAGE_INFO_SET_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ALTITUDE, MAVLINK_MESSAGE_INFO_RESOURCE_REQUEST, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE3, MAVLINK_MESSAGE_INFO_FOLLOW_TARGET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CONTROL_SYSTEM_STATE, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION, MAVLINK_MESSAGE_INFO_LANDING_TARGET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ESTIMATOR_STATUS, MAVLINK_MESSAGE_INFO_WIND_COV, MAVLINK_MESSAGE_INFO_GPS_INPUT, MAVLINK_MESSAGE_INFO_GPS_RTCM_DATA, MAVLINK_MESSAGE_INFO_HIGH_LATENCY, MAVLINK_MESSAGE_INFO_HIGH_LATENCY2, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VIBRATION, MAVLINK_MESSAGE_INFO_HOME_POSITION, MAVLINK_MESSAGE_INFO_SET_HOME_POSITION, MAVLINK_MESSAGE_INFO_MESSAGE_INTERVAL, MAVLINK_MESSAGE_INFO_EXTENDED_SYS_STATE, MAVLINK_MESSAGE_INFO_ADSB_VEHICLE, MAVLINK_MESSAGE_INFO_COLLISION, MAVLINK_MESSAGE_INFO_V2_EXTENSION, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} -# define MAVLINK_MESSAGE_NAMES {{ "ACTUATOR_CONTROL_TARGET", 140 }, { "ADSB_VEHICLE", 246 }, { "ALTITUDE", 141 }, { "ATTITUDE", 30 }, { "ATTITUDE_QUATERNION", 31 }, { "ATTITUDE_QUATERNION_COV", 61 }, { "ATTITUDE_TARGET", 83 }, { "ATT_POS_MOCAP", 138 }, { "AUTH_KEY", 7 }, { "AUTOPILOT_VERSION", 148 }, { "BATTERY_STATUS", 147 }, { "CAMERA_TRIGGER", 112 }, { "CHANGE_OPERATOR_CONTROL", 5 }, { "CHANGE_OPERATOR_CONTROL_ACK", 6 }, { "COLLISION", 247 }, { "COMMAND_ACK", 77 }, { "COMMAND_CANCEL", 80 }, { "COMMAND_INT", 75 }, { "COMMAND_LONG", 76 }, { "CONTROL_SYSTEM_STATE", 146 }, { "DATA_STREAM", 67 }, { "DATA_TRANSMISSION_HANDSHAKE", 130 }, { "DEBUG", 254 }, { "DEBUG_VECT", 250 }, { "DISTANCE_SENSOR", 132 }, { "ENCAPSULATED_DATA", 131 }, { "ESTIMATOR_STATUS", 230 }, { "EXTENDED_SYS_STATE", 245 }, { "FENCE_STATUS", 162 }, { "FILE_TRANSFER_PROTOCOL", 110 }, { "FOLLOW_TARGET", 144 }, { "GLOBAL_POSITION_INT", 33 }, { "GLOBAL_POSITION_INT_COV", 63 }, { "GLOBAL_VISION_POSITION_ESTIMATE", 101 }, { "GPS2_RAW", 124 }, { "GPS2_RTK", 128 }, { "GPS_GLOBAL_ORIGIN", 49 }, { "GPS_INJECT_DATA", 123 }, { "GPS_INPUT", 232 }, { "GPS_RAW_INT", 24 }, { "GPS_RTCM_DATA", 233 }, { "GPS_RTK", 127 }, { "GPS_STATUS", 25 }, { "HEARTBEAT", 0 }, { "HIGHRES_IMU", 105 }, { "HIGH_LATENCY", 234 }, { "HIGH_LATENCY2", 235 }, { "HIL_ACTUATOR_CONTROLS", 93 }, { "HIL_CONTROLS", 91 }, { "HIL_GPS", 113 }, { "HIL_OPTICAL_FLOW", 114 }, { "HIL_RC_INPUTS_RAW", 92 }, { "HIL_SENSOR", 107 }, { "HIL_STATE", 90 }, { "HIL_STATE_QUATERNION", 115 }, { "HOME_POSITION", 242 }, { "LANDING_TARGET", 149 }, { "LINK_NODE_STATUS", 8 }, { "LOCAL_POSITION_NED", 32 }, { "LOCAL_POSITION_NED_COV", 64 }, { "LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET", 89 }, { "LOG_DATA", 120 }, { "LOG_ENTRY", 118 }, { "LOG_ERASE", 121 }, { "LOG_REQUEST_DATA", 119 }, { "LOG_REQUEST_END", 122 }, { "LOG_REQUEST_LIST", 117 }, { "MANUAL_CONTROL", 69 }, { "MANUAL_SETPOINT", 81 }, { "MEMORY_VECT", 249 }, { "MESSAGE_INTERVAL", 244 }, { "MISSION_ACK", 47 }, { "MISSION_CHANGED", 52 }, { "MISSION_CLEAR_ALL", 45 }, { "MISSION_COUNT", 44 }, { "MISSION_CURRENT", 42 }, { "MISSION_ITEM", 39 }, { "MISSION_ITEM_INT", 73 }, { "MISSION_ITEM_REACHED", 46 }, { "MISSION_REQUEST", 40 }, { "MISSION_REQUEST_INT", 51 }, { "MISSION_REQUEST_LIST", 43 }, { "MISSION_REQUEST_PARTIAL_LIST", 37 }, { "MISSION_SET_CURRENT", 41 }, { "MISSION_WRITE_PARTIAL_LIST", 38 }, { "NAMED_VALUE_FLOAT", 251 }, { "NAMED_VALUE_INT", 252 }, { "NAV_CONTROLLER_OUTPUT", 62 }, { "OPTICAL_FLOW", 100 }, { "OPTICAL_FLOW_RAD", 106 }, { "PARAM_ACK_TRANSACTION", 19 }, { "PARAM_MAP_RC", 50 }, { "PARAM_REQUEST_LIST", 21 }, { "PARAM_REQUEST_READ", 20 }, { "PARAM_SET", 23 }, { "PARAM_VALUE", 22 }, { "PING", 4 }, { "POSITION_TARGET_GLOBAL_INT", 87 }, { "POSITION_TARGET_LOCAL_NED", 85 }, { "POWER_STATUS", 125 }, { "RADIO_STATUS", 109 }, { "RAW_IMU", 27 }, { "RAW_PRESSURE", 28 }, { "RC_CHANNELS", 65 }, { "RC_CHANNELS_OVERRIDE", 70 }, { "RC_CHANNELS_RAW", 35 }, { "RC_CHANNELS_SCALED", 34 }, { "REQUEST_DATA_STREAM", 66 }, { "RESOURCE_REQUEST", 142 }, { "SAFETY_ALLOWED_AREA", 55 }, { "SAFETY_SET_ALLOWED_AREA", 54 }, { "SCALED_IMU", 26 }, { "SCALED_IMU2", 116 }, { "SCALED_IMU3", 129 }, { "SCALED_PRESSURE", 29 }, { "SCALED_PRESSURE2", 137 }, { "SCALED_PRESSURE3", 143 }, { "SERIAL_CONTROL", 126 }, { "SERVO_OUTPUT_RAW", 36 }, { "SET_ACTUATOR_CONTROL_TARGET", 139 }, { "SET_ATTITUDE_TARGET", 82 }, { "SET_GPS_GLOBAL_ORIGIN", 48 }, { "SET_HOME_POSITION", 243 }, { "SET_MODE", 11 }, { "SET_POSITION_TARGET_GLOBAL_INT", 86 }, { "SET_POSITION_TARGET_LOCAL_NED", 84 }, { "SIM_STATE", 108 }, { "STATUSTEXT", 253 }, { "SYSTEM_TIME", 2 }, { "SYS_STATUS", 1 }, { "TERRAIN_CHECK", 135 }, { "TERRAIN_DATA", 134 }, { "TERRAIN_REPORT", 136 }, { "TERRAIN_REQUEST", 133 }, { "TIMESYNC", 111 }, { "V2_EXTENSION", 248 }, { "VFR_HUD", 74 }, { "VIBRATION", 241 }, { "VICON_POSITION_ESTIMATE", 104 }, { "VISION_POSITION_ESTIMATE", 102 }, { "VISION_SPEED_ESTIMATE", 103 }, { "WIND_COV", 231 }} +# define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, MAVLINK_MESSAGE_INFO_LINK_NODE_STATUS, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_PARAM_ACK_TRANSACTION, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_PARAM_MAP_RC, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_INT, MAVLINK_MESSAGE_INFO_MISSION_CHANGED, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION_COV, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT_COV, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_COV, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_INT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, MAVLINK_MESSAGE_INFO_COMMAND_CANCEL, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, MAVLINK_MESSAGE_INFO_HIL_ACTUATOR_CONTROLS, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW_RAD, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_PROTOCOL, MAVLINK_MESSAGE_INFO_TIMESYNC, MAVLINK_MESSAGE_INFO_CAMERA_TRIGGER, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, MAVLINK_MESSAGE_INFO_GPS_RTK, MAVLINK_MESSAGE_INFO_GPS2_RTK, MAVLINK_MESSAGE_INFO_SCALED_IMU3, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, MAVLINK_MESSAGE_INFO_TERRAIN_REQUEST, MAVLINK_MESSAGE_INFO_TERRAIN_DATA, MAVLINK_MESSAGE_INFO_TERRAIN_CHECK, MAVLINK_MESSAGE_INFO_TERRAIN_REPORT, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE2, MAVLINK_MESSAGE_INFO_ATT_POS_MOCAP, MAVLINK_MESSAGE_INFO_SET_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ALTITUDE, MAVLINK_MESSAGE_INFO_RESOURCE_REQUEST, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE3, MAVLINK_MESSAGE_INFO_FOLLOW_TARGET, MAVLINK_MESSAGE_INFO_CONTROL_SYSTEM_STATE, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION, MAVLINK_MESSAGE_INFO_LANDING_TARGET, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT, MAVLINK_MESSAGE_INFO_EFI_STATUS, MAVLINK_MESSAGE_INFO_ESTIMATOR_STATUS, MAVLINK_MESSAGE_INFO_WIND_COV, MAVLINK_MESSAGE_INFO_GPS_INPUT, MAVLINK_MESSAGE_INFO_GPS_RTCM_DATA, MAVLINK_MESSAGE_INFO_HIGH_LATENCY, MAVLINK_MESSAGE_INFO_HIGH_LATENCY2, MAVLINK_MESSAGE_INFO_VIBRATION, MAVLINK_MESSAGE_INFO_HOME_POSITION, MAVLINK_MESSAGE_INFO_SET_HOME_POSITION, MAVLINK_MESSAGE_INFO_MESSAGE_INTERVAL, MAVLINK_MESSAGE_INFO_EXTENDED_SYS_STATE, MAVLINK_MESSAGE_INFO_ADSB_VEHICLE, MAVLINK_MESSAGE_INFO_COLLISION, MAVLINK_MESSAGE_INFO_V2_EXTENSION, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, MAVLINK_MESSAGE_INFO_SETUP_SIGNING, MAVLINK_MESSAGE_INFO_BUTTON_CHANGE, MAVLINK_MESSAGE_INFO_PLAY_TUNE, MAVLINK_MESSAGE_INFO_CAMERA_INFORMATION, MAVLINK_MESSAGE_INFO_CAMERA_SETTINGS, MAVLINK_MESSAGE_INFO_STORAGE_INFORMATION, MAVLINK_MESSAGE_INFO_CAMERA_CAPTURE_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_IMAGE_CAPTURED, MAVLINK_MESSAGE_INFO_FLIGHT_INFORMATION, MAVLINK_MESSAGE_INFO_MOUNT_ORIENTATION, MAVLINK_MESSAGE_INFO_LOGGING_DATA, MAVLINK_MESSAGE_INFO_LOGGING_DATA_ACKED, MAVLINK_MESSAGE_INFO_LOGGING_ACK, MAVLINK_MESSAGE_INFO_VIDEO_STREAM_INFORMATION, MAVLINK_MESSAGE_INFO_VIDEO_STREAM_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_FOV_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_IMAGE_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_GEO_STATUS, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_INFORMATION, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_STATUS, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_ATTITUDE, MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_INFORMATION, MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_SET_ATTITUDE, MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_ATTITUDE_STATUS, MAVLINK_MESSAGE_INFO_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_PITCHYAW, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_ESC_INFO, MAVLINK_MESSAGE_INFO_ESC_STATUS, MAVLINK_MESSAGE_INFO_WIFI_CONFIG_AP, MAVLINK_MESSAGE_INFO_PROTOCOL_VERSION, MAVLINK_MESSAGE_INFO_AIS_VESSEL, MAVLINK_MESSAGE_INFO_UAVCAN_NODE_STATUS, MAVLINK_MESSAGE_INFO_UAVCAN_NODE_INFO, MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_EXT_VALUE, MAVLINK_MESSAGE_INFO_PARAM_EXT_SET, MAVLINK_MESSAGE_INFO_PARAM_EXT_ACK, MAVLINK_MESSAGE_INFO_OBSTACLE_DISTANCE, MAVLINK_MESSAGE_INFO_ODOMETRY, MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_WAYPOINTS, MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_BEZIER, MAVLINK_MESSAGE_INFO_CELLULAR_STATUS, MAVLINK_MESSAGE_INFO_ISBD_LINK_STATUS, MAVLINK_MESSAGE_INFO_CELLULAR_CONFIG, MAVLINK_MESSAGE_INFO_RAW_RPM, MAVLINK_MESSAGE_INFO_UTM_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_DEBUG_FLOAT_ARRAY, MAVLINK_MESSAGE_INFO_ORBIT_EXECUTION_STATUS, MAVLINK_MESSAGE_INFO_SMART_BATTERY_INFO, MAVLINK_MESSAGE_INFO_GENERATOR_STATUS, MAVLINK_MESSAGE_INFO_ACTUATOR_OUTPUT_STATUS, MAVLINK_MESSAGE_INFO_TIME_ESTIMATE_TO_TARGET, MAVLINK_MESSAGE_INFO_TUNNEL, MAVLINK_MESSAGE_INFO_ONBOARD_COMPUTER_STATUS, MAVLINK_MESSAGE_INFO_COMPONENT_INFORMATION, MAVLINK_MESSAGE_INFO_PLAY_TUNE_V2, MAVLINK_MESSAGE_INFO_SUPPORTED_TUNES, MAVLINK_MESSAGE_INFO_WHEEL_DISTANCE, MAVLINK_MESSAGE_INFO_WINCH_STATUS, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_BASIC_ID, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_LOCATION, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_AUTHENTICATION, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SELF_ID, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SYSTEM, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_OPERATOR_ID, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_MESSAGE_PACK} +# define MAVLINK_MESSAGE_NAMES {{ "ACTUATOR_CONTROL_TARGET", 140 }, { "ACTUATOR_OUTPUT_STATUS", 375 }, { "ADSB_VEHICLE", 246 }, { "AIS_VESSEL", 301 }, { "ALTITUDE", 141 }, { "ATTITUDE", 30 }, { "ATTITUDE_QUATERNION", 31 }, { "ATTITUDE_QUATERNION_COV", 61 }, { "ATTITUDE_TARGET", 83 }, { "ATT_POS_MOCAP", 138 }, { "AUTH_KEY", 7 }, { "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE", 286 }, { "AUTOPILOT_VERSION", 148 }, { "BATTERY_STATUS", 147 }, { "BUTTON_CHANGE", 257 }, { "CAMERA_CAPTURE_STATUS", 262 }, { "CAMERA_FOV_STATUS", 271 }, { "CAMERA_IMAGE_CAPTURED", 263 }, { "CAMERA_INFORMATION", 259 }, { "CAMERA_SETTINGS", 260 }, { "CAMERA_TRACKING_GEO_STATUS", 276 }, { "CAMERA_TRACKING_IMAGE_STATUS", 275 }, { "CAMERA_TRIGGER", 112 }, { "CELLULAR_CONFIG", 336 }, { "CELLULAR_STATUS", 334 }, { "CHANGE_OPERATOR_CONTROL", 5 }, { "CHANGE_OPERATOR_CONTROL_ACK", 6 }, { "COLLISION", 247 }, { "COMMAND_ACK", 77 }, { "COMMAND_CANCEL", 80 }, { "COMMAND_INT", 75 }, { "COMMAND_LONG", 76 }, { "COMPONENT_INFORMATION", 395 }, { "CONTROL_SYSTEM_STATE", 146 }, { "DATA_STREAM", 67 }, { "DATA_TRANSMISSION_HANDSHAKE", 130 }, { "DEBUG", 254 }, { "DEBUG_FLOAT_ARRAY", 350 }, { "DEBUG_VECT", 250 }, { "DISTANCE_SENSOR", 132 }, { "EFI_STATUS", 225 }, { "ENCAPSULATED_DATA", 131 }, { "ESC_INFO", 290 }, { "ESC_STATUS", 291 }, { "ESTIMATOR_STATUS", 230 }, { "EXTENDED_SYS_STATE", 245 }, { "FENCE_STATUS", 162 }, { "FILE_TRANSFER_PROTOCOL", 110 }, { "FLIGHT_INFORMATION", 264 }, { "FOLLOW_TARGET", 144 }, { "GENERATOR_STATUS", 373 }, { "GIMBAL_DEVICE_ATTITUDE_STATUS", 285 }, { "GIMBAL_DEVICE_INFORMATION", 283 }, { "GIMBAL_DEVICE_SET_ATTITUDE", 284 }, { "GIMBAL_MANAGER_INFORMATION", 280 }, { "GIMBAL_MANAGER_SET_ATTITUDE", 282 }, { "GIMBAL_MANAGER_SET_MANUAL_CONTROL", 288 }, { "GIMBAL_MANAGER_SET_PITCHYAW", 287 }, { "GIMBAL_MANAGER_STATUS", 281 }, { "GLOBAL_POSITION_INT", 33 }, { "GLOBAL_POSITION_INT_COV", 63 }, { "GLOBAL_VISION_POSITION_ESTIMATE", 101 }, { "GPS2_RAW", 124 }, { "GPS2_RTK", 128 }, { "GPS_GLOBAL_ORIGIN", 49 }, { "GPS_INJECT_DATA", 123 }, { "GPS_INPUT", 232 }, { "GPS_RAW_INT", 24 }, { "GPS_RTCM_DATA", 233 }, { "GPS_RTK", 127 }, { "GPS_STATUS", 25 }, { "HEARTBEAT", 0 }, { "HIGHRES_IMU", 105 }, { "HIGH_LATENCY", 234 }, { "HIGH_LATENCY2", 235 }, { "HIL_ACTUATOR_CONTROLS", 93 }, { "HIL_CONTROLS", 91 }, { "HIL_GPS", 113 }, { "HIL_OPTICAL_FLOW", 114 }, { "HIL_RC_INPUTS_RAW", 92 }, { "HIL_SENSOR", 107 }, { "HIL_STATE", 90 }, { "HIL_STATE_QUATERNION", 115 }, { "HOME_POSITION", 242 }, { "ISBD_LINK_STATUS", 335 }, { "LANDING_TARGET", 149 }, { "LINK_NODE_STATUS", 8 }, { "LOCAL_POSITION_NED", 32 }, { "LOCAL_POSITION_NED_COV", 64 }, { "LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET", 89 }, { "LOGGING_ACK", 268 }, { "LOGGING_DATA", 266 }, { "LOGGING_DATA_ACKED", 267 }, { "LOG_DATA", 120 }, { "LOG_ENTRY", 118 }, { "LOG_ERASE", 121 }, { "LOG_REQUEST_DATA", 119 }, { "LOG_REQUEST_END", 122 }, { "LOG_REQUEST_LIST", 117 }, { "MAG_CAL_REPORT", 192 }, { "MANUAL_CONTROL", 69 }, { "MANUAL_SETPOINT", 81 }, { "MEMORY_VECT", 249 }, { "MESSAGE_INTERVAL", 244 }, { "MISSION_ACK", 47 }, { "MISSION_CHANGED", 52 }, { "MISSION_CLEAR_ALL", 45 }, { "MISSION_COUNT", 44 }, { "MISSION_CURRENT", 42 }, { "MISSION_ITEM", 39 }, { "MISSION_ITEM_INT", 73 }, { "MISSION_ITEM_REACHED", 46 }, { "MISSION_REQUEST", 40 }, { "MISSION_REQUEST_INT", 51 }, { "MISSION_REQUEST_LIST", 43 }, { "MISSION_REQUEST_PARTIAL_LIST", 37 }, { "MISSION_SET_CURRENT", 41 }, { "MISSION_WRITE_PARTIAL_LIST", 38 }, { "MOUNT_ORIENTATION", 265 }, { "NAMED_VALUE_FLOAT", 251 }, { "NAMED_VALUE_INT", 252 }, { "NAV_CONTROLLER_OUTPUT", 62 }, { "OBSTACLE_DISTANCE", 330 }, { "ODOMETRY", 331 }, { "ONBOARD_COMPUTER_STATUS", 390 }, { "OPEN_DRONE_ID_AUTHENTICATION", 12902 }, { "OPEN_DRONE_ID_BASIC_ID", 12900 }, { "OPEN_DRONE_ID_LOCATION", 12901 }, { "OPEN_DRONE_ID_MESSAGE_PACK", 12915 }, { "OPEN_DRONE_ID_OPERATOR_ID", 12905 }, { "OPEN_DRONE_ID_SELF_ID", 12903 }, { "OPEN_DRONE_ID_SYSTEM", 12904 }, { "OPTICAL_FLOW", 100 }, { "OPTICAL_FLOW_RAD", 106 }, { "ORBIT_EXECUTION_STATUS", 360 }, { "PARAM_ACK_TRANSACTION", 19 }, { "PARAM_EXT_ACK", 324 }, { "PARAM_EXT_REQUEST_LIST", 321 }, { "PARAM_EXT_REQUEST_READ", 320 }, { "PARAM_EXT_SET", 323 }, { "PARAM_EXT_VALUE", 322 }, { "PARAM_MAP_RC", 50 }, { "PARAM_REQUEST_LIST", 21 }, { "PARAM_REQUEST_READ", 20 }, { "PARAM_SET", 23 }, { "PARAM_VALUE", 22 }, { "PING", 4 }, { "PLAY_TUNE", 258 }, { "PLAY_TUNE_V2", 400 }, { "POSITION_TARGET_GLOBAL_INT", 87 }, { "POSITION_TARGET_LOCAL_NED", 85 }, { "POWER_STATUS", 125 }, { "PROTOCOL_VERSION", 300 }, { "RADIO_STATUS", 109 }, { "RAW_IMU", 27 }, { "RAW_PRESSURE", 28 }, { "RAW_RPM", 339 }, { "RC_CHANNELS", 65 }, { "RC_CHANNELS_OVERRIDE", 70 }, { "RC_CHANNELS_RAW", 35 }, { "RC_CHANNELS_SCALED", 34 }, { "REQUEST_DATA_STREAM", 66 }, { "RESOURCE_REQUEST", 142 }, { "SAFETY_ALLOWED_AREA", 55 }, { "SAFETY_SET_ALLOWED_AREA", 54 }, { "SCALED_IMU", 26 }, { "SCALED_IMU2", 116 }, { "SCALED_IMU3", 129 }, { "SCALED_PRESSURE", 29 }, { "SCALED_PRESSURE2", 137 }, { "SCALED_PRESSURE3", 143 }, { "SERIAL_CONTROL", 126 }, { "SERVO_OUTPUT_RAW", 36 }, { "SETUP_SIGNING", 256 }, { "SET_ACTUATOR_CONTROL_TARGET", 139 }, { "SET_ATTITUDE_TARGET", 82 }, { "SET_GPS_GLOBAL_ORIGIN", 48 }, { "SET_HOME_POSITION", 243 }, { "SET_MODE", 11 }, { "SET_POSITION_TARGET_GLOBAL_INT", 86 }, { "SET_POSITION_TARGET_LOCAL_NED", 84 }, { "SIM_STATE", 108 }, { "SMART_BATTERY_INFO", 370 }, { "STATUSTEXT", 253 }, { "STORAGE_INFORMATION", 261 }, { "SUPPORTED_TUNES", 401 }, { "SYSTEM_TIME", 2 }, { "SYS_STATUS", 1 }, { "TERRAIN_CHECK", 135 }, { "TERRAIN_DATA", 134 }, { "TERRAIN_REPORT", 136 }, { "TERRAIN_REQUEST", 133 }, { "TIMESYNC", 111 }, { "TIME_ESTIMATE_TO_TARGET", 380 }, { "TRAJECTORY_REPRESENTATION_BEZIER", 333 }, { "TRAJECTORY_REPRESENTATION_WAYPOINTS", 332 }, { "TUNNEL", 385 }, { "UAVCAN_NODE_INFO", 311 }, { "UAVCAN_NODE_STATUS", 310 }, { "UTM_GLOBAL_POSITION", 340 }, { "V2_EXTENSION", 248 }, { "VFR_HUD", 74 }, { "VIBRATION", 241 }, { "VICON_POSITION_ESTIMATE", 104 }, { "VIDEO_STREAM_INFORMATION", 269 }, { "VIDEO_STREAM_STATUS", 270 }, { "VISION_POSITION_ESTIMATE", 102 }, { "VISION_SPEED_ESTIMATE", 103 }, { "WHEEL_DISTANCE", 9000 }, { "WIFI_CONFIG_AP", 299 }, { "WINCH_STATUS", 9005 }, { "WIND_COV", 231 }} # if MAVLINK_COMMAND_24BIT # include "../mavlink_get_info.h" # endif diff --git a/lib/main/MAVLink/common/mavlink.h b/lib/main/MAVLink/common/mavlink.h index 35163d3459..39d85d2d5f 100755 --- a/lib/main/MAVLink/common/mavlink.h +++ b/lib/main/MAVLink/common/mavlink.h @@ -9,7 +9,7 @@ #define MAVLINK_PRIMARY_XML_IDX 0 #ifndef MAVLINK_STX -#define MAVLINK_STX 254 +#define MAVLINK_STX 253 #endif #ifndef MAVLINK_ENDIAN @@ -25,7 +25,7 @@ #endif #ifndef MAVLINK_COMMAND_24BIT -#define MAVLINK_COMMAND_24BIT 0 +#define MAVLINK_COMMAND_24BIT 1 #endif #include "version.h" diff --git a/lib/main/MAVLink/common/mavlink_msg_actuator_output_status.h b/lib/main/MAVLink/common/mavlink_msg_actuator_output_status.h new file mode 100644 index 0000000000..52ee04df75 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_actuator_output_status.h @@ -0,0 +1,255 @@ +#pragma once +// MESSAGE ACTUATOR_OUTPUT_STATUS PACKING + +#define MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS 375 + + +typedef struct __mavlink_actuator_output_status_t { + uint64_t time_usec; /*< [us] Timestamp (since system boot).*/ + uint32_t active; /*< Active outputs*/ + float actuator[32]; /*< Servo / motor output array values. Zero values indicate unused channels.*/ +} mavlink_actuator_output_status_t; + +#define MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN 140 +#define MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN 140 +#define MAVLINK_MSG_ID_375_LEN 140 +#define MAVLINK_MSG_ID_375_MIN_LEN 140 + +#define MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC 251 +#define MAVLINK_MSG_ID_375_CRC 251 + +#define MAVLINK_MSG_ACTUATOR_OUTPUT_STATUS_FIELD_ACTUATOR_LEN 32 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_ACTUATOR_OUTPUT_STATUS { \ + 375, \ + "ACTUATOR_OUTPUT_STATUS", \ + 3, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_actuator_output_status_t, time_usec) }, \ + { "active", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_actuator_output_status_t, active) }, \ + { "actuator", NULL, MAVLINK_TYPE_FLOAT, 32, 12, offsetof(mavlink_actuator_output_status_t, actuator) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_ACTUATOR_OUTPUT_STATUS { \ + "ACTUATOR_OUTPUT_STATUS", \ + 3, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_actuator_output_status_t, time_usec) }, \ + { "active", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_actuator_output_status_t, active) }, \ + { "actuator", NULL, MAVLINK_TYPE_FLOAT, 32, 12, offsetof(mavlink_actuator_output_status_t, actuator) }, \ + } \ +} +#endif + +/** + * @brief Pack a actuator_output_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (since system boot). + * @param active Active outputs + * @param actuator Servo / motor output array values. Zero values indicate unused channels. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_actuator_output_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint32_t active, const float *actuator) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, active); + _mav_put_float_array(buf, 12, actuator, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN); +#else + mavlink_actuator_output_status_t packet; + packet.time_usec = time_usec; + packet.active = active; + mav_array_memcpy(packet.actuator, actuator, sizeof(float)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); +} + +/** + * @brief Pack a actuator_output_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (since system boot). + * @param active Active outputs + * @param actuator Servo / motor output array values. Zero values indicate unused channels. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_actuator_output_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint32_t active,const float *actuator) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, active); + _mav_put_float_array(buf, 12, actuator, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN); +#else + mavlink_actuator_output_status_t packet; + packet.time_usec = time_usec; + packet.active = active; + mav_array_memcpy(packet.actuator, actuator, sizeof(float)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); +} + +/** + * @brief Encode a actuator_output_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param actuator_output_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_actuator_output_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_actuator_output_status_t* actuator_output_status) +{ + return mavlink_msg_actuator_output_status_pack(system_id, component_id, msg, actuator_output_status->time_usec, actuator_output_status->active, actuator_output_status->actuator); +} + +/** + * @brief Encode a actuator_output_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param actuator_output_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_actuator_output_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_actuator_output_status_t* actuator_output_status) +{ + return mavlink_msg_actuator_output_status_pack_chan(system_id, component_id, chan, msg, actuator_output_status->time_usec, actuator_output_status->active, actuator_output_status->actuator); +} + +/** + * @brief Send a actuator_output_status message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (since system boot). + * @param active Active outputs + * @param actuator Servo / motor output array values. Zero values indicate unused channels. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_actuator_output_status_send(mavlink_channel_t chan, uint64_t time_usec, uint32_t active, const float *actuator) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, active); + _mav_put_float_array(buf, 12, actuator, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS, buf, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); +#else + mavlink_actuator_output_status_t packet; + packet.time_usec = time_usec; + packet.active = active; + mav_array_memcpy(packet.actuator, actuator, sizeof(float)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); +#endif +} + +/** + * @brief Send a actuator_output_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_actuator_output_status_send_struct(mavlink_channel_t chan, const mavlink_actuator_output_status_t* actuator_output_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_actuator_output_status_send(chan, actuator_output_status->time_usec, actuator_output_status->active, actuator_output_status->actuator); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS, (const char *)actuator_output_status, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_actuator_output_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint32_t active, const float *actuator) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, active); + _mav_put_float_array(buf, 12, actuator, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS, buf, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); +#else + mavlink_actuator_output_status_t *packet = (mavlink_actuator_output_status_t *)msgbuf; + packet->time_usec = time_usec; + packet->active = active; + mav_array_memcpy(packet->actuator, actuator, sizeof(float)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS, (const char *)packet, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE ACTUATOR_OUTPUT_STATUS UNPACKING + + +/** + * @brief Get field time_usec from actuator_output_status message + * + * @return [us] Timestamp (since system boot). + */ +static inline uint64_t mavlink_msg_actuator_output_status_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field active from actuator_output_status message + * + * @return Active outputs + */ +static inline uint32_t mavlink_msg_actuator_output_status_get_active(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field actuator from actuator_output_status message + * + * @return Servo / motor output array values. Zero values indicate unused channels. + */ +static inline uint16_t mavlink_msg_actuator_output_status_get_actuator(const mavlink_message_t* msg, float *actuator) +{ + return _MAV_RETURN_float_array(msg, actuator, 32, 12); +} + +/** + * @brief Decode a actuator_output_status message into a struct + * + * @param msg The message to decode + * @param actuator_output_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_actuator_output_status_decode(const mavlink_message_t* msg, mavlink_actuator_output_status_t* actuator_output_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + actuator_output_status->time_usec = mavlink_msg_actuator_output_status_get_time_usec(msg); + actuator_output_status->active = mavlink_msg_actuator_output_status_get_active(msg); + mavlink_msg_actuator_output_status_get_actuator(msg, actuator_output_status->actuator); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN? msg->len : MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN; + memset(actuator_output_status, 0, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN); + memcpy(actuator_output_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_ais_vessel.h b/lib/main/MAVLink/common/mavlink_msg_ais_vessel.h new file mode 100644 index 0000000000..e97fe42bcd --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_ais_vessel.h @@ -0,0 +1,606 @@ +#pragma once +// MESSAGE AIS_VESSEL PACKING + +#define MAVLINK_MSG_ID_AIS_VESSEL 301 + + +typedef struct __mavlink_ais_vessel_t { + uint32_t MMSI; /*< Mobile Marine Service Identifier, 9 decimal digits*/ + int32_t lat; /*< [degE7] Latitude*/ + int32_t lon; /*< [degE7] Longitude*/ + uint16_t COG; /*< [cdeg] Course over ground*/ + uint16_t heading; /*< [cdeg] True heading*/ + uint16_t velocity; /*< [cm/s] Speed over ground*/ + uint16_t dimension_bow; /*< [m] Distance from lat/lon location to bow*/ + uint16_t dimension_stern; /*< [m] Distance from lat/lon location to stern*/ + uint16_t tslc; /*< [s] Time since last communication in seconds*/ + uint16_t flags; /*< Bitmask to indicate various statuses including valid data fields*/ + int8_t turn_rate; /*< [cdeg/s] Turn rate*/ + uint8_t navigational_status; /*< Navigational status*/ + uint8_t type; /*< Type of vessels*/ + uint8_t dimension_port; /*< [m] Distance from lat/lon location to port side*/ + uint8_t dimension_starboard; /*< [m] Distance from lat/lon location to starboard side*/ + char callsign[7]; /*< The vessel callsign*/ + char name[20]; /*< The vessel name*/ +} mavlink_ais_vessel_t; + +#define MAVLINK_MSG_ID_AIS_VESSEL_LEN 58 +#define MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN 58 +#define MAVLINK_MSG_ID_301_LEN 58 +#define MAVLINK_MSG_ID_301_MIN_LEN 58 + +#define MAVLINK_MSG_ID_AIS_VESSEL_CRC 243 +#define MAVLINK_MSG_ID_301_CRC 243 + +#define MAVLINK_MSG_AIS_VESSEL_FIELD_CALLSIGN_LEN 7 +#define MAVLINK_MSG_AIS_VESSEL_FIELD_NAME_LEN 20 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_AIS_VESSEL { \ + 301, \ + "AIS_VESSEL", \ + 17, \ + { { "MMSI", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_ais_vessel_t, MMSI) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_ais_vessel_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_ais_vessel_t, lon) }, \ + { "COG", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_ais_vessel_t, COG) }, \ + { "heading", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_ais_vessel_t, heading) }, \ + { "velocity", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_ais_vessel_t, velocity) }, \ + { "turn_rate", NULL, MAVLINK_TYPE_INT8_T, 0, 26, offsetof(mavlink_ais_vessel_t, turn_rate) }, \ + { "navigational_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_ais_vessel_t, navigational_status) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_ais_vessel_t, type) }, \ + { "dimension_bow", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_ais_vessel_t, dimension_bow) }, \ + { "dimension_stern", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_ais_vessel_t, dimension_stern) }, \ + { "dimension_port", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_ais_vessel_t, dimension_port) }, \ + { "dimension_starboard", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_ais_vessel_t, dimension_starboard) }, \ + { "callsign", NULL, MAVLINK_TYPE_CHAR, 7, 31, offsetof(mavlink_ais_vessel_t, callsign) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 20, 38, offsetof(mavlink_ais_vessel_t, name) }, \ + { "tslc", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_ais_vessel_t, tslc) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_ais_vessel_t, flags) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_AIS_VESSEL { \ + "AIS_VESSEL", \ + 17, \ + { { "MMSI", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_ais_vessel_t, MMSI) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_ais_vessel_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_ais_vessel_t, lon) }, \ + { "COG", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_ais_vessel_t, COG) }, \ + { "heading", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_ais_vessel_t, heading) }, \ + { "velocity", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_ais_vessel_t, velocity) }, \ + { "turn_rate", NULL, MAVLINK_TYPE_INT8_T, 0, 26, offsetof(mavlink_ais_vessel_t, turn_rate) }, \ + { "navigational_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_ais_vessel_t, navigational_status) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_ais_vessel_t, type) }, \ + { "dimension_bow", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_ais_vessel_t, dimension_bow) }, \ + { "dimension_stern", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_ais_vessel_t, dimension_stern) }, \ + { "dimension_port", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_ais_vessel_t, dimension_port) }, \ + { "dimension_starboard", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_ais_vessel_t, dimension_starboard) }, \ + { "callsign", NULL, MAVLINK_TYPE_CHAR, 7, 31, offsetof(mavlink_ais_vessel_t, callsign) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 20, 38, offsetof(mavlink_ais_vessel_t, name) }, \ + { "tslc", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_ais_vessel_t, tslc) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_ais_vessel_t, flags) }, \ + } \ +} +#endif + +/** + * @brief Pack a ais_vessel message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param MMSI Mobile Marine Service Identifier, 9 decimal digits + * @param lat [degE7] Latitude + * @param lon [degE7] Longitude + * @param COG [cdeg] Course over ground + * @param heading [cdeg] True heading + * @param velocity [cm/s] Speed over ground + * @param turn_rate [cdeg/s] Turn rate + * @param navigational_status Navigational status + * @param type Type of vessels + * @param dimension_bow [m] Distance from lat/lon location to bow + * @param dimension_stern [m] Distance from lat/lon location to stern + * @param dimension_port [m] Distance from lat/lon location to port side + * @param dimension_starboard [m] Distance from lat/lon location to starboard side + * @param callsign The vessel callsign + * @param name The vessel name + * @param tslc [s] Time since last communication in seconds + * @param flags Bitmask to indicate various statuses including valid data fields + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ais_vessel_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t MMSI, int32_t lat, int32_t lon, uint16_t COG, uint16_t heading, uint16_t velocity, int8_t turn_rate, uint8_t navigational_status, uint8_t type, uint16_t dimension_bow, uint16_t dimension_stern, uint8_t dimension_port, uint8_t dimension_starboard, const char *callsign, const char *name, uint16_t tslc, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIS_VESSEL_LEN]; + _mav_put_uint32_t(buf, 0, MMSI); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_uint16_t(buf, 12, COG); + _mav_put_uint16_t(buf, 14, heading); + _mav_put_uint16_t(buf, 16, velocity); + _mav_put_uint16_t(buf, 18, dimension_bow); + _mav_put_uint16_t(buf, 20, dimension_stern); + _mav_put_uint16_t(buf, 22, tslc); + _mav_put_uint16_t(buf, 24, flags); + _mav_put_int8_t(buf, 26, turn_rate); + _mav_put_uint8_t(buf, 27, navigational_status); + _mav_put_uint8_t(buf, 28, type); + _mav_put_uint8_t(buf, 29, dimension_port); + _mav_put_uint8_t(buf, 30, dimension_starboard); + _mav_put_char_array(buf, 31, callsign, 7); + _mav_put_char_array(buf, 38, name, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIS_VESSEL_LEN); +#else + mavlink_ais_vessel_t packet; + packet.MMSI = MMSI; + packet.lat = lat; + packet.lon = lon; + packet.COG = COG; + packet.heading = heading; + packet.velocity = velocity; + packet.dimension_bow = dimension_bow; + packet.dimension_stern = dimension_stern; + packet.tslc = tslc; + packet.flags = flags; + packet.turn_rate = turn_rate; + packet.navigational_status = navigational_status; + packet.type = type; + packet.dimension_port = dimension_port; + packet.dimension_starboard = dimension_starboard; + mav_array_memcpy(packet.callsign, callsign, sizeof(char)*7); + mav_array_memcpy(packet.name, name, sizeof(char)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIS_VESSEL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AIS_VESSEL; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); +} + +/** + * @brief Pack a ais_vessel message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param MMSI Mobile Marine Service Identifier, 9 decimal digits + * @param lat [degE7] Latitude + * @param lon [degE7] Longitude + * @param COG [cdeg] Course over ground + * @param heading [cdeg] True heading + * @param velocity [cm/s] Speed over ground + * @param turn_rate [cdeg/s] Turn rate + * @param navigational_status Navigational status + * @param type Type of vessels + * @param dimension_bow [m] Distance from lat/lon location to bow + * @param dimension_stern [m] Distance from lat/lon location to stern + * @param dimension_port [m] Distance from lat/lon location to port side + * @param dimension_starboard [m] Distance from lat/lon location to starboard side + * @param callsign The vessel callsign + * @param name The vessel name + * @param tslc [s] Time since last communication in seconds + * @param flags Bitmask to indicate various statuses including valid data fields + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ais_vessel_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t MMSI,int32_t lat,int32_t lon,uint16_t COG,uint16_t heading,uint16_t velocity,int8_t turn_rate,uint8_t navigational_status,uint8_t type,uint16_t dimension_bow,uint16_t dimension_stern,uint8_t dimension_port,uint8_t dimension_starboard,const char *callsign,const char *name,uint16_t tslc,uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIS_VESSEL_LEN]; + _mav_put_uint32_t(buf, 0, MMSI); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_uint16_t(buf, 12, COG); + _mav_put_uint16_t(buf, 14, heading); + _mav_put_uint16_t(buf, 16, velocity); + _mav_put_uint16_t(buf, 18, dimension_bow); + _mav_put_uint16_t(buf, 20, dimension_stern); + _mav_put_uint16_t(buf, 22, tslc); + _mav_put_uint16_t(buf, 24, flags); + _mav_put_int8_t(buf, 26, turn_rate); + _mav_put_uint8_t(buf, 27, navigational_status); + _mav_put_uint8_t(buf, 28, type); + _mav_put_uint8_t(buf, 29, dimension_port); + _mav_put_uint8_t(buf, 30, dimension_starboard); + _mav_put_char_array(buf, 31, callsign, 7); + _mav_put_char_array(buf, 38, name, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIS_VESSEL_LEN); +#else + mavlink_ais_vessel_t packet; + packet.MMSI = MMSI; + packet.lat = lat; + packet.lon = lon; + packet.COG = COG; + packet.heading = heading; + packet.velocity = velocity; + packet.dimension_bow = dimension_bow; + packet.dimension_stern = dimension_stern; + packet.tslc = tslc; + packet.flags = flags; + packet.turn_rate = turn_rate; + packet.navigational_status = navigational_status; + packet.type = type; + packet.dimension_port = dimension_port; + packet.dimension_starboard = dimension_starboard; + mav_array_memcpy(packet.callsign, callsign, sizeof(char)*7); + mav_array_memcpy(packet.name, name, sizeof(char)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIS_VESSEL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AIS_VESSEL; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); +} + +/** + * @brief Encode a ais_vessel struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param ais_vessel C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ais_vessel_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ais_vessel_t* ais_vessel) +{ + return mavlink_msg_ais_vessel_pack(system_id, component_id, msg, ais_vessel->MMSI, ais_vessel->lat, ais_vessel->lon, ais_vessel->COG, ais_vessel->heading, ais_vessel->velocity, ais_vessel->turn_rate, ais_vessel->navigational_status, ais_vessel->type, ais_vessel->dimension_bow, ais_vessel->dimension_stern, ais_vessel->dimension_port, ais_vessel->dimension_starboard, ais_vessel->callsign, ais_vessel->name, ais_vessel->tslc, ais_vessel->flags); +} + +/** + * @brief Encode a ais_vessel struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ais_vessel C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ais_vessel_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ais_vessel_t* ais_vessel) +{ + return mavlink_msg_ais_vessel_pack_chan(system_id, component_id, chan, msg, ais_vessel->MMSI, ais_vessel->lat, ais_vessel->lon, ais_vessel->COG, ais_vessel->heading, ais_vessel->velocity, ais_vessel->turn_rate, ais_vessel->navigational_status, ais_vessel->type, ais_vessel->dimension_bow, ais_vessel->dimension_stern, ais_vessel->dimension_port, ais_vessel->dimension_starboard, ais_vessel->callsign, ais_vessel->name, ais_vessel->tslc, ais_vessel->flags); +} + +/** + * @brief Send a ais_vessel message + * @param chan MAVLink channel to send the message + * + * @param MMSI Mobile Marine Service Identifier, 9 decimal digits + * @param lat [degE7] Latitude + * @param lon [degE7] Longitude + * @param COG [cdeg] Course over ground + * @param heading [cdeg] True heading + * @param velocity [cm/s] Speed over ground + * @param turn_rate [cdeg/s] Turn rate + * @param navigational_status Navigational status + * @param type Type of vessels + * @param dimension_bow [m] Distance from lat/lon location to bow + * @param dimension_stern [m] Distance from lat/lon location to stern + * @param dimension_port [m] Distance from lat/lon location to port side + * @param dimension_starboard [m] Distance from lat/lon location to starboard side + * @param callsign The vessel callsign + * @param name The vessel name + * @param tslc [s] Time since last communication in seconds + * @param flags Bitmask to indicate various statuses including valid data fields + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ais_vessel_send(mavlink_channel_t chan, uint32_t MMSI, int32_t lat, int32_t lon, uint16_t COG, uint16_t heading, uint16_t velocity, int8_t turn_rate, uint8_t navigational_status, uint8_t type, uint16_t dimension_bow, uint16_t dimension_stern, uint8_t dimension_port, uint8_t dimension_starboard, const char *callsign, const char *name, uint16_t tslc, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIS_VESSEL_LEN]; + _mav_put_uint32_t(buf, 0, MMSI); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_uint16_t(buf, 12, COG); + _mav_put_uint16_t(buf, 14, heading); + _mav_put_uint16_t(buf, 16, velocity); + _mav_put_uint16_t(buf, 18, dimension_bow); + _mav_put_uint16_t(buf, 20, dimension_stern); + _mav_put_uint16_t(buf, 22, tslc); + _mav_put_uint16_t(buf, 24, flags); + _mav_put_int8_t(buf, 26, turn_rate); + _mav_put_uint8_t(buf, 27, navigational_status); + _mav_put_uint8_t(buf, 28, type); + _mav_put_uint8_t(buf, 29, dimension_port); + _mav_put_uint8_t(buf, 30, dimension_starboard); + _mav_put_char_array(buf, 31, callsign, 7); + _mav_put_char_array(buf, 38, name, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIS_VESSEL, buf, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); +#else + mavlink_ais_vessel_t packet; + packet.MMSI = MMSI; + packet.lat = lat; + packet.lon = lon; + packet.COG = COG; + packet.heading = heading; + packet.velocity = velocity; + packet.dimension_bow = dimension_bow; + packet.dimension_stern = dimension_stern; + packet.tslc = tslc; + packet.flags = flags; + packet.turn_rate = turn_rate; + packet.navigational_status = navigational_status; + packet.type = type; + packet.dimension_port = dimension_port; + packet.dimension_starboard = dimension_starboard; + mav_array_memcpy(packet.callsign, callsign, sizeof(char)*7); + mav_array_memcpy(packet.name, name, sizeof(char)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIS_VESSEL, (const char *)&packet, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); +#endif +} + +/** + * @brief Send a ais_vessel message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_ais_vessel_send_struct(mavlink_channel_t chan, const mavlink_ais_vessel_t* ais_vessel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_ais_vessel_send(chan, ais_vessel->MMSI, ais_vessel->lat, ais_vessel->lon, ais_vessel->COG, ais_vessel->heading, ais_vessel->velocity, ais_vessel->turn_rate, ais_vessel->navigational_status, ais_vessel->type, ais_vessel->dimension_bow, ais_vessel->dimension_stern, ais_vessel->dimension_port, ais_vessel->dimension_starboard, ais_vessel->callsign, ais_vessel->name, ais_vessel->tslc, ais_vessel->flags); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIS_VESSEL, (const char *)ais_vessel, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); +#endif +} + +#if MAVLINK_MSG_ID_AIS_VESSEL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_ais_vessel_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t MMSI, int32_t lat, int32_t lon, uint16_t COG, uint16_t heading, uint16_t velocity, int8_t turn_rate, uint8_t navigational_status, uint8_t type, uint16_t dimension_bow, uint16_t dimension_stern, uint8_t dimension_port, uint8_t dimension_starboard, const char *callsign, const char *name, uint16_t tslc, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, MMSI); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_uint16_t(buf, 12, COG); + _mav_put_uint16_t(buf, 14, heading); + _mav_put_uint16_t(buf, 16, velocity); + _mav_put_uint16_t(buf, 18, dimension_bow); + _mav_put_uint16_t(buf, 20, dimension_stern); + _mav_put_uint16_t(buf, 22, tslc); + _mav_put_uint16_t(buf, 24, flags); + _mav_put_int8_t(buf, 26, turn_rate); + _mav_put_uint8_t(buf, 27, navigational_status); + _mav_put_uint8_t(buf, 28, type); + _mav_put_uint8_t(buf, 29, dimension_port); + _mav_put_uint8_t(buf, 30, dimension_starboard); + _mav_put_char_array(buf, 31, callsign, 7); + _mav_put_char_array(buf, 38, name, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIS_VESSEL, buf, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); +#else + mavlink_ais_vessel_t *packet = (mavlink_ais_vessel_t *)msgbuf; + packet->MMSI = MMSI; + packet->lat = lat; + packet->lon = lon; + packet->COG = COG; + packet->heading = heading; + packet->velocity = velocity; + packet->dimension_bow = dimension_bow; + packet->dimension_stern = dimension_stern; + packet->tslc = tslc; + packet->flags = flags; + packet->turn_rate = turn_rate; + packet->navigational_status = navigational_status; + packet->type = type; + packet->dimension_port = dimension_port; + packet->dimension_starboard = dimension_starboard; + mav_array_memcpy(packet->callsign, callsign, sizeof(char)*7); + mav_array_memcpy(packet->name, name, sizeof(char)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIS_VESSEL, (const char *)packet, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); +#endif +} +#endif + +#endif + +// MESSAGE AIS_VESSEL UNPACKING + + +/** + * @brief Get field MMSI from ais_vessel message + * + * @return Mobile Marine Service Identifier, 9 decimal digits + */ +static inline uint32_t mavlink_msg_ais_vessel_get_MMSI(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field lat from ais_vessel message + * + * @return [degE7] Latitude + */ +static inline int32_t mavlink_msg_ais_vessel_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field lon from ais_vessel message + * + * @return [degE7] Longitude + */ +static inline int32_t mavlink_msg_ais_vessel_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field COG from ais_vessel message + * + * @return [cdeg] Course over ground + */ +static inline uint16_t mavlink_msg_ais_vessel_get_COG(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field heading from ais_vessel message + * + * @return [cdeg] True heading + */ +static inline uint16_t mavlink_msg_ais_vessel_get_heading(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Get field velocity from ais_vessel message + * + * @return [cm/s] Speed over ground + */ +static inline uint16_t mavlink_msg_ais_vessel_get_velocity(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field turn_rate from ais_vessel message + * + * @return [cdeg/s] Turn rate + */ +static inline int8_t mavlink_msg_ais_vessel_get_turn_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int8_t(msg, 26); +} + +/** + * @brief Get field navigational_status from ais_vessel message + * + * @return Navigational status + */ +static inline uint8_t mavlink_msg_ais_vessel_get_navigational_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 27); +} + +/** + * @brief Get field type from ais_vessel message + * + * @return Type of vessels + */ +static inline uint8_t mavlink_msg_ais_vessel_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 28); +} + +/** + * @brief Get field dimension_bow from ais_vessel message + * + * @return [m] Distance from lat/lon location to bow + */ +static inline uint16_t mavlink_msg_ais_vessel_get_dimension_bow(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field dimension_stern from ais_vessel message + * + * @return [m] Distance from lat/lon location to stern + */ +static inline uint16_t mavlink_msg_ais_vessel_get_dimension_stern(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 20); +} + +/** + * @brief Get field dimension_port from ais_vessel message + * + * @return [m] Distance from lat/lon location to port side + */ +static inline uint8_t mavlink_msg_ais_vessel_get_dimension_port(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 29); +} + +/** + * @brief Get field dimension_starboard from ais_vessel message + * + * @return [m] Distance from lat/lon location to starboard side + */ +static inline uint8_t mavlink_msg_ais_vessel_get_dimension_starboard(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 30); +} + +/** + * @brief Get field callsign from ais_vessel message + * + * @return The vessel callsign + */ +static inline uint16_t mavlink_msg_ais_vessel_get_callsign(const mavlink_message_t* msg, char *callsign) +{ + return _MAV_RETURN_char_array(msg, callsign, 7, 31); +} + +/** + * @brief Get field name from ais_vessel message + * + * @return The vessel name + */ +static inline uint16_t mavlink_msg_ais_vessel_get_name(const mavlink_message_t* msg, char *name) +{ + return _MAV_RETURN_char_array(msg, name, 20, 38); +} + +/** + * @brief Get field tslc from ais_vessel message + * + * @return [s] Time since last communication in seconds + */ +static inline uint16_t mavlink_msg_ais_vessel_get_tslc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 22); +} + +/** + * @brief Get field flags from ais_vessel message + * + * @return Bitmask to indicate various statuses including valid data fields + */ +static inline uint16_t mavlink_msg_ais_vessel_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Decode a ais_vessel message into a struct + * + * @param msg The message to decode + * @param ais_vessel C-struct to decode the message contents into + */ +static inline void mavlink_msg_ais_vessel_decode(const mavlink_message_t* msg, mavlink_ais_vessel_t* ais_vessel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + ais_vessel->MMSI = mavlink_msg_ais_vessel_get_MMSI(msg); + ais_vessel->lat = mavlink_msg_ais_vessel_get_lat(msg); + ais_vessel->lon = mavlink_msg_ais_vessel_get_lon(msg); + ais_vessel->COG = mavlink_msg_ais_vessel_get_COG(msg); + ais_vessel->heading = mavlink_msg_ais_vessel_get_heading(msg); + ais_vessel->velocity = mavlink_msg_ais_vessel_get_velocity(msg); + ais_vessel->dimension_bow = mavlink_msg_ais_vessel_get_dimension_bow(msg); + ais_vessel->dimension_stern = mavlink_msg_ais_vessel_get_dimension_stern(msg); + ais_vessel->tslc = mavlink_msg_ais_vessel_get_tslc(msg); + ais_vessel->flags = mavlink_msg_ais_vessel_get_flags(msg); + ais_vessel->turn_rate = mavlink_msg_ais_vessel_get_turn_rate(msg); + ais_vessel->navigational_status = mavlink_msg_ais_vessel_get_navigational_status(msg); + ais_vessel->type = mavlink_msg_ais_vessel_get_type(msg); + ais_vessel->dimension_port = mavlink_msg_ais_vessel_get_dimension_port(msg); + ais_vessel->dimension_starboard = mavlink_msg_ais_vessel_get_dimension_starboard(msg); + mavlink_msg_ais_vessel_get_callsign(msg, ais_vessel->callsign); + mavlink_msg_ais_vessel_get_name(msg, ais_vessel->name); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_AIS_VESSEL_LEN? msg->len : MAVLINK_MSG_ID_AIS_VESSEL_LEN; + memset(ais_vessel, 0, MAVLINK_MSG_ID_AIS_VESSEL_LEN); + memcpy(ais_vessel, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_att_pos_mocap.h b/lib/main/MAVLink/common/mavlink_msg_att_pos_mocap.h index dab6f557b6..2bf0bfd117 100755 --- a/lib/main/MAVLink/common/mavlink_msg_att_pos_mocap.h +++ b/lib/main/MAVLink/common/mavlink_msg_att_pos_mocap.h @@ -10,39 +10,43 @@ typedef struct __mavlink_att_pos_mocap_t { float x; /*< [m] X position (NED)*/ float y; /*< [m] Y position (NED)*/ float z; /*< [m] Z position (NED)*/ + float covariance[21]; /*< Row-major representation of a pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.*/ } mavlink_att_pos_mocap_t; -#define MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN 36 +#define MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN 120 #define MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN 36 -#define MAVLINK_MSG_ID_138_LEN 36 +#define MAVLINK_MSG_ID_138_LEN 120 #define MAVLINK_MSG_ID_138_MIN_LEN 36 #define MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC 109 #define MAVLINK_MSG_ID_138_CRC 109 #define MAVLINK_MSG_ATT_POS_MOCAP_FIELD_Q_LEN 4 +#define MAVLINK_MSG_ATT_POS_MOCAP_FIELD_COVARIANCE_LEN 21 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_ATT_POS_MOCAP { \ 138, \ "ATT_POS_MOCAP", \ - 5, \ + 6, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_att_pos_mocap_t, time_usec) }, \ { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 8, offsetof(mavlink_att_pos_mocap_t, q) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_att_pos_mocap_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_att_pos_mocap_t, y) }, \ { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_att_pos_mocap_t, z) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 36, offsetof(mavlink_att_pos_mocap_t, covariance) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_ATT_POS_MOCAP { \ "ATT_POS_MOCAP", \ - 5, \ + 6, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_att_pos_mocap_t, time_usec) }, \ { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 8, offsetof(mavlink_att_pos_mocap_t, q) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_att_pos_mocap_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_att_pos_mocap_t, y) }, \ { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_att_pos_mocap_t, z) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 36, offsetof(mavlink_att_pos_mocap_t, covariance) }, \ } \ } #endif @@ -58,10 +62,11 @@ typedef struct __mavlink_att_pos_mocap_t { * @param x [m] X position (NED) * @param y [m] Y position (NED) * @param z [m] Z position (NED) + * @param covariance Row-major representation of a pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_att_pos_mocap_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, const float *q, float x, float y, float z) + uint64_t time_usec, const float *q, float x, float y, float z, const float *covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN]; @@ -70,6 +75,7 @@ static inline uint16_t mavlink_msg_att_pos_mocap_pack(uint8_t system_id, uint8_t _mav_put_float(buf, 28, y); _mav_put_float(buf, 32, z); _mav_put_float_array(buf, 8, q, 4); + _mav_put_float_array(buf, 36, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); #else mavlink_att_pos_mocap_t packet; @@ -78,6 +84,7 @@ static inline uint16_t mavlink_msg_att_pos_mocap_pack(uint8_t system_id, uint8_t packet.y = y; packet.z = z; mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); #endif @@ -96,11 +103,12 @@ static inline uint16_t mavlink_msg_att_pos_mocap_pack(uint8_t system_id, uint8_t * @param x [m] X position (NED) * @param y [m] Y position (NED) * @param z [m] Z position (NED) + * @param covariance Row-major representation of a pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_att_pos_mocap_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,const float *q,float x,float y,float z) + uint64_t time_usec,const float *q,float x,float y,float z,const float *covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN]; @@ -109,6 +117,7 @@ static inline uint16_t mavlink_msg_att_pos_mocap_pack_chan(uint8_t system_id, ui _mav_put_float(buf, 28, y); _mav_put_float(buf, 32, z); _mav_put_float_array(buf, 8, q, 4); + _mav_put_float_array(buf, 36, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); #else mavlink_att_pos_mocap_t packet; @@ -117,6 +126,7 @@ static inline uint16_t mavlink_msg_att_pos_mocap_pack_chan(uint8_t system_id, ui packet.y = y; packet.z = z; mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); #endif @@ -134,7 +144,7 @@ static inline uint16_t mavlink_msg_att_pos_mocap_pack_chan(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_att_pos_mocap_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_att_pos_mocap_t* att_pos_mocap) { - return mavlink_msg_att_pos_mocap_pack(system_id, component_id, msg, att_pos_mocap->time_usec, att_pos_mocap->q, att_pos_mocap->x, att_pos_mocap->y, att_pos_mocap->z); + return mavlink_msg_att_pos_mocap_pack(system_id, component_id, msg, att_pos_mocap->time_usec, att_pos_mocap->q, att_pos_mocap->x, att_pos_mocap->y, att_pos_mocap->z, att_pos_mocap->covariance); } /** @@ -148,7 +158,7 @@ static inline uint16_t mavlink_msg_att_pos_mocap_encode(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_att_pos_mocap_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_att_pos_mocap_t* att_pos_mocap) { - return mavlink_msg_att_pos_mocap_pack_chan(system_id, component_id, chan, msg, att_pos_mocap->time_usec, att_pos_mocap->q, att_pos_mocap->x, att_pos_mocap->y, att_pos_mocap->z); + return mavlink_msg_att_pos_mocap_pack_chan(system_id, component_id, chan, msg, att_pos_mocap->time_usec, att_pos_mocap->q, att_pos_mocap->x, att_pos_mocap->y, att_pos_mocap->z, att_pos_mocap->covariance); } /** @@ -160,10 +170,11 @@ static inline uint16_t mavlink_msg_att_pos_mocap_encode_chan(uint8_t system_id, * @param x [m] X position (NED) * @param y [m] Y position (NED) * @param z [m] Z position (NED) + * @param covariance Row-major representation of a pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_att_pos_mocap_send(mavlink_channel_t chan, uint64_t time_usec, const float *q, float x, float y, float z) +static inline void mavlink_msg_att_pos_mocap_send(mavlink_channel_t chan, uint64_t time_usec, const float *q, float x, float y, float z, const float *covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN]; @@ -172,6 +183,7 @@ static inline void mavlink_msg_att_pos_mocap_send(mavlink_channel_t chan, uint64 _mav_put_float(buf, 28, y); _mav_put_float(buf, 32, z); _mav_put_float_array(buf, 8, q, 4); + _mav_put_float_array(buf, 36, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, buf, MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); #else mavlink_att_pos_mocap_t packet; @@ -180,6 +192,7 @@ static inline void mavlink_msg_att_pos_mocap_send(mavlink_channel_t chan, uint64 packet.y = y; packet.z = z; mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, (const char *)&packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); #endif } @@ -192,7 +205,7 @@ static inline void mavlink_msg_att_pos_mocap_send(mavlink_channel_t chan, uint64 static inline void mavlink_msg_att_pos_mocap_send_struct(mavlink_channel_t chan, const mavlink_att_pos_mocap_t* att_pos_mocap) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_att_pos_mocap_send(chan, att_pos_mocap->time_usec, att_pos_mocap->q, att_pos_mocap->x, att_pos_mocap->y, att_pos_mocap->z); + mavlink_msg_att_pos_mocap_send(chan, att_pos_mocap->time_usec, att_pos_mocap->q, att_pos_mocap->x, att_pos_mocap->y, att_pos_mocap->z, att_pos_mocap->covariance); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, (const char *)att_pos_mocap, MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); #endif @@ -206,7 +219,7 @@ static inline void mavlink_msg_att_pos_mocap_send_struct(mavlink_channel_t chan, is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_att_pos_mocap_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, const float *q, float x, float y, float z) +static inline void mavlink_msg_att_pos_mocap_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, const float *q, float x, float y, float z, const float *covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -215,6 +228,7 @@ static inline void mavlink_msg_att_pos_mocap_send_buf(mavlink_message_t *msgbuf, _mav_put_float(buf, 28, y); _mav_put_float(buf, 32, z); _mav_put_float_array(buf, 8, q, 4); + _mav_put_float_array(buf, 36, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, buf, MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); #else mavlink_att_pos_mocap_t *packet = (mavlink_att_pos_mocap_t *)msgbuf; @@ -223,6 +237,7 @@ static inline void mavlink_msg_att_pos_mocap_send_buf(mavlink_message_t *msgbuf, packet->y = y; packet->z = z; mav_array_memcpy(packet->q, q, sizeof(float)*4); + mav_array_memcpy(packet->covariance, covariance, sizeof(float)*21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, (const char *)packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); #endif } @@ -283,6 +298,16 @@ static inline float mavlink_msg_att_pos_mocap_get_z(const mavlink_message_t* msg return _MAV_RETURN_float(msg, 32); } +/** + * @brief Get field covariance from att_pos_mocap message + * + * @return Row-major representation of a pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + */ +static inline uint16_t mavlink_msg_att_pos_mocap_get_covariance(const mavlink_message_t* msg, float *covariance) +{ + return _MAV_RETURN_float_array(msg, covariance, 21, 36); +} + /** * @brief Decode a att_pos_mocap message into a struct * @@ -297,6 +322,7 @@ static inline void mavlink_msg_att_pos_mocap_decode(const mavlink_message_t* msg att_pos_mocap->x = mavlink_msg_att_pos_mocap_get_x(msg); att_pos_mocap->y = mavlink_msg_att_pos_mocap_get_y(msg); att_pos_mocap->z = mavlink_msg_att_pos_mocap_get_z(msg); + mavlink_msg_att_pos_mocap_get_covariance(msg, att_pos_mocap->covariance); #else uint8_t len = msg->len < MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN? msg->len : MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN; memset(att_pos_mocap, 0, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_attitude_quaternion.h b/lib/main/MAVLink/common/mavlink_msg_attitude_quaternion.h index 3ef53c4413..a613816221 100755 --- a/lib/main/MAVLink/common/mavlink_msg_attitude_quaternion.h +++ b/lib/main/MAVLink/common/mavlink_msg_attitude_quaternion.h @@ -13,23 +13,24 @@ typedef struct __mavlink_attitude_quaternion_t { float rollspeed; /*< [rad/s] Roll angular speed*/ float pitchspeed; /*< [rad/s] Pitch angular speed*/ float yawspeed; /*< [rad/s] Yaw angular speed*/ + float repr_offset_q[4]; /*< Rotation offset by which the attitude quaternion and angular speed vector should be rotated for user display (quaternion with [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], send [0, 0, 0, 0] if field not supported). This field is intended for systems in which the reference attitude may change during flight. For example, tailsitters VTOLs rotate their reference attitude by 90 degrees between hover mode and fixed wing mode, thus repr_offset_q is equal to [1, 0, 0, 0] in hover mode and equal to [0.7071, 0, 0.7071, 0] in fixed wing mode.*/ } mavlink_attitude_quaternion_t; -#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN 32 +#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN 48 #define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN 32 -#define MAVLINK_MSG_ID_31_LEN 32 +#define MAVLINK_MSG_ID_31_LEN 48 #define MAVLINK_MSG_ID_31_MIN_LEN 32 #define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC 246 #define MAVLINK_MSG_ID_31_CRC 246 - +#define MAVLINK_MSG_ATTITUDE_QUATERNION_FIELD_REPR_OFFSET_Q_LEN 4 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION { \ 31, \ "ATTITUDE_QUATERNION", \ - 8, \ + 9, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_quaternion_t, time_boot_ms) }, \ { "q1", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_attitude_quaternion_t, q1) }, \ { "q2", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_attitude_quaternion_t, q2) }, \ @@ -38,12 +39,13 @@ typedef struct __mavlink_attitude_quaternion_t { { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_quaternion_t, rollspeed) }, \ { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_quaternion_t, pitchspeed) }, \ { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_attitude_quaternion_t, yawspeed) }, \ + { "repr_offset_q", NULL, MAVLINK_TYPE_FLOAT, 4, 32, offsetof(mavlink_attitude_quaternion_t, repr_offset_q) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION { \ "ATTITUDE_QUATERNION", \ - 8, \ + 9, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_quaternion_t, time_boot_ms) }, \ { "q1", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_attitude_quaternion_t, q1) }, \ { "q2", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_attitude_quaternion_t, q2) }, \ @@ -52,6 +54,7 @@ typedef struct __mavlink_attitude_quaternion_t { { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_quaternion_t, rollspeed) }, \ { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_quaternion_t, pitchspeed) }, \ { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_attitude_quaternion_t, yawspeed) }, \ + { "repr_offset_q", NULL, MAVLINK_TYPE_FLOAT, 4, 32, offsetof(mavlink_attitude_quaternion_t, repr_offset_q) }, \ } \ } #endif @@ -70,10 +73,11 @@ typedef struct __mavlink_attitude_quaternion_t { * @param rollspeed [rad/s] Roll angular speed * @param pitchspeed [rad/s] Pitch angular speed * @param yawspeed [rad/s] Yaw angular speed + * @param repr_offset_q Rotation offset by which the attitude quaternion and angular speed vector should be rotated for user display (quaternion with [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], send [0, 0, 0, 0] if field not supported). This field is intended for systems in which the reference attitude may change during flight. For example, tailsitters VTOLs rotate their reference attitude by 90 degrees between hover mode and fixed wing mode, thus repr_offset_q is equal to [1, 0, 0, 0] in hover mode and equal to [0.7071, 0, 0.7071, 0] in fixed wing mode. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed) + uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed, const float *repr_offset_q) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; @@ -85,7 +89,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, u _mav_put_float(buf, 20, rollspeed); _mav_put_float(buf, 24, pitchspeed); _mav_put_float(buf, 28, yawspeed); - + _mav_put_float_array(buf, 32, repr_offset_q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); #else mavlink_attitude_quaternion_t packet; @@ -97,7 +101,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, u packet.rollspeed = rollspeed; packet.pitchspeed = pitchspeed; packet.yawspeed = yawspeed; - + mav_array_memcpy(packet.repr_offset_q, repr_offset_q, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); #endif @@ -119,11 +123,12 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, u * @param rollspeed [rad/s] Roll angular speed * @param pitchspeed [rad/s] Pitch angular speed * @param yawspeed [rad/s] Yaw angular speed + * @param repr_offset_q Rotation offset by which the attitude quaternion and angular speed vector should be rotated for user display (quaternion with [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], send [0, 0, 0, 0] if field not supported). This field is intended for systems in which the reference attitude may change during flight. For example, tailsitters VTOLs rotate their reference attitude by 90 degrees between hover mode and fixed wing mode, thus repr_offset_q is equal to [1, 0, 0, 0] in hover mode and equal to [0.7071, 0, 0.7071, 0] in fixed wing mode. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,float q1,float q2,float q3,float q4,float rollspeed,float pitchspeed,float yawspeed) + uint32_t time_boot_ms,float q1,float q2,float q3,float q4,float rollspeed,float pitchspeed,float yawspeed,const float *repr_offset_q) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; @@ -135,7 +140,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_ _mav_put_float(buf, 20, rollspeed); _mav_put_float(buf, 24, pitchspeed); _mav_put_float(buf, 28, yawspeed); - + _mav_put_float_array(buf, 32, repr_offset_q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); #else mavlink_attitude_quaternion_t packet; @@ -147,7 +152,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_ packet.rollspeed = rollspeed; packet.pitchspeed = pitchspeed; packet.yawspeed = yawspeed; - + mav_array_memcpy(packet.repr_offset_q, repr_offset_q, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); #endif @@ -165,7 +170,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_ */ static inline uint16_t mavlink_msg_attitude_quaternion_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_quaternion_t* attitude_quaternion) { - return mavlink_msg_attitude_quaternion_pack(system_id, component_id, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed); + return mavlink_msg_attitude_quaternion_pack(system_id, component_id, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed, attitude_quaternion->repr_offset_q); } /** @@ -179,7 +184,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_encode(uint8_t system_id, */ static inline uint16_t mavlink_msg_attitude_quaternion_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_quaternion_t* attitude_quaternion) { - return mavlink_msg_attitude_quaternion_pack_chan(system_id, component_id, chan, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed); + return mavlink_msg_attitude_quaternion_pack_chan(system_id, component_id, chan, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed, attitude_quaternion->repr_offset_q); } /** @@ -194,10 +199,11 @@ static inline uint16_t mavlink_msg_attitude_quaternion_encode_chan(uint8_t syste * @param rollspeed [rad/s] Roll angular speed * @param pitchspeed [rad/s] Pitch angular speed * @param yawspeed [rad/s] Yaw angular speed + * @param repr_offset_q Rotation offset by which the attitude quaternion and angular speed vector should be rotated for user display (quaternion with [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], send [0, 0, 0, 0] if field not supported). This field is intended for systems in which the reference attitude may change during flight. For example, tailsitters VTOLs rotate their reference attitude by 90 degrees between hover mode and fixed wing mode, thus repr_offset_q is equal to [1, 0, 0, 0] in hover mode and equal to [0.7071, 0, 0.7071, 0] in fixed wing mode. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed) +static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed, const float *repr_offset_q) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; @@ -209,7 +215,7 @@ static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, _mav_put_float(buf, 20, rollspeed); _mav_put_float(buf, 24, pitchspeed); _mav_put_float(buf, 28, yawspeed); - + _mav_put_float_array(buf, 32, repr_offset_q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); #else mavlink_attitude_quaternion_t packet; @@ -221,7 +227,7 @@ static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, packet.rollspeed = rollspeed; packet.pitchspeed = pitchspeed; packet.yawspeed = yawspeed; - + mav_array_memcpy(packet.repr_offset_q, repr_offset_q, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); #endif } @@ -234,7 +240,7 @@ static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, static inline void mavlink_msg_attitude_quaternion_send_struct(mavlink_channel_t chan, const mavlink_attitude_quaternion_t* attitude_quaternion) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_attitude_quaternion_send(chan, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed); + mavlink_msg_attitude_quaternion_send(chan, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed, attitude_quaternion->repr_offset_q); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)attitude_quaternion, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); #endif @@ -248,7 +254,7 @@ static inline void mavlink_msg_attitude_quaternion_send_struct(mavlink_channel_t is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_attitude_quaternion_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed) +static inline void mavlink_msg_attitude_quaternion_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed, const float *repr_offset_q) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -260,7 +266,7 @@ static inline void mavlink_msg_attitude_quaternion_send_buf(mavlink_message_t *m _mav_put_float(buf, 20, rollspeed); _mav_put_float(buf, 24, pitchspeed); _mav_put_float(buf, 28, yawspeed); - + _mav_put_float_array(buf, 32, repr_offset_q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); #else mavlink_attitude_quaternion_t *packet = (mavlink_attitude_quaternion_t *)msgbuf; @@ -272,7 +278,7 @@ static inline void mavlink_msg_attitude_quaternion_send_buf(mavlink_message_t *m packet->rollspeed = rollspeed; packet->pitchspeed = pitchspeed; packet->yawspeed = yawspeed; - + mav_array_memcpy(packet->repr_offset_q, repr_offset_q, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); #endif } @@ -363,6 +369,16 @@ static inline float mavlink_msg_attitude_quaternion_get_yawspeed(const mavlink_m return _MAV_RETURN_float(msg, 28); } +/** + * @brief Get field repr_offset_q from attitude_quaternion message + * + * @return Rotation offset by which the attitude quaternion and angular speed vector should be rotated for user display (quaternion with [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], send [0, 0, 0, 0] if field not supported). This field is intended for systems in which the reference attitude may change during flight. For example, tailsitters VTOLs rotate their reference attitude by 90 degrees between hover mode and fixed wing mode, thus repr_offset_q is equal to [1, 0, 0, 0] in hover mode and equal to [0.7071, 0, 0.7071, 0] in fixed wing mode. + */ +static inline uint16_t mavlink_msg_attitude_quaternion_get_repr_offset_q(const mavlink_message_t* msg, float *repr_offset_q) +{ + return _MAV_RETURN_float_array(msg, repr_offset_q, 4, 32); +} + /** * @brief Decode a attitude_quaternion message into a struct * @@ -380,6 +396,7 @@ static inline void mavlink_msg_attitude_quaternion_decode(const mavlink_message_ attitude_quaternion->rollspeed = mavlink_msg_attitude_quaternion_get_rollspeed(msg); attitude_quaternion->pitchspeed = mavlink_msg_attitude_quaternion_get_pitchspeed(msg); attitude_quaternion->yawspeed = mavlink_msg_attitude_quaternion_get_yawspeed(msg); + mavlink_msg_attitude_quaternion_get_repr_offset_q(msg, attitude_quaternion->repr_offset_q); #else uint8_t len = msg->len < MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN? msg->len : MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN; memset(attitude_quaternion, 0, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_attitude_target.h b/lib/main/MAVLink/common/mavlink_msg_attitude_target.h index af578884f4..bedbb93e9f 100755 --- a/lib/main/MAVLink/common/mavlink_msg_attitude_target.h +++ b/lib/main/MAVLink/common/mavlink_msg_attitude_target.h @@ -11,7 +11,7 @@ typedef struct __mavlink_attitude_target_t { float body_pitch_rate; /*< [rad/s] Body pitch rate*/ float body_yaw_rate; /*< [rad/s] Body yaw rate*/ float thrust; /*< Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust)*/ - uint8_t type_mask; /*< Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude*/ + uint8_t type_mask; /*< Bitmap to indicate which dimensions should be ignored by the vehicle.*/ } mavlink_attitude_target_t; #define MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN 37 @@ -60,7 +60,7 @@ typedef struct __mavlink_attitude_target_t { * @param msg The MAVLink message to compress the data into * * @param time_boot_ms [ms] Timestamp (time since system boot). - * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param body_roll_rate [rad/s] Body roll rate * @param body_pitch_rate [rad/s] Body pitch rate @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_attitude_target_pack(uint8_t system_id, uint8 * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms [ms] Timestamp (time since system boot). - * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param body_roll_rate [rad/s] Body roll rate * @param body_pitch_rate [rad/s] Body pitch rate @@ -174,7 +174,7 @@ static inline uint16_t mavlink_msg_attitude_target_encode_chan(uint8_t system_id * @param chan MAVLink channel to send the message * * @param time_boot_ms [ms] Timestamp (time since system boot). - * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param body_roll_rate [rad/s] Body roll rate * @param body_pitch_rate [rad/s] Body pitch rate @@ -274,7 +274,7 @@ static inline uint32_t mavlink_msg_attitude_target_get_time_boot_ms(const mavlin /** * @brief Get field type_mask from attitude_target message * - * @return Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude + * @return Bitmap to indicate which dimensions should be ignored by the vehicle. */ static inline uint8_t mavlink_msg_attitude_target_get_type_mask(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_autopilot_state_for_gimbal_device.h b/lib/main/MAVLink/common/mavlink_msg_autopilot_state_for_gimbal_device.h new file mode 100644 index 0000000000..c9cdce8d54 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_autopilot_state_for_gimbal_device.h @@ -0,0 +1,480 @@ +#pragma once +// MESSAGE AUTOPILOT_STATE_FOR_GIMBAL_DEVICE PACKING + +#define MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE 286 + + +typedef struct __mavlink_autopilot_state_for_gimbal_device_t { + uint64_t time_boot_us; /*< [us] Timestamp (time since system boot).*/ + float q[4]; /*< Quaternion components of autopilot attitude: w, x, y, z (1 0 0 0 is the null-rotation, Hamilton convention).*/ + uint32_t q_estimated_delay_us; /*< [us] Estimated delay of the attitude data.*/ + float vx; /*< [m/s] X Speed in NED (North, East, Down).*/ + float vy; /*< [m/s] Y Speed in NED (North, East, Down).*/ + float vz; /*< [m/s] Z Speed in NED (North, East, Down).*/ + uint32_t v_estimated_delay_us; /*< [us] Estimated delay of the speed data.*/ + float feed_forward_angular_velocity_z; /*< [rad/s] Feed forward Z component of angular velocity, positive is yawing to the right, NaN to be ignored. This is to indicate if the autopilot is actively yawing.*/ + uint16_t estimator_status; /*< Bitmap indicating which estimator outputs are valid.*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + uint8_t landed_state; /*< The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown.*/ +} mavlink_autopilot_state_for_gimbal_device_t; + +#define MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN 53 +#define MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN 53 +#define MAVLINK_MSG_ID_286_LEN 53 +#define MAVLINK_MSG_ID_286_MIN_LEN 53 + +#define MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC 210 +#define MAVLINK_MSG_ID_286_CRC 210 + +#define MAVLINK_MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_FIELD_Q_LEN 4 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE { \ + 286, \ + "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE", \ + 12, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 50, offsetof(mavlink_autopilot_state_for_gimbal_device_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 51, offsetof(mavlink_autopilot_state_for_gimbal_device_t, target_component) }, \ + { "time_boot_us", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_autopilot_state_for_gimbal_device_t, time_boot_us) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 8, offsetof(mavlink_autopilot_state_for_gimbal_device_t, q) }, \ + { "q_estimated_delay_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 24, offsetof(mavlink_autopilot_state_for_gimbal_device_t, q_estimated_delay_us) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_autopilot_state_for_gimbal_device_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_autopilot_state_for_gimbal_device_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_autopilot_state_for_gimbal_device_t, vz) }, \ + { "v_estimated_delay_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 40, offsetof(mavlink_autopilot_state_for_gimbal_device_t, v_estimated_delay_us) }, \ + { "feed_forward_angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_autopilot_state_for_gimbal_device_t, feed_forward_angular_velocity_z) }, \ + { "estimator_status", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_autopilot_state_for_gimbal_device_t, estimator_status) }, \ + { "landed_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_autopilot_state_for_gimbal_device_t, landed_state) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE { \ + "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE", \ + 12, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 50, offsetof(mavlink_autopilot_state_for_gimbal_device_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 51, offsetof(mavlink_autopilot_state_for_gimbal_device_t, target_component) }, \ + { "time_boot_us", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_autopilot_state_for_gimbal_device_t, time_boot_us) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 8, offsetof(mavlink_autopilot_state_for_gimbal_device_t, q) }, \ + { "q_estimated_delay_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 24, offsetof(mavlink_autopilot_state_for_gimbal_device_t, q_estimated_delay_us) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_autopilot_state_for_gimbal_device_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_autopilot_state_for_gimbal_device_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_autopilot_state_for_gimbal_device_t, vz) }, \ + { "v_estimated_delay_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 40, offsetof(mavlink_autopilot_state_for_gimbal_device_t, v_estimated_delay_us) }, \ + { "feed_forward_angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_autopilot_state_for_gimbal_device_t, feed_forward_angular_velocity_z) }, \ + { "estimator_status", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_autopilot_state_for_gimbal_device_t, estimator_status) }, \ + { "landed_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_autopilot_state_for_gimbal_device_t, landed_state) }, \ + } \ +} +#endif + +/** + * @brief Pack a autopilot_state_for_gimbal_device message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param time_boot_us [us] Timestamp (time since system boot). + * @param q Quaternion components of autopilot attitude: w, x, y, z (1 0 0 0 is the null-rotation, Hamilton convention). + * @param q_estimated_delay_us [us] Estimated delay of the attitude data. + * @param vx [m/s] X Speed in NED (North, East, Down). + * @param vy [m/s] Y Speed in NED (North, East, Down). + * @param vz [m/s] Z Speed in NED (North, East, Down). + * @param v_estimated_delay_us [us] Estimated delay of the speed data. + * @param feed_forward_angular_velocity_z [rad/s] Feed forward Z component of angular velocity, positive is yawing to the right, NaN to be ignored. This is to indicate if the autopilot is actively yawing. + * @param estimator_status Bitmap indicating which estimator outputs are valid. + * @param landed_state The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint64_t time_boot_us, const float *q, uint32_t q_estimated_delay_us, float vx, float vy, float vz, uint32_t v_estimated_delay_us, float feed_forward_angular_velocity_z, uint16_t estimator_status, uint8_t landed_state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN]; + _mav_put_uint64_t(buf, 0, time_boot_us); + _mav_put_uint32_t(buf, 24, q_estimated_delay_us); + _mav_put_float(buf, 28, vx); + _mav_put_float(buf, 32, vy); + _mav_put_float(buf, 36, vz); + _mav_put_uint32_t(buf, 40, v_estimated_delay_us); + _mav_put_float(buf, 44, feed_forward_angular_velocity_z); + _mav_put_uint16_t(buf, 48, estimator_status); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, landed_state); + _mav_put_float_array(buf, 8, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); +#else + mavlink_autopilot_state_for_gimbal_device_t packet; + packet.time_boot_us = time_boot_us; + packet.q_estimated_delay_us = q_estimated_delay_us; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.v_estimated_delay_us = v_estimated_delay_us; + packet.feed_forward_angular_velocity_z = feed_forward_angular_velocity_z; + packet.estimator_status = estimator_status; + packet.target_system = target_system; + packet.target_component = target_component; + packet.landed_state = landed_state; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); +} + +/** + * @brief Pack a autopilot_state_for_gimbal_device message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param time_boot_us [us] Timestamp (time since system boot). + * @param q Quaternion components of autopilot attitude: w, x, y, z (1 0 0 0 is the null-rotation, Hamilton convention). + * @param q_estimated_delay_us [us] Estimated delay of the attitude data. + * @param vx [m/s] X Speed in NED (North, East, Down). + * @param vy [m/s] Y Speed in NED (North, East, Down). + * @param vz [m/s] Z Speed in NED (North, East, Down). + * @param v_estimated_delay_us [us] Estimated delay of the speed data. + * @param feed_forward_angular_velocity_z [rad/s] Feed forward Z component of angular velocity, positive is yawing to the right, NaN to be ignored. This is to indicate if the autopilot is actively yawing. + * @param estimator_status Bitmap indicating which estimator outputs are valid. + * @param landed_state The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint64_t time_boot_us,const float *q,uint32_t q_estimated_delay_us,float vx,float vy,float vz,uint32_t v_estimated_delay_us,float feed_forward_angular_velocity_z,uint16_t estimator_status,uint8_t landed_state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN]; + _mav_put_uint64_t(buf, 0, time_boot_us); + _mav_put_uint32_t(buf, 24, q_estimated_delay_us); + _mav_put_float(buf, 28, vx); + _mav_put_float(buf, 32, vy); + _mav_put_float(buf, 36, vz); + _mav_put_uint32_t(buf, 40, v_estimated_delay_us); + _mav_put_float(buf, 44, feed_forward_angular_velocity_z); + _mav_put_uint16_t(buf, 48, estimator_status); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, landed_state); + _mav_put_float_array(buf, 8, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); +#else + mavlink_autopilot_state_for_gimbal_device_t packet; + packet.time_boot_us = time_boot_us; + packet.q_estimated_delay_us = q_estimated_delay_us; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.v_estimated_delay_us = v_estimated_delay_us; + packet.feed_forward_angular_velocity_z = feed_forward_angular_velocity_z; + packet.estimator_status = estimator_status; + packet.target_system = target_system; + packet.target_component = target_component; + packet.landed_state = landed_state; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); +} + +/** + * @brief Encode a autopilot_state_for_gimbal_device struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param autopilot_state_for_gimbal_device C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_autopilot_state_for_gimbal_device_t* autopilot_state_for_gimbal_device) +{ + return mavlink_msg_autopilot_state_for_gimbal_device_pack(system_id, component_id, msg, autopilot_state_for_gimbal_device->target_system, autopilot_state_for_gimbal_device->target_component, autopilot_state_for_gimbal_device->time_boot_us, autopilot_state_for_gimbal_device->q, autopilot_state_for_gimbal_device->q_estimated_delay_us, autopilot_state_for_gimbal_device->vx, autopilot_state_for_gimbal_device->vy, autopilot_state_for_gimbal_device->vz, autopilot_state_for_gimbal_device->v_estimated_delay_us, autopilot_state_for_gimbal_device->feed_forward_angular_velocity_z, autopilot_state_for_gimbal_device->estimator_status, autopilot_state_for_gimbal_device->landed_state); +} + +/** + * @brief Encode a autopilot_state_for_gimbal_device struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param autopilot_state_for_gimbal_device C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_autopilot_state_for_gimbal_device_t* autopilot_state_for_gimbal_device) +{ + return mavlink_msg_autopilot_state_for_gimbal_device_pack_chan(system_id, component_id, chan, msg, autopilot_state_for_gimbal_device->target_system, autopilot_state_for_gimbal_device->target_component, autopilot_state_for_gimbal_device->time_boot_us, autopilot_state_for_gimbal_device->q, autopilot_state_for_gimbal_device->q_estimated_delay_us, autopilot_state_for_gimbal_device->vx, autopilot_state_for_gimbal_device->vy, autopilot_state_for_gimbal_device->vz, autopilot_state_for_gimbal_device->v_estimated_delay_us, autopilot_state_for_gimbal_device->feed_forward_angular_velocity_z, autopilot_state_for_gimbal_device->estimator_status, autopilot_state_for_gimbal_device->landed_state); +} + +/** + * @brief Send a autopilot_state_for_gimbal_device message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param time_boot_us [us] Timestamp (time since system boot). + * @param q Quaternion components of autopilot attitude: w, x, y, z (1 0 0 0 is the null-rotation, Hamilton convention). + * @param q_estimated_delay_us [us] Estimated delay of the attitude data. + * @param vx [m/s] X Speed in NED (North, East, Down). + * @param vy [m/s] Y Speed in NED (North, East, Down). + * @param vz [m/s] Z Speed in NED (North, East, Down). + * @param v_estimated_delay_us [us] Estimated delay of the speed data. + * @param feed_forward_angular_velocity_z [rad/s] Feed forward Z component of angular velocity, positive is yawing to the right, NaN to be ignored. This is to indicate if the autopilot is actively yawing. + * @param estimator_status Bitmap indicating which estimator outputs are valid. + * @param landed_state The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_autopilot_state_for_gimbal_device_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint64_t time_boot_us, const float *q, uint32_t q_estimated_delay_us, float vx, float vy, float vz, uint32_t v_estimated_delay_us, float feed_forward_angular_velocity_z, uint16_t estimator_status, uint8_t landed_state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN]; + _mav_put_uint64_t(buf, 0, time_boot_us); + _mav_put_uint32_t(buf, 24, q_estimated_delay_us); + _mav_put_float(buf, 28, vx); + _mav_put_float(buf, 32, vy); + _mav_put_float(buf, 36, vz); + _mav_put_uint32_t(buf, 40, v_estimated_delay_us); + _mav_put_float(buf, 44, feed_forward_angular_velocity_z); + _mav_put_uint16_t(buf, 48, estimator_status); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, landed_state); + _mav_put_float_array(buf, 8, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, buf, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); +#else + mavlink_autopilot_state_for_gimbal_device_t packet; + packet.time_boot_us = time_boot_us; + packet.q_estimated_delay_us = q_estimated_delay_us; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.v_estimated_delay_us = v_estimated_delay_us; + packet.feed_forward_angular_velocity_z = feed_forward_angular_velocity_z; + packet.estimator_status = estimator_status; + packet.target_system = target_system; + packet.target_component = target_component; + packet.landed_state = landed_state; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, (const char *)&packet, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); +#endif +} + +/** + * @brief Send a autopilot_state_for_gimbal_device message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_autopilot_state_for_gimbal_device_send_struct(mavlink_channel_t chan, const mavlink_autopilot_state_for_gimbal_device_t* autopilot_state_for_gimbal_device) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_autopilot_state_for_gimbal_device_send(chan, autopilot_state_for_gimbal_device->target_system, autopilot_state_for_gimbal_device->target_component, autopilot_state_for_gimbal_device->time_boot_us, autopilot_state_for_gimbal_device->q, autopilot_state_for_gimbal_device->q_estimated_delay_us, autopilot_state_for_gimbal_device->vx, autopilot_state_for_gimbal_device->vy, autopilot_state_for_gimbal_device->vz, autopilot_state_for_gimbal_device->v_estimated_delay_us, autopilot_state_for_gimbal_device->feed_forward_angular_velocity_z, autopilot_state_for_gimbal_device->estimator_status, autopilot_state_for_gimbal_device->landed_state); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, (const char *)autopilot_state_for_gimbal_device, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_autopilot_state_for_gimbal_device_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint64_t time_boot_us, const float *q, uint32_t q_estimated_delay_us, float vx, float vy, float vz, uint32_t v_estimated_delay_us, float feed_forward_angular_velocity_z, uint16_t estimator_status, uint8_t landed_state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_boot_us); + _mav_put_uint32_t(buf, 24, q_estimated_delay_us); + _mav_put_float(buf, 28, vx); + _mav_put_float(buf, 32, vy); + _mav_put_float(buf, 36, vz); + _mav_put_uint32_t(buf, 40, v_estimated_delay_us); + _mav_put_float(buf, 44, feed_forward_angular_velocity_z); + _mav_put_uint16_t(buf, 48, estimator_status); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, landed_state); + _mav_put_float_array(buf, 8, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, buf, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); +#else + mavlink_autopilot_state_for_gimbal_device_t *packet = (mavlink_autopilot_state_for_gimbal_device_t *)msgbuf; + packet->time_boot_us = time_boot_us; + packet->q_estimated_delay_us = q_estimated_delay_us; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->v_estimated_delay_us = v_estimated_delay_us; + packet->feed_forward_angular_velocity_z = feed_forward_angular_velocity_z; + packet->estimator_status = estimator_status; + packet->target_system = target_system; + packet->target_component = target_component; + packet->landed_state = landed_state; + mav_array_memcpy(packet->q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, (const char *)packet, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE AUTOPILOT_STATE_FOR_GIMBAL_DEVICE UNPACKING + + +/** + * @brief Get field target_system from autopilot_state_for_gimbal_device message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_autopilot_state_for_gimbal_device_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 50); +} + +/** + * @brief Get field target_component from autopilot_state_for_gimbal_device message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_autopilot_state_for_gimbal_device_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 51); +} + +/** + * @brief Get field time_boot_us from autopilot_state_for_gimbal_device message + * + * @return [us] Timestamp (time since system boot). + */ +static inline uint64_t mavlink_msg_autopilot_state_for_gimbal_device_get_time_boot_us(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field q from autopilot_state_for_gimbal_device message + * + * @return Quaternion components of autopilot attitude: w, x, y, z (1 0 0 0 is the null-rotation, Hamilton convention). + */ +static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 8); +} + +/** + * @brief Get field q_estimated_delay_us from autopilot_state_for_gimbal_device message + * + * @return [us] Estimated delay of the attitude data. + */ +static inline uint32_t mavlink_msg_autopilot_state_for_gimbal_device_get_q_estimated_delay_us(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 24); +} + +/** + * @brief Get field vx from autopilot_state_for_gimbal_device message + * + * @return [m/s] X Speed in NED (North, East, Down). + */ +static inline float mavlink_msg_autopilot_state_for_gimbal_device_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field vy from autopilot_state_for_gimbal_device message + * + * @return [m/s] Y Speed in NED (North, East, Down). + */ +static inline float mavlink_msg_autopilot_state_for_gimbal_device_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field vz from autopilot_state_for_gimbal_device message + * + * @return [m/s] Z Speed in NED (North, East, Down). + */ +static inline float mavlink_msg_autopilot_state_for_gimbal_device_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field v_estimated_delay_us from autopilot_state_for_gimbal_device message + * + * @return [us] Estimated delay of the speed data. + */ +static inline uint32_t mavlink_msg_autopilot_state_for_gimbal_device_get_v_estimated_delay_us(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 40); +} + +/** + * @brief Get field feed_forward_angular_velocity_z from autopilot_state_for_gimbal_device message + * + * @return [rad/s] Feed forward Z component of angular velocity, positive is yawing to the right, NaN to be ignored. This is to indicate if the autopilot is actively yawing. + */ +static inline float mavlink_msg_autopilot_state_for_gimbal_device_get_feed_forward_angular_velocity_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field estimator_status from autopilot_state_for_gimbal_device message + * + * @return Bitmap indicating which estimator outputs are valid. + */ +static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_get_estimator_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 48); +} + +/** + * @brief Get field landed_state from autopilot_state_for_gimbal_device message + * + * @return The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + */ +static inline uint8_t mavlink_msg_autopilot_state_for_gimbal_device_get_landed_state(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 52); +} + +/** + * @brief Decode a autopilot_state_for_gimbal_device message into a struct + * + * @param msg The message to decode + * @param autopilot_state_for_gimbal_device C-struct to decode the message contents into + */ +static inline void mavlink_msg_autopilot_state_for_gimbal_device_decode(const mavlink_message_t* msg, mavlink_autopilot_state_for_gimbal_device_t* autopilot_state_for_gimbal_device) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + autopilot_state_for_gimbal_device->time_boot_us = mavlink_msg_autopilot_state_for_gimbal_device_get_time_boot_us(msg); + mavlink_msg_autopilot_state_for_gimbal_device_get_q(msg, autopilot_state_for_gimbal_device->q); + autopilot_state_for_gimbal_device->q_estimated_delay_us = mavlink_msg_autopilot_state_for_gimbal_device_get_q_estimated_delay_us(msg); + autopilot_state_for_gimbal_device->vx = mavlink_msg_autopilot_state_for_gimbal_device_get_vx(msg); + autopilot_state_for_gimbal_device->vy = mavlink_msg_autopilot_state_for_gimbal_device_get_vy(msg); + autopilot_state_for_gimbal_device->vz = mavlink_msg_autopilot_state_for_gimbal_device_get_vz(msg); + autopilot_state_for_gimbal_device->v_estimated_delay_us = mavlink_msg_autopilot_state_for_gimbal_device_get_v_estimated_delay_us(msg); + autopilot_state_for_gimbal_device->feed_forward_angular_velocity_z = mavlink_msg_autopilot_state_for_gimbal_device_get_feed_forward_angular_velocity_z(msg); + autopilot_state_for_gimbal_device->estimator_status = mavlink_msg_autopilot_state_for_gimbal_device_get_estimator_status(msg); + autopilot_state_for_gimbal_device->target_system = mavlink_msg_autopilot_state_for_gimbal_device_get_target_system(msg); + autopilot_state_for_gimbal_device->target_component = mavlink_msg_autopilot_state_for_gimbal_device_get_target_component(msg); + autopilot_state_for_gimbal_device->landed_state = mavlink_msg_autopilot_state_for_gimbal_device_get_landed_state(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN? msg->len : MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN; + memset(autopilot_state_for_gimbal_device, 0, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); + memcpy(autopilot_state_for_gimbal_device, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_autopilot_version.h b/lib/main/MAVLink/common/mavlink_msg_autopilot_version.h index ae0324b75b..37b87be38f 100755 --- a/lib/main/MAVLink/common/mavlink_msg_autopilot_version.h +++ b/lib/main/MAVLink/common/mavlink_msg_autopilot_version.h @@ -16,11 +16,12 @@ typedef struct __mavlink_autopilot_version_t { uint8_t flight_custom_version[8]; /*< Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases.*/ uint8_t middleware_custom_version[8]; /*< Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases.*/ uint8_t os_custom_version[8]; /*< Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases.*/ + uint8_t uid2[18]; /*< UID if provided by hardware (supersedes the uid field. If this is non-zero, use this field, otherwise use uid)*/ } mavlink_autopilot_version_t; -#define MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN 60 +#define MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN 78 #define MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN 60 -#define MAVLINK_MSG_ID_148_LEN 60 +#define MAVLINK_MSG_ID_148_LEN 78 #define MAVLINK_MSG_ID_148_MIN_LEN 60 #define MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC 178 @@ -29,12 +30,13 @@ typedef struct __mavlink_autopilot_version_t { #define MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_FLIGHT_CUSTOM_VERSION_LEN 8 #define MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_MIDDLEWARE_CUSTOM_VERSION_LEN 8 #define MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_OS_CUSTOM_VERSION_LEN 8 +#define MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_UID2_LEN 18 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION { \ 148, \ "AUTOPILOT_VERSION", \ - 11, \ + 12, \ { { "capabilities", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_autopilot_version_t, capabilities) }, \ { "flight_sw_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 16, offsetof(mavlink_autopilot_version_t, flight_sw_version) }, \ { "middleware_sw_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_autopilot_version_t, middleware_sw_version) }, \ @@ -46,12 +48,13 @@ typedef struct __mavlink_autopilot_version_t { { "vendor_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_autopilot_version_t, vendor_id) }, \ { "product_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 34, offsetof(mavlink_autopilot_version_t, product_id) }, \ { "uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_autopilot_version_t, uid) }, \ + { "uid2", NULL, MAVLINK_TYPE_UINT8_T, 18, 60, offsetof(mavlink_autopilot_version_t, uid2) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION { \ "AUTOPILOT_VERSION", \ - 11, \ + 12, \ { { "capabilities", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_autopilot_version_t, capabilities) }, \ { "flight_sw_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 16, offsetof(mavlink_autopilot_version_t, flight_sw_version) }, \ { "middleware_sw_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_autopilot_version_t, middleware_sw_version) }, \ @@ -63,6 +66,7 @@ typedef struct __mavlink_autopilot_version_t { { "vendor_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_autopilot_version_t, vendor_id) }, \ { "product_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 34, offsetof(mavlink_autopilot_version_t, product_id) }, \ { "uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_autopilot_version_t, uid) }, \ + { "uid2", NULL, MAVLINK_TYPE_UINT8_T, 18, 60, offsetof(mavlink_autopilot_version_t, uid2) }, \ } \ } #endif @@ -84,10 +88,11 @@ typedef struct __mavlink_autopilot_version_t { * @param vendor_id ID of the board vendor * @param product_id ID of the product * @param uid UID if provided by hardware (see uid2) + * @param uid2 UID if provided by hardware (supersedes the uid field. If this is non-zero, use this field, otherwise use uid) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_autopilot_version_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, const uint8_t *flight_custom_version, const uint8_t *middleware_custom_version, const uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid) + uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, const uint8_t *flight_custom_version, const uint8_t *middleware_custom_version, const uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid, const uint8_t *uid2) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN]; @@ -102,6 +107,7 @@ static inline uint16_t mavlink_msg_autopilot_version_pack(uint8_t system_id, uin _mav_put_uint8_t_array(buf, 36, flight_custom_version, 8); _mav_put_uint8_t_array(buf, 44, middleware_custom_version, 8); _mav_put_uint8_t_array(buf, 52, os_custom_version, 8); + _mav_put_uint8_t_array(buf, 60, uid2, 18); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); #else mavlink_autopilot_version_t packet; @@ -116,6 +122,7 @@ static inline uint16_t mavlink_msg_autopilot_version_pack(uint8_t system_id, uin mav_array_memcpy(packet.flight_custom_version, flight_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet.middleware_custom_version, middleware_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet.os_custom_version, os_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet.uid2, uid2, sizeof(uint8_t)*18); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); #endif @@ -140,11 +147,12 @@ static inline uint16_t mavlink_msg_autopilot_version_pack(uint8_t system_id, uin * @param vendor_id ID of the board vendor * @param product_id ID of the product * @param uid UID if provided by hardware (see uid2) + * @param uid2 UID if provided by hardware (supersedes the uid field. If this is non-zero, use this field, otherwise use uid) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_autopilot_version_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t capabilities,uint32_t flight_sw_version,uint32_t middleware_sw_version,uint32_t os_sw_version,uint32_t board_version,const uint8_t *flight_custom_version,const uint8_t *middleware_custom_version,const uint8_t *os_custom_version,uint16_t vendor_id,uint16_t product_id,uint64_t uid) + uint64_t capabilities,uint32_t flight_sw_version,uint32_t middleware_sw_version,uint32_t os_sw_version,uint32_t board_version,const uint8_t *flight_custom_version,const uint8_t *middleware_custom_version,const uint8_t *os_custom_version,uint16_t vendor_id,uint16_t product_id,uint64_t uid,const uint8_t *uid2) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN]; @@ -159,6 +167,7 @@ static inline uint16_t mavlink_msg_autopilot_version_pack_chan(uint8_t system_id _mav_put_uint8_t_array(buf, 36, flight_custom_version, 8); _mav_put_uint8_t_array(buf, 44, middleware_custom_version, 8); _mav_put_uint8_t_array(buf, 52, os_custom_version, 8); + _mav_put_uint8_t_array(buf, 60, uid2, 18); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); #else mavlink_autopilot_version_t packet; @@ -173,6 +182,7 @@ static inline uint16_t mavlink_msg_autopilot_version_pack_chan(uint8_t system_id mav_array_memcpy(packet.flight_custom_version, flight_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet.middleware_custom_version, middleware_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet.os_custom_version, os_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet.uid2, uid2, sizeof(uint8_t)*18); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); #endif @@ -190,7 +200,7 @@ static inline uint16_t mavlink_msg_autopilot_version_pack_chan(uint8_t system_id */ static inline uint16_t mavlink_msg_autopilot_version_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_autopilot_version_t* autopilot_version) { - return mavlink_msg_autopilot_version_pack(system_id, component_id, msg, autopilot_version->capabilities, autopilot_version->flight_sw_version, autopilot_version->middleware_sw_version, autopilot_version->os_sw_version, autopilot_version->board_version, autopilot_version->flight_custom_version, autopilot_version->middleware_custom_version, autopilot_version->os_custom_version, autopilot_version->vendor_id, autopilot_version->product_id, autopilot_version->uid); + return mavlink_msg_autopilot_version_pack(system_id, component_id, msg, autopilot_version->capabilities, autopilot_version->flight_sw_version, autopilot_version->middleware_sw_version, autopilot_version->os_sw_version, autopilot_version->board_version, autopilot_version->flight_custom_version, autopilot_version->middleware_custom_version, autopilot_version->os_custom_version, autopilot_version->vendor_id, autopilot_version->product_id, autopilot_version->uid, autopilot_version->uid2); } /** @@ -204,7 +214,7 @@ static inline uint16_t mavlink_msg_autopilot_version_encode(uint8_t system_id, u */ static inline uint16_t mavlink_msg_autopilot_version_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_autopilot_version_t* autopilot_version) { - return mavlink_msg_autopilot_version_pack_chan(system_id, component_id, chan, msg, autopilot_version->capabilities, autopilot_version->flight_sw_version, autopilot_version->middleware_sw_version, autopilot_version->os_sw_version, autopilot_version->board_version, autopilot_version->flight_custom_version, autopilot_version->middleware_custom_version, autopilot_version->os_custom_version, autopilot_version->vendor_id, autopilot_version->product_id, autopilot_version->uid); + return mavlink_msg_autopilot_version_pack_chan(system_id, component_id, chan, msg, autopilot_version->capabilities, autopilot_version->flight_sw_version, autopilot_version->middleware_sw_version, autopilot_version->os_sw_version, autopilot_version->board_version, autopilot_version->flight_custom_version, autopilot_version->middleware_custom_version, autopilot_version->os_custom_version, autopilot_version->vendor_id, autopilot_version->product_id, autopilot_version->uid, autopilot_version->uid2); } /** @@ -222,10 +232,11 @@ static inline uint16_t mavlink_msg_autopilot_version_encode_chan(uint8_t system_ * @param vendor_id ID of the board vendor * @param product_id ID of the product * @param uid UID if provided by hardware (see uid2) + * @param uid2 UID if provided by hardware (supersedes the uid field. If this is non-zero, use this field, otherwise use uid) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_autopilot_version_send(mavlink_channel_t chan, uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, const uint8_t *flight_custom_version, const uint8_t *middleware_custom_version, const uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid) +static inline void mavlink_msg_autopilot_version_send(mavlink_channel_t chan, uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, const uint8_t *flight_custom_version, const uint8_t *middleware_custom_version, const uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid, const uint8_t *uid2) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN]; @@ -240,6 +251,7 @@ static inline void mavlink_msg_autopilot_version_send(mavlink_channel_t chan, ui _mav_put_uint8_t_array(buf, 36, flight_custom_version, 8); _mav_put_uint8_t_array(buf, 44, middleware_custom_version, 8); _mav_put_uint8_t_array(buf, 52, os_custom_version, 8); + _mav_put_uint8_t_array(buf, 60, uid2, 18); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); #else mavlink_autopilot_version_t packet; @@ -254,6 +266,7 @@ static inline void mavlink_msg_autopilot_version_send(mavlink_channel_t chan, ui mav_array_memcpy(packet.flight_custom_version, flight_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet.middleware_custom_version, middleware_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet.os_custom_version, os_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet.uid2, uid2, sizeof(uint8_t)*18); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, (const char *)&packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); #endif } @@ -266,7 +279,7 @@ static inline void mavlink_msg_autopilot_version_send(mavlink_channel_t chan, ui static inline void mavlink_msg_autopilot_version_send_struct(mavlink_channel_t chan, const mavlink_autopilot_version_t* autopilot_version) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_autopilot_version_send(chan, autopilot_version->capabilities, autopilot_version->flight_sw_version, autopilot_version->middleware_sw_version, autopilot_version->os_sw_version, autopilot_version->board_version, autopilot_version->flight_custom_version, autopilot_version->middleware_custom_version, autopilot_version->os_custom_version, autopilot_version->vendor_id, autopilot_version->product_id, autopilot_version->uid); + mavlink_msg_autopilot_version_send(chan, autopilot_version->capabilities, autopilot_version->flight_sw_version, autopilot_version->middleware_sw_version, autopilot_version->os_sw_version, autopilot_version->board_version, autopilot_version->flight_custom_version, autopilot_version->middleware_custom_version, autopilot_version->os_custom_version, autopilot_version->vendor_id, autopilot_version->product_id, autopilot_version->uid, autopilot_version->uid2); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, (const char *)autopilot_version, MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); #endif @@ -280,7 +293,7 @@ static inline void mavlink_msg_autopilot_version_send_struct(mavlink_channel_t c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_autopilot_version_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, const uint8_t *flight_custom_version, const uint8_t *middleware_custom_version, const uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid) +static inline void mavlink_msg_autopilot_version_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, const uint8_t *flight_custom_version, const uint8_t *middleware_custom_version, const uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid, const uint8_t *uid2) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -295,6 +308,7 @@ static inline void mavlink_msg_autopilot_version_send_buf(mavlink_message_t *msg _mav_put_uint8_t_array(buf, 36, flight_custom_version, 8); _mav_put_uint8_t_array(buf, 44, middleware_custom_version, 8); _mav_put_uint8_t_array(buf, 52, os_custom_version, 8); + _mav_put_uint8_t_array(buf, 60, uid2, 18); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); #else mavlink_autopilot_version_t *packet = (mavlink_autopilot_version_t *)msgbuf; @@ -309,6 +323,7 @@ static inline void mavlink_msg_autopilot_version_send_buf(mavlink_message_t *msg mav_array_memcpy(packet->flight_custom_version, flight_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet->middleware_custom_version, middleware_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet->os_custom_version, os_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet->uid2, uid2, sizeof(uint8_t)*18); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, (const char *)packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); #endif } @@ -429,6 +444,16 @@ static inline uint64_t mavlink_msg_autopilot_version_get_uid(const mavlink_messa return _MAV_RETURN_uint64_t(msg, 8); } +/** + * @brief Get field uid2 from autopilot_version message + * + * @return UID if provided by hardware (supersedes the uid field. If this is non-zero, use this field, otherwise use uid) + */ +static inline uint16_t mavlink_msg_autopilot_version_get_uid2(const mavlink_message_t* msg, uint8_t *uid2) +{ + return _MAV_RETURN_uint8_t_array(msg, uid2, 18, 60); +} + /** * @brief Decode a autopilot_version message into a struct * @@ -449,6 +474,7 @@ static inline void mavlink_msg_autopilot_version_decode(const mavlink_message_t* mavlink_msg_autopilot_version_get_flight_custom_version(msg, autopilot_version->flight_custom_version); mavlink_msg_autopilot_version_get_middleware_custom_version(msg, autopilot_version->middleware_custom_version); mavlink_msg_autopilot_version_get_os_custom_version(msg, autopilot_version->os_custom_version); + mavlink_msg_autopilot_version_get_uid2(msg, autopilot_version->uid2); #else uint8_t len = msg->len < MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN? msg->len : MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN; memset(autopilot_version, 0, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_battery_status.h b/lib/main/MAVLink/common/mavlink_msg_battery_status.h index 9d5ac516b2..084b2a3886 100755 --- a/lib/main/MAVLink/common/mavlink_msg_battery_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_battery_status.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_BATTERY_STATUS 147 - +MAVPACKED( typedef struct __mavlink_battery_status_t { int32_t current_consumed; /*< [mAh] Consumed charge, -1: autopilot does not provide consumption estimate*/ int32_t energy_consumed; /*< [hJ] Consumed energy, -1: autopilot does not provide energy consumption estimate*/ @@ -14,23 +14,29 @@ typedef struct __mavlink_battery_status_t { uint8_t battery_function; /*< Function of the battery*/ uint8_t type; /*< Type (chemistry) of the battery*/ int8_t battery_remaining; /*< [%] Remaining battery energy. Values: [0-100], -1: autopilot does not estimate the remaining battery.*/ -} mavlink_battery_status_t; + int32_t time_remaining; /*< [s] Remaining battery time, 0: autopilot does not provide remaining battery time estimate*/ + uint8_t charge_state; /*< State for extent of discharge, provided by autopilot for warning or external reactions*/ + uint16_t voltages_ext[4]; /*< [mV] Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead.*/ + uint8_t mode; /*< Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode.*/ + uint32_t fault_bitmask; /*< Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported).*/ +}) mavlink_battery_status_t; -#define MAVLINK_MSG_ID_BATTERY_STATUS_LEN 36 +#define MAVLINK_MSG_ID_BATTERY_STATUS_LEN 54 #define MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN 36 -#define MAVLINK_MSG_ID_147_LEN 36 +#define MAVLINK_MSG_ID_147_LEN 54 #define MAVLINK_MSG_ID_147_MIN_LEN 36 #define MAVLINK_MSG_ID_BATTERY_STATUS_CRC 154 #define MAVLINK_MSG_ID_147_CRC 154 #define MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN 10 +#define MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN 4 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_BATTERY_STATUS { \ 147, \ "BATTERY_STATUS", \ - 9, \ + 14, \ { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_battery_status_t, id) }, \ { "battery_function", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_battery_status_t, battery_function) }, \ { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_battery_status_t, type) }, \ @@ -40,12 +46,17 @@ typedef struct __mavlink_battery_status_t { { "current_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_battery_status_t, current_consumed) }, \ { "energy_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_battery_status_t, energy_consumed) }, \ { "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 35, offsetof(mavlink_battery_status_t, battery_remaining) }, \ + { "time_remaining", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_battery_status_t, time_remaining) }, \ + { "charge_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_battery_status_t, charge_state) }, \ + { "voltages_ext", NULL, MAVLINK_TYPE_UINT16_T, 4, 41, offsetof(mavlink_battery_status_t, voltages_ext) }, \ + { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 49, offsetof(mavlink_battery_status_t, mode) }, \ + { "fault_bitmask", NULL, MAVLINK_TYPE_UINT32_T, 0, 50, offsetof(mavlink_battery_status_t, fault_bitmask) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_BATTERY_STATUS { \ "BATTERY_STATUS", \ - 9, \ + 14, \ { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_battery_status_t, id) }, \ { "battery_function", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_battery_status_t, battery_function) }, \ { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_battery_status_t, type) }, \ @@ -55,6 +66,11 @@ typedef struct __mavlink_battery_status_t { { "current_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_battery_status_t, current_consumed) }, \ { "energy_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_battery_status_t, energy_consumed) }, \ { "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 35, offsetof(mavlink_battery_status_t, battery_remaining) }, \ + { "time_remaining", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_battery_status_t, time_remaining) }, \ + { "charge_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_battery_status_t, charge_state) }, \ + { "voltages_ext", NULL, MAVLINK_TYPE_UINT16_T, 4, 41, offsetof(mavlink_battery_status_t, voltages_ext) }, \ + { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 49, offsetof(mavlink_battery_status_t, mode) }, \ + { "fault_bitmask", NULL, MAVLINK_TYPE_UINT32_T, 0, 50, offsetof(mavlink_battery_status_t, fault_bitmask) }, \ } \ } #endif @@ -74,10 +90,15 @@ typedef struct __mavlink_battery_status_t { * @param current_consumed [mAh] Consumed charge, -1: autopilot does not provide consumption estimate * @param energy_consumed [hJ] Consumed energy, -1: autopilot does not provide energy consumption estimate * @param battery_remaining [%] Remaining battery energy. Values: [0-100], -1: autopilot does not estimate the remaining battery. + * @param time_remaining [s] Remaining battery time, 0: autopilot does not provide remaining battery time estimate + * @param charge_state State for extent of discharge, provided by autopilot for warning or external reactions + * @param voltages_ext [mV] Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead. + * @param mode Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode. + * @param fault_bitmask Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) + uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining, int32_t time_remaining, uint8_t charge_state, const uint16_t *voltages_ext, uint8_t mode, uint32_t fault_bitmask) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; @@ -89,7 +110,12 @@ static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_ _mav_put_uint8_t(buf, 33, battery_function); _mav_put_uint8_t(buf, 34, type); _mav_put_int8_t(buf, 35, battery_remaining); + _mav_put_int32_t(buf, 36, time_remaining); + _mav_put_uint8_t(buf, 40, charge_state); + _mav_put_uint8_t(buf, 49, mode); + _mav_put_uint32_t(buf, 50, fault_bitmask); _mav_put_uint16_t_array(buf, 10, voltages, 10); + _mav_put_uint16_t_array(buf, 41, voltages_ext, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); #else mavlink_battery_status_t packet; @@ -101,7 +127,12 @@ static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_ packet.battery_function = battery_function; packet.type = type; packet.battery_remaining = battery_remaining; + packet.time_remaining = time_remaining; + packet.charge_state = charge_state; + packet.mode = mode; + packet.fault_bitmask = fault_bitmask; mav_array_memcpy(packet.voltages, voltages, sizeof(uint16_t)*10); + mav_array_memcpy(packet.voltages_ext, voltages_ext, sizeof(uint16_t)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); #endif @@ -124,11 +155,16 @@ static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_ * @param current_consumed [mAh] Consumed charge, -1: autopilot does not provide consumption estimate * @param energy_consumed [hJ] Consumed energy, -1: autopilot does not provide energy consumption estimate * @param battery_remaining [%] Remaining battery energy. Values: [0-100], -1: autopilot does not estimate the remaining battery. + * @param time_remaining [s] Remaining battery time, 0: autopilot does not provide remaining battery time estimate + * @param charge_state State for extent of discharge, provided by autopilot for warning or external reactions + * @param voltages_ext [mV] Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead. + * @param mode Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode. + * @param fault_bitmask Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t id,uint8_t battery_function,uint8_t type,int16_t temperature,const uint16_t *voltages,int16_t current_battery,int32_t current_consumed,int32_t energy_consumed,int8_t battery_remaining) + uint8_t id,uint8_t battery_function,uint8_t type,int16_t temperature,const uint16_t *voltages,int16_t current_battery,int32_t current_consumed,int32_t energy_consumed,int8_t battery_remaining,int32_t time_remaining,uint8_t charge_state,const uint16_t *voltages_ext,uint8_t mode,uint32_t fault_bitmask) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; @@ -140,7 +176,12 @@ static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, u _mav_put_uint8_t(buf, 33, battery_function); _mav_put_uint8_t(buf, 34, type); _mav_put_int8_t(buf, 35, battery_remaining); + _mav_put_int32_t(buf, 36, time_remaining); + _mav_put_uint8_t(buf, 40, charge_state); + _mav_put_uint8_t(buf, 49, mode); + _mav_put_uint32_t(buf, 50, fault_bitmask); _mav_put_uint16_t_array(buf, 10, voltages, 10); + _mav_put_uint16_t_array(buf, 41, voltages_ext, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); #else mavlink_battery_status_t packet; @@ -152,7 +193,12 @@ static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, u packet.battery_function = battery_function; packet.type = type; packet.battery_remaining = battery_remaining; + packet.time_remaining = time_remaining; + packet.charge_state = charge_state; + packet.mode = mode; + packet.fault_bitmask = fault_bitmask; mav_array_memcpy(packet.voltages, voltages, sizeof(uint16_t)*10); + mav_array_memcpy(packet.voltages_ext, voltages_ext, sizeof(uint16_t)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); #endif @@ -170,7 +216,7 @@ static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, u */ static inline uint16_t mavlink_msg_battery_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status) { - return mavlink_msg_battery_status_pack(system_id, component_id, msg, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining); + return mavlink_msg_battery_status_pack(system_id, component_id, msg, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining, battery_status->time_remaining, battery_status->charge_state, battery_status->voltages_ext, battery_status->mode, battery_status->fault_bitmask); } /** @@ -184,7 +230,7 @@ static inline uint16_t mavlink_msg_battery_status_encode(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_battery_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status) { - return mavlink_msg_battery_status_pack_chan(system_id, component_id, chan, msg, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining); + return mavlink_msg_battery_status_pack_chan(system_id, component_id, chan, msg, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining, battery_status->time_remaining, battery_status->charge_state, battery_status->voltages_ext, battery_status->mode, battery_status->fault_bitmask); } /** @@ -200,10 +246,15 @@ static inline uint16_t mavlink_msg_battery_status_encode_chan(uint8_t system_id, * @param current_consumed [mAh] Consumed charge, -1: autopilot does not provide consumption estimate * @param energy_consumed [hJ] Consumed energy, -1: autopilot does not provide energy consumption estimate * @param battery_remaining [%] Remaining battery energy. Values: [0-100], -1: autopilot does not estimate the remaining battery. + * @param time_remaining [s] Remaining battery time, 0: autopilot does not provide remaining battery time estimate + * @param charge_state State for extent of discharge, provided by autopilot for warning or external reactions + * @param voltages_ext [mV] Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead. + * @param mode Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode. + * @param fault_bitmask Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) +static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining, int32_t time_remaining, uint8_t charge_state, const uint16_t *voltages_ext, uint8_t mode, uint32_t fault_bitmask) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; @@ -215,7 +266,12 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8 _mav_put_uint8_t(buf, 33, battery_function); _mav_put_uint8_t(buf, 34, type); _mav_put_int8_t(buf, 35, battery_remaining); + _mav_put_int32_t(buf, 36, time_remaining); + _mav_put_uint8_t(buf, 40, charge_state); + _mav_put_uint8_t(buf, 49, mode); + _mav_put_uint32_t(buf, 50, fault_bitmask); _mav_put_uint16_t_array(buf, 10, voltages, 10); + _mav_put_uint16_t_array(buf, 41, voltages_ext, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); #else mavlink_battery_status_t packet; @@ -227,7 +283,12 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8 packet.battery_function = battery_function; packet.type = type; packet.battery_remaining = battery_remaining; + packet.time_remaining = time_remaining; + packet.charge_state = charge_state; + packet.mode = mode; + packet.fault_bitmask = fault_bitmask; mav_array_memcpy(packet.voltages, voltages, sizeof(uint16_t)*10); + mav_array_memcpy(packet.voltages_ext, voltages_ext, sizeof(uint16_t)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)&packet, MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); #endif } @@ -240,7 +301,7 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8 static inline void mavlink_msg_battery_status_send_struct(mavlink_channel_t chan, const mavlink_battery_status_t* battery_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_battery_status_send(chan, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining); + mavlink_msg_battery_status_send(chan, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining, battery_status->time_remaining, battery_status->charge_state, battery_status->voltages_ext, battery_status->mode, battery_status->fault_bitmask); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)battery_status, MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); #endif @@ -254,7 +315,7 @@ static inline void mavlink_msg_battery_status_send_struct(mavlink_channel_t chan is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_battery_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) +static inline void mavlink_msg_battery_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining, int32_t time_remaining, uint8_t charge_state, const uint16_t *voltages_ext, uint8_t mode, uint32_t fault_bitmask) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -266,7 +327,12 @@ static inline void mavlink_msg_battery_status_send_buf(mavlink_message_t *msgbuf _mav_put_uint8_t(buf, 33, battery_function); _mav_put_uint8_t(buf, 34, type); _mav_put_int8_t(buf, 35, battery_remaining); + _mav_put_int32_t(buf, 36, time_remaining); + _mav_put_uint8_t(buf, 40, charge_state); + _mav_put_uint8_t(buf, 49, mode); + _mav_put_uint32_t(buf, 50, fault_bitmask); _mav_put_uint16_t_array(buf, 10, voltages, 10); + _mav_put_uint16_t_array(buf, 41, voltages_ext, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); #else mavlink_battery_status_t *packet = (mavlink_battery_status_t *)msgbuf; @@ -278,7 +344,12 @@ static inline void mavlink_msg_battery_status_send_buf(mavlink_message_t *msgbuf packet->battery_function = battery_function; packet->type = type; packet->battery_remaining = battery_remaining; + packet->time_remaining = time_remaining; + packet->charge_state = charge_state; + packet->mode = mode; + packet->fault_bitmask = fault_bitmask; mav_array_memcpy(packet->voltages, voltages, sizeof(uint16_t)*10); + mav_array_memcpy(packet->voltages_ext, voltages_ext, sizeof(uint16_t)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); #endif } @@ -379,6 +450,56 @@ static inline int8_t mavlink_msg_battery_status_get_battery_remaining(const mavl return _MAV_RETURN_int8_t(msg, 35); } +/** + * @brief Get field time_remaining from battery_status message + * + * @return [s] Remaining battery time, 0: autopilot does not provide remaining battery time estimate + */ +static inline int32_t mavlink_msg_battery_status_get_time_remaining(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 36); +} + +/** + * @brief Get field charge_state from battery_status message + * + * @return State for extent of discharge, provided by autopilot for warning or external reactions + */ +static inline uint8_t mavlink_msg_battery_status_get_charge_state(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 40); +} + +/** + * @brief Get field voltages_ext from battery_status message + * + * @return [mV] Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead. + */ +static inline uint16_t mavlink_msg_battery_status_get_voltages_ext(const mavlink_message_t* msg, uint16_t *voltages_ext) +{ + return _MAV_RETURN_uint16_t_array(msg, voltages_ext, 4, 41); +} + +/** + * @brief Get field mode from battery_status message + * + * @return Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode. + */ +static inline uint8_t mavlink_msg_battery_status_get_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 49); +} + +/** + * @brief Get field fault_bitmask from battery_status message + * + * @return Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported). + */ +static inline uint32_t mavlink_msg_battery_status_get_fault_bitmask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 50); +} + /** * @brief Decode a battery_status message into a struct * @@ -397,6 +518,11 @@ static inline void mavlink_msg_battery_status_decode(const mavlink_message_t* ms battery_status->battery_function = mavlink_msg_battery_status_get_battery_function(msg); battery_status->type = mavlink_msg_battery_status_get_type(msg); battery_status->battery_remaining = mavlink_msg_battery_status_get_battery_remaining(msg); + battery_status->time_remaining = mavlink_msg_battery_status_get_time_remaining(msg); + battery_status->charge_state = mavlink_msg_battery_status_get_charge_state(msg); + mavlink_msg_battery_status_get_voltages_ext(msg, battery_status->voltages_ext); + battery_status->mode = mavlink_msg_battery_status_get_mode(msg); + battery_status->fault_bitmask = mavlink_msg_battery_status_get_fault_bitmask(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_BATTERY_STATUS_LEN? msg->len : MAVLINK_MSG_ID_BATTERY_STATUS_LEN; memset(battery_status, 0, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_button_change.h b/lib/main/MAVLink/common/mavlink_msg_button_change.h new file mode 100644 index 0000000000..831bc60fc3 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_button_change.h @@ -0,0 +1,263 @@ +#pragma once +// MESSAGE BUTTON_CHANGE PACKING + +#define MAVLINK_MSG_ID_BUTTON_CHANGE 257 + + +typedef struct __mavlink_button_change_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + uint32_t last_change_ms; /*< [ms] Time of last change of button state.*/ + uint8_t state; /*< Bitmap for state of buttons.*/ +} mavlink_button_change_t; + +#define MAVLINK_MSG_ID_BUTTON_CHANGE_LEN 9 +#define MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN 9 +#define MAVLINK_MSG_ID_257_LEN 9 +#define MAVLINK_MSG_ID_257_MIN_LEN 9 + +#define MAVLINK_MSG_ID_BUTTON_CHANGE_CRC 131 +#define MAVLINK_MSG_ID_257_CRC 131 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_BUTTON_CHANGE { \ + 257, \ + "BUTTON_CHANGE", \ + 3, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_button_change_t, time_boot_ms) }, \ + { "last_change_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_button_change_t, last_change_ms) }, \ + { "state", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_button_change_t, state) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_BUTTON_CHANGE { \ + "BUTTON_CHANGE", \ + 3, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_button_change_t, time_boot_ms) }, \ + { "last_change_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_button_change_t, last_change_ms) }, \ + { "state", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_button_change_t, state) }, \ + } \ +} +#endif + +/** + * @brief Pack a button_change message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param last_change_ms [ms] Time of last change of button state. + * @param state Bitmap for state of buttons. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_button_change_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint32_t last_change_ms, uint8_t state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BUTTON_CHANGE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, last_change_ms); + _mav_put_uint8_t(buf, 8, state); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN); +#else + mavlink_button_change_t packet; + packet.time_boot_ms = time_boot_ms; + packet.last_change_ms = last_change_ms; + packet.state = state; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_BUTTON_CHANGE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_CRC); +} + +/** + * @brief Pack a button_change message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param last_change_ms [ms] Time of last change of button state. + * @param state Bitmap for state of buttons. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_button_change_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint32_t last_change_ms,uint8_t state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BUTTON_CHANGE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, last_change_ms); + _mav_put_uint8_t(buf, 8, state); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN); +#else + mavlink_button_change_t packet; + packet.time_boot_ms = time_boot_ms; + packet.last_change_ms = last_change_ms; + packet.state = state; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_BUTTON_CHANGE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_CRC); +} + +/** + * @brief Encode a button_change struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param button_change C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_button_change_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_button_change_t* button_change) +{ + return mavlink_msg_button_change_pack(system_id, component_id, msg, button_change->time_boot_ms, button_change->last_change_ms, button_change->state); +} + +/** + * @brief Encode a button_change struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param button_change C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_button_change_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_button_change_t* button_change) +{ + return mavlink_msg_button_change_pack_chan(system_id, component_id, chan, msg, button_change->time_boot_ms, button_change->last_change_ms, button_change->state); +} + +/** + * @brief Send a button_change message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param last_change_ms [ms] Time of last change of button state. + * @param state Bitmap for state of buttons. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_button_change_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t last_change_ms, uint8_t state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BUTTON_CHANGE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, last_change_ms); + _mav_put_uint8_t(buf, 8, state); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BUTTON_CHANGE, buf, MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_CRC); +#else + mavlink_button_change_t packet; + packet.time_boot_ms = time_boot_ms; + packet.last_change_ms = last_change_ms; + packet.state = state; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BUTTON_CHANGE, (const char *)&packet, MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_CRC); +#endif +} + +/** + * @brief Send a button_change message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_button_change_send_struct(mavlink_channel_t chan, const mavlink_button_change_t* button_change) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_button_change_send(chan, button_change->time_boot_ms, button_change->last_change_ms, button_change->state); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BUTTON_CHANGE, (const char *)button_change, MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_BUTTON_CHANGE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_button_change_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t last_change_ms, uint8_t state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, last_change_ms); + _mav_put_uint8_t(buf, 8, state); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BUTTON_CHANGE, buf, MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_CRC); +#else + mavlink_button_change_t *packet = (mavlink_button_change_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->last_change_ms = last_change_ms; + packet->state = state; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BUTTON_CHANGE, (const char *)packet, MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE BUTTON_CHANGE UNPACKING + + +/** + * @brief Get field time_boot_ms from button_change message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_button_change_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field last_change_ms from button_change message + * + * @return [ms] Time of last change of button state. + */ +static inline uint32_t mavlink_msg_button_change_get_last_change_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field state from button_change message + * + * @return Bitmap for state of buttons. + */ +static inline uint8_t mavlink_msg_button_change_get_state(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Decode a button_change message into a struct + * + * @param msg The message to decode + * @param button_change C-struct to decode the message contents into + */ +static inline void mavlink_msg_button_change_decode(const mavlink_message_t* msg, mavlink_button_change_t* button_change) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + button_change->time_boot_ms = mavlink_msg_button_change_get_time_boot_ms(msg); + button_change->last_change_ms = mavlink_msg_button_change_get_last_change_ms(msg); + button_change->state = mavlink_msg_button_change_get_state(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_BUTTON_CHANGE_LEN? msg->len : MAVLINK_MSG_ID_BUTTON_CHANGE_LEN; + memset(button_change, 0, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN); + memcpy(button_change, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_capture_status.h b/lib/main/MAVLink/common/mavlink_msg_camera_capture_status.h new file mode 100644 index 0000000000..8951db6a79 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_camera_capture_status.h @@ -0,0 +1,363 @@ +#pragma once +// MESSAGE CAMERA_CAPTURE_STATUS PACKING + +#define MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS 262 + +MAVPACKED( +typedef struct __mavlink_camera_capture_status_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + float image_interval; /*< [s] Image capture interval*/ + uint32_t recording_time_ms; /*< [ms] Time since recording started*/ + float available_capacity; /*< [MiB] Available storage capacity.*/ + uint8_t image_status; /*< Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress)*/ + uint8_t video_status; /*< Current status of video capturing (0: idle, 1: capture in progress)*/ + int32_t image_count; /*< Total number of images captured ('forever', or until reset using MAV_CMD_STORAGE_FORMAT).*/ +}) mavlink_camera_capture_status_t; + +#define MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN 22 +#define MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN 18 +#define MAVLINK_MSG_ID_262_LEN 22 +#define MAVLINK_MSG_ID_262_MIN_LEN 18 + +#define MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC 12 +#define MAVLINK_MSG_ID_262_CRC 12 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CAMERA_CAPTURE_STATUS { \ + 262, \ + "CAMERA_CAPTURE_STATUS", \ + 7, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_capture_status_t, time_boot_ms) }, \ + { "image_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_camera_capture_status_t, image_status) }, \ + { "video_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_camera_capture_status_t, video_status) }, \ + { "image_interval", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_camera_capture_status_t, image_interval) }, \ + { "recording_time_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_camera_capture_status_t, recording_time_ms) }, \ + { "available_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_capture_status_t, available_capacity) }, \ + { "image_count", NULL, MAVLINK_TYPE_INT32_T, 0, 18, offsetof(mavlink_camera_capture_status_t, image_count) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CAMERA_CAPTURE_STATUS { \ + "CAMERA_CAPTURE_STATUS", \ + 7, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_capture_status_t, time_boot_ms) }, \ + { "image_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_camera_capture_status_t, image_status) }, \ + { "video_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_camera_capture_status_t, video_status) }, \ + { "image_interval", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_camera_capture_status_t, image_interval) }, \ + { "recording_time_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_camera_capture_status_t, recording_time_ms) }, \ + { "available_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_capture_status_t, available_capacity) }, \ + { "image_count", NULL, MAVLINK_TYPE_INT32_T, 0, 18, offsetof(mavlink_camera_capture_status_t, image_count) }, \ + } \ +} +#endif + +/** + * @brief Pack a camera_capture_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param image_status Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress) + * @param video_status Current status of video capturing (0: idle, 1: capture in progress) + * @param image_interval [s] Image capture interval + * @param recording_time_ms [ms] Time since recording started + * @param available_capacity [MiB] Available storage capacity. + * @param image_count Total number of images captured ('forever', or until reset using MAV_CMD_STORAGE_FORMAT). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_capture_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t image_status, uint8_t video_status, float image_interval, uint32_t recording_time_ms, float available_capacity, int32_t image_count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, image_interval); + _mav_put_uint32_t(buf, 8, recording_time_ms); + _mav_put_float(buf, 12, available_capacity); + _mav_put_uint8_t(buf, 16, image_status); + _mav_put_uint8_t(buf, 17, video_status); + _mav_put_int32_t(buf, 18, image_count); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); +#else + mavlink_camera_capture_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.image_interval = image_interval; + packet.recording_time_ms = recording_time_ms; + packet.available_capacity = available_capacity; + packet.image_status = image_status; + packet.video_status = video_status; + packet.image_count = image_count; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); +} + +/** + * @brief Pack a camera_capture_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param image_status Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress) + * @param video_status Current status of video capturing (0: idle, 1: capture in progress) + * @param image_interval [s] Image capture interval + * @param recording_time_ms [ms] Time since recording started + * @param available_capacity [MiB] Available storage capacity. + * @param image_count Total number of images captured ('forever', or until reset using MAV_CMD_STORAGE_FORMAT). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_capture_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t image_status,uint8_t video_status,float image_interval,uint32_t recording_time_ms,float available_capacity,int32_t image_count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, image_interval); + _mav_put_uint32_t(buf, 8, recording_time_ms); + _mav_put_float(buf, 12, available_capacity); + _mav_put_uint8_t(buf, 16, image_status); + _mav_put_uint8_t(buf, 17, video_status); + _mav_put_int32_t(buf, 18, image_count); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); +#else + mavlink_camera_capture_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.image_interval = image_interval; + packet.recording_time_ms = recording_time_ms; + packet.available_capacity = available_capacity; + packet.image_status = image_status; + packet.video_status = video_status; + packet.image_count = image_count; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); +} + +/** + * @brief Encode a camera_capture_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_capture_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_capture_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_capture_status_t* camera_capture_status) +{ + return mavlink_msg_camera_capture_status_pack(system_id, component_id, msg, camera_capture_status->time_boot_ms, camera_capture_status->image_status, camera_capture_status->video_status, camera_capture_status->image_interval, camera_capture_status->recording_time_ms, camera_capture_status->available_capacity, camera_capture_status->image_count); +} + +/** + * @brief Encode a camera_capture_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_capture_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_capture_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_capture_status_t* camera_capture_status) +{ + return mavlink_msg_camera_capture_status_pack_chan(system_id, component_id, chan, msg, camera_capture_status->time_boot_ms, camera_capture_status->image_status, camera_capture_status->video_status, camera_capture_status->image_interval, camera_capture_status->recording_time_ms, camera_capture_status->available_capacity, camera_capture_status->image_count); +} + +/** + * @brief Send a camera_capture_status message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param image_status Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress) + * @param video_status Current status of video capturing (0: idle, 1: capture in progress) + * @param image_interval [s] Image capture interval + * @param recording_time_ms [ms] Time since recording started + * @param available_capacity [MiB] Available storage capacity. + * @param image_count Total number of images captured ('forever', or until reset using MAV_CMD_STORAGE_FORMAT). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_capture_status_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t image_status, uint8_t video_status, float image_interval, uint32_t recording_time_ms, float available_capacity, int32_t image_count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, image_interval); + _mav_put_uint32_t(buf, 8, recording_time_ms); + _mav_put_float(buf, 12, available_capacity); + _mav_put_uint8_t(buf, 16, image_status); + _mav_put_uint8_t(buf, 17, video_status); + _mav_put_int32_t(buf, 18, image_count); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, buf, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); +#else + mavlink_camera_capture_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.image_interval = image_interval; + packet.recording_time_ms = recording_time_ms; + packet.available_capacity = available_capacity; + packet.image_status = image_status; + packet.video_status = video_status; + packet.image_count = image_count; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); +#endif +} + +/** + * @brief Send a camera_capture_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_camera_capture_status_send_struct(mavlink_channel_t chan, const mavlink_camera_capture_status_t* camera_capture_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_camera_capture_status_send(chan, camera_capture_status->time_boot_ms, camera_capture_status->image_status, camera_capture_status->video_status, camera_capture_status->image_interval, camera_capture_status->recording_time_ms, camera_capture_status->available_capacity, camera_capture_status->image_count); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, (const char *)camera_capture_status, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_capture_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t image_status, uint8_t video_status, float image_interval, uint32_t recording_time_ms, float available_capacity, int32_t image_count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, image_interval); + _mav_put_uint32_t(buf, 8, recording_time_ms); + _mav_put_float(buf, 12, available_capacity); + _mav_put_uint8_t(buf, 16, image_status); + _mav_put_uint8_t(buf, 17, video_status); + _mav_put_int32_t(buf, 18, image_count); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, buf, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); +#else + mavlink_camera_capture_status_t *packet = (mavlink_camera_capture_status_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->image_interval = image_interval; + packet->recording_time_ms = recording_time_ms; + packet->available_capacity = available_capacity; + packet->image_status = image_status; + packet->video_status = video_status; + packet->image_count = image_count; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, (const char *)packet, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_CAPTURE_STATUS UNPACKING + + +/** + * @brief Get field time_boot_ms from camera_capture_status message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_camera_capture_status_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field image_status from camera_capture_status message + * + * @return Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress) + */ +static inline uint8_t mavlink_msg_camera_capture_status_get_image_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 16); +} + +/** + * @brief Get field video_status from camera_capture_status message + * + * @return Current status of video capturing (0: idle, 1: capture in progress) + */ +static inline uint8_t mavlink_msg_camera_capture_status_get_video_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 17); +} + +/** + * @brief Get field image_interval from camera_capture_status message + * + * @return [s] Image capture interval + */ +static inline float mavlink_msg_camera_capture_status_get_image_interval(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field recording_time_ms from camera_capture_status message + * + * @return [ms] Time since recording started + */ +static inline uint32_t mavlink_msg_camera_capture_status_get_recording_time_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field available_capacity from camera_capture_status message + * + * @return [MiB] Available storage capacity. + */ +static inline float mavlink_msg_camera_capture_status_get_available_capacity(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field image_count from camera_capture_status message + * + * @return Total number of images captured ('forever', or until reset using MAV_CMD_STORAGE_FORMAT). + */ +static inline int32_t mavlink_msg_camera_capture_status_get_image_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 18); +} + +/** + * @brief Decode a camera_capture_status message into a struct + * + * @param msg The message to decode + * @param camera_capture_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_capture_status_decode(const mavlink_message_t* msg, mavlink_camera_capture_status_t* camera_capture_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + camera_capture_status->time_boot_ms = mavlink_msg_camera_capture_status_get_time_boot_ms(msg); + camera_capture_status->image_interval = mavlink_msg_camera_capture_status_get_image_interval(msg); + camera_capture_status->recording_time_ms = mavlink_msg_camera_capture_status_get_recording_time_ms(msg); + camera_capture_status->available_capacity = mavlink_msg_camera_capture_status_get_available_capacity(msg); + camera_capture_status->image_status = mavlink_msg_camera_capture_status_get_image_status(msg); + camera_capture_status->video_status = mavlink_msg_camera_capture_status_get_video_status(msg); + camera_capture_status->image_count = mavlink_msg_camera_capture_status_get_image_count(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN; + memset(camera_capture_status, 0, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); + memcpy(camera_capture_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_fov_status.h b/lib/main/MAVLink/common/mavlink_msg_camera_fov_status.h new file mode 100644 index 0000000000..6378e56aa8 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_camera_fov_status.h @@ -0,0 +1,430 @@ +#pragma once +// MESSAGE CAMERA_FOV_STATUS PACKING + +#define MAVLINK_MSG_ID_CAMERA_FOV_STATUS 271 + + +typedef struct __mavlink_camera_fov_status_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + int32_t lat_camera; /*< [degE7] Latitude of camera (INT32_MAX if unknown).*/ + int32_t lon_camera; /*< [degE7] Longitude of camera (INT32_MAX if unknown).*/ + int32_t alt_camera; /*< [mm] Altitude (MSL) of camera (INT32_MAX if unknown).*/ + int32_t lat_image; /*< [degE7] Latitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon).*/ + int32_t lon_image; /*< [degE7] Longitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon).*/ + int32_t alt_image; /*< [mm] Altitude (MSL) of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon).*/ + float q[4]; /*< Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0)*/ + float hfov; /*< [deg] Horizontal field of view (NaN if unknown).*/ + float vfov; /*< [deg] Vertical field of view (NaN if unknown).*/ +} mavlink_camera_fov_status_t; + +#define MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN 52 +#define MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN 52 +#define MAVLINK_MSG_ID_271_LEN 52 +#define MAVLINK_MSG_ID_271_MIN_LEN 52 + +#define MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC 22 +#define MAVLINK_MSG_ID_271_CRC 22 + +#define MAVLINK_MSG_CAMERA_FOV_STATUS_FIELD_Q_LEN 4 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CAMERA_FOV_STATUS { \ + 271, \ + "CAMERA_FOV_STATUS", \ + 10, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_fov_status_t, time_boot_ms) }, \ + { "lat_camera", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_camera_fov_status_t, lat_camera) }, \ + { "lon_camera", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_camera_fov_status_t, lon_camera) }, \ + { "alt_camera", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_camera_fov_status_t, alt_camera) }, \ + { "lat_image", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_camera_fov_status_t, lat_image) }, \ + { "lon_image", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_camera_fov_status_t, lon_image) }, \ + { "alt_image", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_camera_fov_status_t, alt_image) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 28, offsetof(mavlink_camera_fov_status_t, q) }, \ + { "hfov", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_camera_fov_status_t, hfov) }, \ + { "vfov", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_camera_fov_status_t, vfov) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CAMERA_FOV_STATUS { \ + "CAMERA_FOV_STATUS", \ + 10, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_fov_status_t, time_boot_ms) }, \ + { "lat_camera", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_camera_fov_status_t, lat_camera) }, \ + { "lon_camera", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_camera_fov_status_t, lon_camera) }, \ + { "alt_camera", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_camera_fov_status_t, alt_camera) }, \ + { "lat_image", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_camera_fov_status_t, lat_image) }, \ + { "lon_image", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_camera_fov_status_t, lon_image) }, \ + { "alt_image", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_camera_fov_status_t, alt_image) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 28, offsetof(mavlink_camera_fov_status_t, q) }, \ + { "hfov", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_camera_fov_status_t, hfov) }, \ + { "vfov", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_camera_fov_status_t, vfov) }, \ + } \ +} +#endif + +/** + * @brief Pack a camera_fov_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param lat_camera [degE7] Latitude of camera (INT32_MAX if unknown). + * @param lon_camera [degE7] Longitude of camera (INT32_MAX if unknown). + * @param alt_camera [mm] Altitude (MSL) of camera (INT32_MAX if unknown). + * @param lat_image [degE7] Latitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param lon_image [degE7] Longitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param alt_image [mm] Altitude (MSL) of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param hfov [deg] Horizontal field of view (NaN if unknown). + * @param vfov [deg] Vertical field of view (NaN if unknown). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_fov_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, int32_t lat_camera, int32_t lon_camera, int32_t alt_camera, int32_t lat_image, int32_t lon_image, int32_t alt_image, const float *q, float hfov, float vfov) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_camera); + _mav_put_int32_t(buf, 8, lon_camera); + _mav_put_int32_t(buf, 12, alt_camera); + _mav_put_int32_t(buf, 16, lat_image); + _mav_put_int32_t(buf, 20, lon_image); + _mav_put_int32_t(buf, 24, alt_image); + _mav_put_float(buf, 44, hfov); + _mav_put_float(buf, 48, vfov); + _mav_put_float_array(buf, 28, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); +#else + mavlink_camera_fov_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_camera = lat_camera; + packet.lon_camera = lon_camera; + packet.alt_camera = alt_camera; + packet.lat_image = lat_image; + packet.lon_image = lon_image; + packet.alt_image = alt_image; + packet.hfov = hfov; + packet.vfov = vfov; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_FOV_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); +} + +/** + * @brief Pack a camera_fov_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param lat_camera [degE7] Latitude of camera (INT32_MAX if unknown). + * @param lon_camera [degE7] Longitude of camera (INT32_MAX if unknown). + * @param alt_camera [mm] Altitude (MSL) of camera (INT32_MAX if unknown). + * @param lat_image [degE7] Latitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param lon_image [degE7] Longitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param alt_image [mm] Altitude (MSL) of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param hfov [deg] Horizontal field of view (NaN if unknown). + * @param vfov [deg] Vertical field of view (NaN if unknown). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_fov_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,int32_t lat_camera,int32_t lon_camera,int32_t alt_camera,int32_t lat_image,int32_t lon_image,int32_t alt_image,const float *q,float hfov,float vfov) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_camera); + _mav_put_int32_t(buf, 8, lon_camera); + _mav_put_int32_t(buf, 12, alt_camera); + _mav_put_int32_t(buf, 16, lat_image); + _mav_put_int32_t(buf, 20, lon_image); + _mav_put_int32_t(buf, 24, alt_image); + _mav_put_float(buf, 44, hfov); + _mav_put_float(buf, 48, vfov); + _mav_put_float_array(buf, 28, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); +#else + mavlink_camera_fov_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_camera = lat_camera; + packet.lon_camera = lon_camera; + packet.alt_camera = alt_camera; + packet.lat_image = lat_image; + packet.lon_image = lon_image; + packet.alt_image = alt_image; + packet.hfov = hfov; + packet.vfov = vfov; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_FOV_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); +} + +/** + * @brief Encode a camera_fov_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_fov_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_fov_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_fov_status_t* camera_fov_status) +{ + return mavlink_msg_camera_fov_status_pack(system_id, component_id, msg, camera_fov_status->time_boot_ms, camera_fov_status->lat_camera, camera_fov_status->lon_camera, camera_fov_status->alt_camera, camera_fov_status->lat_image, camera_fov_status->lon_image, camera_fov_status->alt_image, camera_fov_status->q, camera_fov_status->hfov, camera_fov_status->vfov); +} + +/** + * @brief Encode a camera_fov_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_fov_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_fov_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_fov_status_t* camera_fov_status) +{ + return mavlink_msg_camera_fov_status_pack_chan(system_id, component_id, chan, msg, camera_fov_status->time_boot_ms, camera_fov_status->lat_camera, camera_fov_status->lon_camera, camera_fov_status->alt_camera, camera_fov_status->lat_image, camera_fov_status->lon_image, camera_fov_status->alt_image, camera_fov_status->q, camera_fov_status->hfov, camera_fov_status->vfov); +} + +/** + * @brief Send a camera_fov_status message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param lat_camera [degE7] Latitude of camera (INT32_MAX if unknown). + * @param lon_camera [degE7] Longitude of camera (INT32_MAX if unknown). + * @param alt_camera [mm] Altitude (MSL) of camera (INT32_MAX if unknown). + * @param lat_image [degE7] Latitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param lon_image [degE7] Longitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param alt_image [mm] Altitude (MSL) of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param hfov [deg] Horizontal field of view (NaN if unknown). + * @param vfov [deg] Vertical field of view (NaN if unknown). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_fov_status_send(mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat_camera, int32_t lon_camera, int32_t alt_camera, int32_t lat_image, int32_t lon_image, int32_t alt_image, const float *q, float hfov, float vfov) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_camera); + _mav_put_int32_t(buf, 8, lon_camera); + _mav_put_int32_t(buf, 12, alt_camera); + _mav_put_int32_t(buf, 16, lat_image); + _mav_put_int32_t(buf, 20, lon_image); + _mav_put_int32_t(buf, 24, alt_image); + _mav_put_float(buf, 44, hfov); + _mav_put_float(buf, 48, vfov); + _mav_put_float_array(buf, 28, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FOV_STATUS, buf, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); +#else + mavlink_camera_fov_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_camera = lat_camera; + packet.lon_camera = lon_camera; + packet.alt_camera = alt_camera; + packet.lat_image = lat_image; + packet.lon_image = lon_image; + packet.alt_image = alt_image; + packet.hfov = hfov; + packet.vfov = vfov; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FOV_STATUS, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); +#endif +} + +/** + * @brief Send a camera_fov_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_camera_fov_status_send_struct(mavlink_channel_t chan, const mavlink_camera_fov_status_t* camera_fov_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_camera_fov_status_send(chan, camera_fov_status->time_boot_ms, camera_fov_status->lat_camera, camera_fov_status->lon_camera, camera_fov_status->alt_camera, camera_fov_status->lat_image, camera_fov_status->lon_image, camera_fov_status->alt_image, camera_fov_status->q, camera_fov_status->hfov, camera_fov_status->vfov); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FOV_STATUS, (const char *)camera_fov_status, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_fov_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat_camera, int32_t lon_camera, int32_t alt_camera, int32_t lat_image, int32_t lon_image, int32_t alt_image, const float *q, float hfov, float vfov) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_camera); + _mav_put_int32_t(buf, 8, lon_camera); + _mav_put_int32_t(buf, 12, alt_camera); + _mav_put_int32_t(buf, 16, lat_image); + _mav_put_int32_t(buf, 20, lon_image); + _mav_put_int32_t(buf, 24, alt_image); + _mav_put_float(buf, 44, hfov); + _mav_put_float(buf, 48, vfov); + _mav_put_float_array(buf, 28, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FOV_STATUS, buf, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); +#else + mavlink_camera_fov_status_t *packet = (mavlink_camera_fov_status_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->lat_camera = lat_camera; + packet->lon_camera = lon_camera; + packet->alt_camera = alt_camera; + packet->lat_image = lat_image; + packet->lon_image = lon_image; + packet->alt_image = alt_image; + packet->hfov = hfov; + packet->vfov = vfov; + mav_array_memcpy(packet->q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FOV_STATUS, (const char *)packet, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_FOV_STATUS UNPACKING + + +/** + * @brief Get field time_boot_ms from camera_fov_status message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_camera_fov_status_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field lat_camera from camera_fov_status message + * + * @return [degE7] Latitude of camera (INT32_MAX if unknown). + */ +static inline int32_t mavlink_msg_camera_fov_status_get_lat_camera(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field lon_camera from camera_fov_status message + * + * @return [degE7] Longitude of camera (INT32_MAX if unknown). + */ +static inline int32_t mavlink_msg_camera_fov_status_get_lon_camera(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field alt_camera from camera_fov_status message + * + * @return [mm] Altitude (MSL) of camera (INT32_MAX if unknown). + */ +static inline int32_t mavlink_msg_camera_fov_status_get_alt_camera(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field lat_image from camera_fov_status message + * + * @return [degE7] Latitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + */ +static inline int32_t mavlink_msg_camera_fov_status_get_lat_image(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field lon_image from camera_fov_status message + * + * @return [degE7] Longitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + */ +static inline int32_t mavlink_msg_camera_fov_status_get_lon_image(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 20); +} + +/** + * @brief Get field alt_image from camera_fov_status message + * + * @return [mm] Altitude (MSL) of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + */ +static inline int32_t mavlink_msg_camera_fov_status_get_alt_image(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 24); +} + +/** + * @brief Get field q from camera_fov_status message + * + * @return Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + */ +static inline uint16_t mavlink_msg_camera_fov_status_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 28); +} + +/** + * @brief Get field hfov from camera_fov_status message + * + * @return [deg] Horizontal field of view (NaN if unknown). + */ +static inline float mavlink_msg_camera_fov_status_get_hfov(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field vfov from camera_fov_status message + * + * @return [deg] Vertical field of view (NaN if unknown). + */ +static inline float mavlink_msg_camera_fov_status_get_vfov(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 48); +} + +/** + * @brief Decode a camera_fov_status message into a struct + * + * @param msg The message to decode + * @param camera_fov_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_fov_status_decode(const mavlink_message_t* msg, mavlink_camera_fov_status_t* camera_fov_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + camera_fov_status->time_boot_ms = mavlink_msg_camera_fov_status_get_time_boot_ms(msg); + camera_fov_status->lat_camera = mavlink_msg_camera_fov_status_get_lat_camera(msg); + camera_fov_status->lon_camera = mavlink_msg_camera_fov_status_get_lon_camera(msg); + camera_fov_status->alt_camera = mavlink_msg_camera_fov_status_get_alt_camera(msg); + camera_fov_status->lat_image = mavlink_msg_camera_fov_status_get_lat_image(msg); + camera_fov_status->lon_image = mavlink_msg_camera_fov_status_get_lon_image(msg); + camera_fov_status->alt_image = mavlink_msg_camera_fov_status_get_alt_image(msg); + mavlink_msg_camera_fov_status_get_q(msg, camera_fov_status->q); + camera_fov_status->hfov = mavlink_msg_camera_fov_status_get_hfov(msg); + camera_fov_status->vfov = mavlink_msg_camera_fov_status_get_vfov(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN; + memset(camera_fov_status, 0, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); + memcpy(camera_fov_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_image_captured.h b/lib/main/MAVLink/common/mavlink_msg_camera_image_captured.h new file mode 100644 index 0000000000..01d772b62a --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_camera_image_captured.h @@ -0,0 +1,456 @@ +#pragma once +// MESSAGE CAMERA_IMAGE_CAPTURED PACKING + +#define MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED 263 + + +typedef struct __mavlink_camera_image_captured_t { + uint64_t time_utc; /*< [us] Timestamp (time since UNIX epoch) in UTC. 0 for unknown.*/ + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + int32_t lat; /*< [degE7] Latitude where image was taken*/ + int32_t lon; /*< [degE7] Longitude where capture was taken*/ + int32_t alt; /*< [mm] Altitude (MSL) where image was taken*/ + int32_t relative_alt; /*< [mm] Altitude above ground*/ + float q[4]; /*< Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0)*/ + int32_t image_index; /*< Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1)*/ + uint8_t camera_id; /*< Deprecated/unused. Component IDs are used to differentiate multiple cameras.*/ + int8_t capture_result; /*< Boolean indicating success (1) or failure (0) while capturing this image.*/ + char file_url[205]; /*< URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface.*/ +} mavlink_camera_image_captured_t; + +#define MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN 255 +#define MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN 255 +#define MAVLINK_MSG_ID_263_LEN 255 +#define MAVLINK_MSG_ID_263_MIN_LEN 255 + +#define MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC 133 +#define MAVLINK_MSG_ID_263_CRC 133 + +#define MAVLINK_MSG_CAMERA_IMAGE_CAPTURED_FIELD_Q_LEN 4 +#define MAVLINK_MSG_CAMERA_IMAGE_CAPTURED_FIELD_FILE_URL_LEN 205 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CAMERA_IMAGE_CAPTURED { \ + 263, \ + "CAMERA_IMAGE_CAPTURED", \ + 11, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_camera_image_captured_t, time_boot_ms) }, \ + { "time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_camera_image_captured_t, time_utc) }, \ + { "camera_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_camera_image_captured_t, camera_id) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_camera_image_captured_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_camera_image_captured_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_camera_image_captured_t, alt) }, \ + { "relative_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_camera_image_captured_t, relative_alt) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 28, offsetof(mavlink_camera_image_captured_t, q) }, \ + { "image_index", NULL, MAVLINK_TYPE_INT32_T, 0, 44, offsetof(mavlink_camera_image_captured_t, image_index) }, \ + { "capture_result", NULL, MAVLINK_TYPE_INT8_T, 0, 49, offsetof(mavlink_camera_image_captured_t, capture_result) }, \ + { "file_url", NULL, MAVLINK_TYPE_CHAR, 205, 50, offsetof(mavlink_camera_image_captured_t, file_url) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CAMERA_IMAGE_CAPTURED { \ + "CAMERA_IMAGE_CAPTURED", \ + 11, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_camera_image_captured_t, time_boot_ms) }, \ + { "time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_camera_image_captured_t, time_utc) }, \ + { "camera_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_camera_image_captured_t, camera_id) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_camera_image_captured_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_camera_image_captured_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_camera_image_captured_t, alt) }, \ + { "relative_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_camera_image_captured_t, relative_alt) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 28, offsetof(mavlink_camera_image_captured_t, q) }, \ + { "image_index", NULL, MAVLINK_TYPE_INT32_T, 0, 44, offsetof(mavlink_camera_image_captured_t, image_index) }, \ + { "capture_result", NULL, MAVLINK_TYPE_INT8_T, 0, 49, offsetof(mavlink_camera_image_captured_t, capture_result) }, \ + { "file_url", NULL, MAVLINK_TYPE_CHAR, 205, 50, offsetof(mavlink_camera_image_captured_t, file_url) }, \ + } \ +} +#endif + +/** + * @brief Pack a camera_image_captured message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param time_utc [us] Timestamp (time since UNIX epoch) in UTC. 0 for unknown. + * @param camera_id Deprecated/unused. Component IDs are used to differentiate multiple cameras. + * @param lat [degE7] Latitude where image was taken + * @param lon [degE7] Longitude where capture was taken + * @param alt [mm] Altitude (MSL) where image was taken + * @param relative_alt [mm] Altitude above ground + * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param image_index Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1) + * @param capture_result Boolean indicating success (1) or failure (0) while capturing this image. + * @param file_url URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_image_captured_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint64_t time_utc, uint8_t camera_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, const float *q, int32_t image_index, int8_t capture_result, const char *file_url) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_int32_t(buf, 20, alt); + _mav_put_int32_t(buf, 24, relative_alt); + _mav_put_int32_t(buf, 44, image_index); + _mav_put_uint8_t(buf, 48, camera_id); + _mav_put_int8_t(buf, 49, capture_result); + _mav_put_float_array(buf, 28, q, 4); + _mav_put_char_array(buf, 50, file_url, 205); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN); +#else + mavlink_camera_image_captured_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.image_index = image_index; + packet.camera_id = camera_id; + packet.capture_result = capture_result; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.file_url, file_url, sizeof(char)*205); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); +} + +/** + * @brief Pack a camera_image_captured message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param time_utc [us] Timestamp (time since UNIX epoch) in UTC. 0 for unknown. + * @param camera_id Deprecated/unused. Component IDs are used to differentiate multiple cameras. + * @param lat [degE7] Latitude where image was taken + * @param lon [degE7] Longitude where capture was taken + * @param alt [mm] Altitude (MSL) where image was taken + * @param relative_alt [mm] Altitude above ground + * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param image_index Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1) + * @param capture_result Boolean indicating success (1) or failure (0) while capturing this image. + * @param file_url URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_image_captured_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint64_t time_utc,uint8_t camera_id,int32_t lat,int32_t lon,int32_t alt,int32_t relative_alt,const float *q,int32_t image_index,int8_t capture_result,const char *file_url) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_int32_t(buf, 20, alt); + _mav_put_int32_t(buf, 24, relative_alt); + _mav_put_int32_t(buf, 44, image_index); + _mav_put_uint8_t(buf, 48, camera_id); + _mav_put_int8_t(buf, 49, capture_result); + _mav_put_float_array(buf, 28, q, 4); + _mav_put_char_array(buf, 50, file_url, 205); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN); +#else + mavlink_camera_image_captured_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.image_index = image_index; + packet.camera_id = camera_id; + packet.capture_result = capture_result; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.file_url, file_url, sizeof(char)*205); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); +} + +/** + * @brief Encode a camera_image_captured struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_image_captured C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_image_captured_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_image_captured_t* camera_image_captured) +{ + return mavlink_msg_camera_image_captured_pack(system_id, component_id, msg, camera_image_captured->time_boot_ms, camera_image_captured->time_utc, camera_image_captured->camera_id, camera_image_captured->lat, camera_image_captured->lon, camera_image_captured->alt, camera_image_captured->relative_alt, camera_image_captured->q, camera_image_captured->image_index, camera_image_captured->capture_result, camera_image_captured->file_url); +} + +/** + * @brief Encode a camera_image_captured struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_image_captured C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_image_captured_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_image_captured_t* camera_image_captured) +{ + return mavlink_msg_camera_image_captured_pack_chan(system_id, component_id, chan, msg, camera_image_captured->time_boot_ms, camera_image_captured->time_utc, camera_image_captured->camera_id, camera_image_captured->lat, camera_image_captured->lon, camera_image_captured->alt, camera_image_captured->relative_alt, camera_image_captured->q, camera_image_captured->image_index, camera_image_captured->capture_result, camera_image_captured->file_url); +} + +/** + * @brief Send a camera_image_captured message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param time_utc [us] Timestamp (time since UNIX epoch) in UTC. 0 for unknown. + * @param camera_id Deprecated/unused. Component IDs are used to differentiate multiple cameras. + * @param lat [degE7] Latitude where image was taken + * @param lon [degE7] Longitude where capture was taken + * @param alt [mm] Altitude (MSL) where image was taken + * @param relative_alt [mm] Altitude above ground + * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param image_index Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1) + * @param capture_result Boolean indicating success (1) or failure (0) while capturing this image. + * @param file_url URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_image_captured_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t time_utc, uint8_t camera_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, const float *q, int32_t image_index, int8_t capture_result, const char *file_url) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_int32_t(buf, 20, alt); + _mav_put_int32_t(buf, 24, relative_alt); + _mav_put_int32_t(buf, 44, image_index); + _mav_put_uint8_t(buf, 48, camera_id); + _mav_put_int8_t(buf, 49, capture_result); + _mav_put_float_array(buf, 28, q, 4); + _mav_put_char_array(buf, 50, file_url, 205); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, buf, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); +#else + mavlink_camera_image_captured_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.image_index = image_index; + packet.camera_id = camera_id; + packet.capture_result = capture_result; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.file_url, file_url, sizeof(char)*205); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); +#endif +} + +/** + * @brief Send a camera_image_captured message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_camera_image_captured_send_struct(mavlink_channel_t chan, const mavlink_camera_image_captured_t* camera_image_captured) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_camera_image_captured_send(chan, camera_image_captured->time_boot_ms, camera_image_captured->time_utc, camera_image_captured->camera_id, camera_image_captured->lat, camera_image_captured->lon, camera_image_captured->alt, camera_image_captured->relative_alt, camera_image_captured->q, camera_image_captured->image_index, camera_image_captured->capture_result, camera_image_captured->file_url); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, (const char *)camera_image_captured, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_image_captured_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t time_utc, uint8_t camera_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, const float *q, int32_t image_index, int8_t capture_result, const char *file_url) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_int32_t(buf, 20, alt); + _mav_put_int32_t(buf, 24, relative_alt); + _mav_put_int32_t(buf, 44, image_index); + _mav_put_uint8_t(buf, 48, camera_id); + _mav_put_int8_t(buf, 49, capture_result); + _mav_put_float_array(buf, 28, q, 4); + _mav_put_char_array(buf, 50, file_url, 205); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, buf, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); +#else + mavlink_camera_image_captured_t *packet = (mavlink_camera_image_captured_t *)msgbuf; + packet->time_utc = time_utc; + packet->time_boot_ms = time_boot_ms; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->relative_alt = relative_alt; + packet->image_index = image_index; + packet->camera_id = camera_id; + packet->capture_result = capture_result; + mav_array_memcpy(packet->q, q, sizeof(float)*4); + mav_array_memcpy(packet->file_url, file_url, sizeof(char)*205); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, (const char *)packet, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_IMAGE_CAPTURED UNPACKING + + +/** + * @brief Get field time_boot_ms from camera_image_captured message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_camera_image_captured_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field time_utc from camera_image_captured message + * + * @return [us] Timestamp (time since UNIX epoch) in UTC. 0 for unknown. + */ +static inline uint64_t mavlink_msg_camera_image_captured_get_time_utc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field camera_id from camera_image_captured message + * + * @return Deprecated/unused. Component IDs are used to differentiate multiple cameras. + */ +static inline uint8_t mavlink_msg_camera_image_captured_get_camera_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 48); +} + +/** + * @brief Get field lat from camera_image_captured message + * + * @return [degE7] Latitude where image was taken + */ +static inline int32_t mavlink_msg_camera_image_captured_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field lon from camera_image_captured message + * + * @return [degE7] Longitude where capture was taken + */ +static inline int32_t mavlink_msg_camera_image_captured_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field alt from camera_image_captured message + * + * @return [mm] Altitude (MSL) where image was taken + */ +static inline int32_t mavlink_msg_camera_image_captured_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 20); +} + +/** + * @brief Get field relative_alt from camera_image_captured message + * + * @return [mm] Altitude above ground + */ +static inline int32_t mavlink_msg_camera_image_captured_get_relative_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 24); +} + +/** + * @brief Get field q from camera_image_captured message + * + * @return Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + */ +static inline uint16_t mavlink_msg_camera_image_captured_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 28); +} + +/** + * @brief Get field image_index from camera_image_captured message + * + * @return Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1) + */ +static inline int32_t mavlink_msg_camera_image_captured_get_image_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 44); +} + +/** + * @brief Get field capture_result from camera_image_captured message + * + * @return Boolean indicating success (1) or failure (0) while capturing this image. + */ +static inline int8_t mavlink_msg_camera_image_captured_get_capture_result(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int8_t(msg, 49); +} + +/** + * @brief Get field file_url from camera_image_captured message + * + * @return URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. + */ +static inline uint16_t mavlink_msg_camera_image_captured_get_file_url(const mavlink_message_t* msg, char *file_url) +{ + return _MAV_RETURN_char_array(msg, file_url, 205, 50); +} + +/** + * @brief Decode a camera_image_captured message into a struct + * + * @param msg The message to decode + * @param camera_image_captured C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_image_captured_decode(const mavlink_message_t* msg, mavlink_camera_image_captured_t* camera_image_captured) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + camera_image_captured->time_utc = mavlink_msg_camera_image_captured_get_time_utc(msg); + camera_image_captured->time_boot_ms = mavlink_msg_camera_image_captured_get_time_boot_ms(msg); + camera_image_captured->lat = mavlink_msg_camera_image_captured_get_lat(msg); + camera_image_captured->lon = mavlink_msg_camera_image_captured_get_lon(msg); + camera_image_captured->alt = mavlink_msg_camera_image_captured_get_alt(msg); + camera_image_captured->relative_alt = mavlink_msg_camera_image_captured_get_relative_alt(msg); + mavlink_msg_camera_image_captured_get_q(msg, camera_image_captured->q); + camera_image_captured->image_index = mavlink_msg_camera_image_captured_get_image_index(msg); + camera_image_captured->camera_id = mavlink_msg_camera_image_captured_get_camera_id(msg); + camera_image_captured->capture_result = mavlink_msg_camera_image_captured_get_capture_result(msg); + mavlink_msg_camera_image_captured_get_file_url(msg, camera_image_captured->file_url); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN; + memset(camera_image_captured, 0, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN); + memcpy(camera_image_captured, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_information.h b/lib/main/MAVLink/common/mavlink_msg_camera_information.h new file mode 100644 index 0000000000..0ae20c4b3e --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_camera_information.h @@ -0,0 +1,507 @@ +#pragma once +// MESSAGE CAMERA_INFORMATION PACKING + +#define MAVLINK_MSG_ID_CAMERA_INFORMATION 259 + + +typedef struct __mavlink_camera_information_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + uint32_t firmware_version; /*< Version of the camera firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff)*/ + float focal_length; /*< [mm] Focal length*/ + float sensor_size_h; /*< [mm] Image sensor size horizontal*/ + float sensor_size_v; /*< [mm] Image sensor size vertical*/ + uint32_t flags; /*< Bitmap of camera capability flags.*/ + uint16_t resolution_h; /*< [pix] Horizontal image resolution*/ + uint16_t resolution_v; /*< [pix] Vertical image resolution*/ + uint16_t cam_definition_version; /*< Camera definition version (iteration)*/ + uint8_t vendor_name[32]; /*< Name of the camera vendor*/ + uint8_t model_name[32]; /*< Name of the camera model*/ + uint8_t lens_id; /*< Reserved for a lens ID*/ + char cam_definition_uri[140]; /*< Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol).*/ +} mavlink_camera_information_t; + +#define MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN 235 +#define MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN 235 +#define MAVLINK_MSG_ID_259_LEN 235 +#define MAVLINK_MSG_ID_259_MIN_LEN 235 + +#define MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC 92 +#define MAVLINK_MSG_ID_259_CRC 92 + +#define MAVLINK_MSG_CAMERA_INFORMATION_FIELD_VENDOR_NAME_LEN 32 +#define MAVLINK_MSG_CAMERA_INFORMATION_FIELD_MODEL_NAME_LEN 32 +#define MAVLINK_MSG_CAMERA_INFORMATION_FIELD_CAM_DEFINITION_URI_LEN 140 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CAMERA_INFORMATION { \ + 259, \ + "CAMERA_INFORMATION", \ + 13, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_information_t, time_boot_ms) }, \ + { "vendor_name", NULL, MAVLINK_TYPE_UINT8_T, 32, 30, offsetof(mavlink_camera_information_t, vendor_name) }, \ + { "model_name", NULL, MAVLINK_TYPE_UINT8_T, 32, 62, offsetof(mavlink_camera_information_t, model_name) }, \ + { "firmware_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_camera_information_t, firmware_version) }, \ + { "focal_length", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_camera_information_t, focal_length) }, \ + { "sensor_size_h", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_information_t, sensor_size_h) }, \ + { "sensor_size_v", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_information_t, sensor_size_v) }, \ + { "resolution_h", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_camera_information_t, resolution_h) }, \ + { "resolution_v", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_camera_information_t, resolution_v) }, \ + { "lens_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 94, offsetof(mavlink_camera_information_t, lens_id) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_camera_information_t, flags) }, \ + { "cam_definition_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_camera_information_t, cam_definition_version) }, \ + { "cam_definition_uri", NULL, MAVLINK_TYPE_CHAR, 140, 95, offsetof(mavlink_camera_information_t, cam_definition_uri) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CAMERA_INFORMATION { \ + "CAMERA_INFORMATION", \ + 13, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_information_t, time_boot_ms) }, \ + { "vendor_name", NULL, MAVLINK_TYPE_UINT8_T, 32, 30, offsetof(mavlink_camera_information_t, vendor_name) }, \ + { "model_name", NULL, MAVLINK_TYPE_UINT8_T, 32, 62, offsetof(mavlink_camera_information_t, model_name) }, \ + { "firmware_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_camera_information_t, firmware_version) }, \ + { "focal_length", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_camera_information_t, focal_length) }, \ + { "sensor_size_h", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_information_t, sensor_size_h) }, \ + { "sensor_size_v", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_information_t, sensor_size_v) }, \ + { "resolution_h", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_camera_information_t, resolution_h) }, \ + { "resolution_v", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_camera_information_t, resolution_v) }, \ + { "lens_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 94, offsetof(mavlink_camera_information_t, lens_id) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_camera_information_t, flags) }, \ + { "cam_definition_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_camera_information_t, cam_definition_version) }, \ + { "cam_definition_uri", NULL, MAVLINK_TYPE_CHAR, 140, 95, offsetof(mavlink_camera_information_t, cam_definition_uri) }, \ + } \ +} +#endif + +/** + * @brief Pack a camera_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param vendor_name Name of the camera vendor + * @param model_name Name of the camera model + * @param firmware_version Version of the camera firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff) + * @param focal_length [mm] Focal length + * @param sensor_size_h [mm] Image sensor size horizontal + * @param sensor_size_v [mm] Image sensor size vertical + * @param resolution_h [pix] Horizontal image resolution + * @param resolution_v [pix] Vertical image resolution + * @param lens_id Reserved for a lens ID + * @param flags Bitmap of camera capability flags. + * @param cam_definition_version Camera definition version (iteration) + * @param cam_definition_uri Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, const uint8_t *vendor_name, const uint8_t *model_name, uint32_t firmware_version, float focal_length, float sensor_size_h, float sensor_size_v, uint16_t resolution_h, uint16_t resolution_v, uint8_t lens_id, uint32_t flags, uint16_t cam_definition_version, const char *cam_definition_uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, firmware_version); + _mav_put_float(buf, 8, focal_length); + _mav_put_float(buf, 12, sensor_size_h); + _mav_put_float(buf, 16, sensor_size_v); + _mav_put_uint32_t(buf, 20, flags); + _mav_put_uint16_t(buf, 24, resolution_h); + _mav_put_uint16_t(buf, 26, resolution_v); + _mav_put_uint16_t(buf, 28, cam_definition_version); + _mav_put_uint8_t(buf, 94, lens_id); + _mav_put_uint8_t_array(buf, 30, vendor_name, 32); + _mav_put_uint8_t_array(buf, 62, model_name, 32); + _mav_put_char_array(buf, 95, cam_definition_uri, 140); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN); +#else + mavlink_camera_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.firmware_version = firmware_version; + packet.focal_length = focal_length; + packet.sensor_size_h = sensor_size_h; + packet.sensor_size_v = sensor_size_v; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.cam_definition_version = cam_definition_version; + packet.lens_id = lens_id; + mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet.model_name, model_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet.cam_definition_uri, cam_definition_uri, sizeof(char)*140); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_INFORMATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); +} + +/** + * @brief Pack a camera_information message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param vendor_name Name of the camera vendor + * @param model_name Name of the camera model + * @param firmware_version Version of the camera firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff) + * @param focal_length [mm] Focal length + * @param sensor_size_h [mm] Image sensor size horizontal + * @param sensor_size_v [mm] Image sensor size vertical + * @param resolution_h [pix] Horizontal image resolution + * @param resolution_v [pix] Vertical image resolution + * @param lens_id Reserved for a lens ID + * @param flags Bitmap of camera capability flags. + * @param cam_definition_version Camera definition version (iteration) + * @param cam_definition_uri Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,const uint8_t *vendor_name,const uint8_t *model_name,uint32_t firmware_version,float focal_length,float sensor_size_h,float sensor_size_v,uint16_t resolution_h,uint16_t resolution_v,uint8_t lens_id,uint32_t flags,uint16_t cam_definition_version,const char *cam_definition_uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, firmware_version); + _mav_put_float(buf, 8, focal_length); + _mav_put_float(buf, 12, sensor_size_h); + _mav_put_float(buf, 16, sensor_size_v); + _mav_put_uint32_t(buf, 20, flags); + _mav_put_uint16_t(buf, 24, resolution_h); + _mav_put_uint16_t(buf, 26, resolution_v); + _mav_put_uint16_t(buf, 28, cam_definition_version); + _mav_put_uint8_t(buf, 94, lens_id); + _mav_put_uint8_t_array(buf, 30, vendor_name, 32); + _mav_put_uint8_t_array(buf, 62, model_name, 32); + _mav_put_char_array(buf, 95, cam_definition_uri, 140); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN); +#else + mavlink_camera_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.firmware_version = firmware_version; + packet.focal_length = focal_length; + packet.sensor_size_h = sensor_size_h; + packet.sensor_size_v = sensor_size_v; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.cam_definition_version = cam_definition_version; + packet.lens_id = lens_id; + mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet.model_name, model_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet.cam_definition_uri, cam_definition_uri, sizeof(char)*140); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_INFORMATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); +} + +/** + * @brief Encode a camera_information struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_information_t* camera_information) +{ + return mavlink_msg_camera_information_pack(system_id, component_id, msg, camera_information->time_boot_ms, camera_information->vendor_name, camera_information->model_name, camera_information->firmware_version, camera_information->focal_length, camera_information->sensor_size_h, camera_information->sensor_size_v, camera_information->resolution_h, camera_information->resolution_v, camera_information->lens_id, camera_information->flags, camera_information->cam_definition_version, camera_information->cam_definition_uri); +} + +/** + * @brief Encode a camera_information struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_information_t* camera_information) +{ + return mavlink_msg_camera_information_pack_chan(system_id, component_id, chan, msg, camera_information->time_boot_ms, camera_information->vendor_name, camera_information->model_name, camera_information->firmware_version, camera_information->focal_length, camera_information->sensor_size_h, camera_information->sensor_size_v, camera_information->resolution_h, camera_information->resolution_v, camera_information->lens_id, camera_information->flags, camera_information->cam_definition_version, camera_information->cam_definition_uri); +} + +/** + * @brief Send a camera_information message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param vendor_name Name of the camera vendor + * @param model_name Name of the camera model + * @param firmware_version Version of the camera firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff) + * @param focal_length [mm] Focal length + * @param sensor_size_h [mm] Image sensor size horizontal + * @param sensor_size_v [mm] Image sensor size vertical + * @param resolution_h [pix] Horizontal image resolution + * @param resolution_v [pix] Vertical image resolution + * @param lens_id Reserved for a lens ID + * @param flags Bitmap of camera capability flags. + * @param cam_definition_version Camera definition version (iteration) + * @param cam_definition_uri Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, const uint8_t *vendor_name, const uint8_t *model_name, uint32_t firmware_version, float focal_length, float sensor_size_h, float sensor_size_v, uint16_t resolution_h, uint16_t resolution_v, uint8_t lens_id, uint32_t flags, uint16_t cam_definition_version, const char *cam_definition_uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, firmware_version); + _mav_put_float(buf, 8, focal_length); + _mav_put_float(buf, 12, sensor_size_h); + _mav_put_float(buf, 16, sensor_size_v); + _mav_put_uint32_t(buf, 20, flags); + _mav_put_uint16_t(buf, 24, resolution_h); + _mav_put_uint16_t(buf, 26, resolution_v); + _mav_put_uint16_t(buf, 28, cam_definition_version); + _mav_put_uint8_t(buf, 94, lens_id); + _mav_put_uint8_t_array(buf, 30, vendor_name, 32); + _mav_put_uint8_t_array(buf, 62, model_name, 32); + _mav_put_char_array(buf, 95, cam_definition_uri, 140); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_INFORMATION, buf, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); +#else + mavlink_camera_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.firmware_version = firmware_version; + packet.focal_length = focal_length; + packet.sensor_size_h = sensor_size_h; + packet.sensor_size_v = sensor_size_v; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.cam_definition_version = cam_definition_version; + packet.lens_id = lens_id; + mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet.model_name, model_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet.cam_definition_uri, cam_definition_uri, sizeof(char)*140); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); +#endif +} + +/** + * @brief Send a camera_information message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_camera_information_send_struct(mavlink_channel_t chan, const mavlink_camera_information_t* camera_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_camera_information_send(chan, camera_information->time_boot_ms, camera_information->vendor_name, camera_information->model_name, camera_information->firmware_version, camera_information->focal_length, camera_information->sensor_size_h, camera_information->sensor_size_v, camera_information->resolution_h, camera_information->resolution_v, camera_information->lens_id, camera_information->flags, camera_information->cam_definition_version, camera_information->cam_definition_uri); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_INFORMATION, (const char *)camera_information, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const uint8_t *vendor_name, const uint8_t *model_name, uint32_t firmware_version, float focal_length, float sensor_size_h, float sensor_size_v, uint16_t resolution_h, uint16_t resolution_v, uint8_t lens_id, uint32_t flags, uint16_t cam_definition_version, const char *cam_definition_uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, firmware_version); + _mav_put_float(buf, 8, focal_length); + _mav_put_float(buf, 12, sensor_size_h); + _mav_put_float(buf, 16, sensor_size_v); + _mav_put_uint32_t(buf, 20, flags); + _mav_put_uint16_t(buf, 24, resolution_h); + _mav_put_uint16_t(buf, 26, resolution_v); + _mav_put_uint16_t(buf, 28, cam_definition_version); + _mav_put_uint8_t(buf, 94, lens_id); + _mav_put_uint8_t_array(buf, 30, vendor_name, 32); + _mav_put_uint8_t_array(buf, 62, model_name, 32); + _mav_put_char_array(buf, 95, cam_definition_uri, 140); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_INFORMATION, buf, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); +#else + mavlink_camera_information_t *packet = (mavlink_camera_information_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->firmware_version = firmware_version; + packet->focal_length = focal_length; + packet->sensor_size_h = sensor_size_h; + packet->sensor_size_v = sensor_size_v; + packet->flags = flags; + packet->resolution_h = resolution_h; + packet->resolution_v = resolution_v; + packet->cam_definition_version = cam_definition_version; + packet->lens_id = lens_id; + mav_array_memcpy(packet->vendor_name, vendor_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet->model_name, model_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet->cam_definition_uri, cam_definition_uri, sizeof(char)*140); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_INFORMATION UNPACKING + + +/** + * @brief Get field time_boot_ms from camera_information message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_camera_information_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field vendor_name from camera_information message + * + * @return Name of the camera vendor + */ +static inline uint16_t mavlink_msg_camera_information_get_vendor_name(const mavlink_message_t* msg, uint8_t *vendor_name) +{ + return _MAV_RETURN_uint8_t_array(msg, vendor_name, 32, 30); +} + +/** + * @brief Get field model_name from camera_information message + * + * @return Name of the camera model + */ +static inline uint16_t mavlink_msg_camera_information_get_model_name(const mavlink_message_t* msg, uint8_t *model_name) +{ + return _MAV_RETURN_uint8_t_array(msg, model_name, 32, 62); +} + +/** + * @brief Get field firmware_version from camera_information message + * + * @return Version of the camera firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff) + */ +static inline uint32_t mavlink_msg_camera_information_get_firmware_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field focal_length from camera_information message + * + * @return [mm] Focal length + */ +static inline float mavlink_msg_camera_information_get_focal_length(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field sensor_size_h from camera_information message + * + * @return [mm] Image sensor size horizontal + */ +static inline float mavlink_msg_camera_information_get_sensor_size_h(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field sensor_size_v from camera_information message + * + * @return [mm] Image sensor size vertical + */ +static inline float mavlink_msg_camera_information_get_sensor_size_v(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field resolution_h from camera_information message + * + * @return [pix] Horizontal image resolution + */ +static inline uint16_t mavlink_msg_camera_information_get_resolution_h(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field resolution_v from camera_information message + * + * @return [pix] Vertical image resolution + */ +static inline uint16_t mavlink_msg_camera_information_get_resolution_v(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 26); +} + +/** + * @brief Get field lens_id from camera_information message + * + * @return Reserved for a lens ID + */ +static inline uint8_t mavlink_msg_camera_information_get_lens_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 94); +} + +/** + * @brief Get field flags from camera_information message + * + * @return Bitmap of camera capability flags. + */ +static inline uint32_t mavlink_msg_camera_information_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 20); +} + +/** + * @brief Get field cam_definition_version from camera_information message + * + * @return Camera definition version (iteration) + */ +static inline uint16_t mavlink_msg_camera_information_get_cam_definition_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field cam_definition_uri from camera_information message + * + * @return Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). + */ +static inline uint16_t mavlink_msg_camera_information_get_cam_definition_uri(const mavlink_message_t* msg, char *cam_definition_uri) +{ + return _MAV_RETURN_char_array(msg, cam_definition_uri, 140, 95); +} + +/** + * @brief Decode a camera_information message into a struct + * + * @param msg The message to decode + * @param camera_information C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_information_decode(const mavlink_message_t* msg, mavlink_camera_information_t* camera_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + camera_information->time_boot_ms = mavlink_msg_camera_information_get_time_boot_ms(msg); + camera_information->firmware_version = mavlink_msg_camera_information_get_firmware_version(msg); + camera_information->focal_length = mavlink_msg_camera_information_get_focal_length(msg); + camera_information->sensor_size_h = mavlink_msg_camera_information_get_sensor_size_h(msg); + camera_information->sensor_size_v = mavlink_msg_camera_information_get_sensor_size_v(msg); + camera_information->flags = mavlink_msg_camera_information_get_flags(msg); + camera_information->resolution_h = mavlink_msg_camera_information_get_resolution_h(msg); + camera_information->resolution_v = mavlink_msg_camera_information_get_resolution_v(msg); + camera_information->cam_definition_version = mavlink_msg_camera_information_get_cam_definition_version(msg); + mavlink_msg_camera_information_get_vendor_name(msg, camera_information->vendor_name); + mavlink_msg_camera_information_get_model_name(msg, camera_information->model_name); + camera_information->lens_id = mavlink_msg_camera_information_get_lens_id(msg); + mavlink_msg_camera_information_get_cam_definition_uri(msg, camera_information->cam_definition_uri); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN; + memset(camera_information, 0, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN); + memcpy(camera_information, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_settings.h b/lib/main/MAVLink/common/mavlink_msg_camera_settings.h new file mode 100644 index 0000000000..8f49780fad --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_camera_settings.h @@ -0,0 +1,288 @@ +#pragma once +// MESSAGE CAMERA_SETTINGS PACKING + +#define MAVLINK_MSG_ID_CAMERA_SETTINGS 260 + +MAVPACKED( +typedef struct __mavlink_camera_settings_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + uint8_t mode_id; /*< Camera mode*/ + float zoomLevel; /*< Current zoom level (0.0 to 100.0, NaN if not known)*/ + float focusLevel; /*< Current focus level (0.0 to 100.0, NaN if not known)*/ +}) mavlink_camera_settings_t; + +#define MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN 13 +#define MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN 5 +#define MAVLINK_MSG_ID_260_LEN 13 +#define MAVLINK_MSG_ID_260_MIN_LEN 5 + +#define MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC 146 +#define MAVLINK_MSG_ID_260_CRC 146 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CAMERA_SETTINGS { \ + 260, \ + "CAMERA_SETTINGS", \ + 4, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_settings_t, time_boot_ms) }, \ + { "mode_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_camera_settings_t, mode_id) }, \ + { "zoomLevel", NULL, MAVLINK_TYPE_FLOAT, 0, 5, offsetof(mavlink_camera_settings_t, zoomLevel) }, \ + { "focusLevel", NULL, MAVLINK_TYPE_FLOAT, 0, 9, offsetof(mavlink_camera_settings_t, focusLevel) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CAMERA_SETTINGS { \ + "CAMERA_SETTINGS", \ + 4, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_settings_t, time_boot_ms) }, \ + { "mode_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_camera_settings_t, mode_id) }, \ + { "zoomLevel", NULL, MAVLINK_TYPE_FLOAT, 0, 5, offsetof(mavlink_camera_settings_t, zoomLevel) }, \ + { "focusLevel", NULL, MAVLINK_TYPE_FLOAT, 0, 9, offsetof(mavlink_camera_settings_t, focusLevel) }, \ + } \ +} +#endif + +/** + * @brief Pack a camera_settings message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param mode_id Camera mode + * @param zoomLevel Current zoom level (0.0 to 100.0, NaN if not known) + * @param focusLevel Current focus level (0.0 to 100.0, NaN if not known) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_settings_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t mode_id, float zoomLevel, float focusLevel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint8_t(buf, 4, mode_id); + _mav_put_float(buf, 5, zoomLevel); + _mav_put_float(buf, 9, focusLevel); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); +#else + mavlink_camera_settings_t packet; + packet.time_boot_ms = time_boot_ms; + packet.mode_id = mode_id; + packet.zoomLevel = zoomLevel; + packet.focusLevel = focusLevel; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_SETTINGS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); +} + +/** + * @brief Pack a camera_settings message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param mode_id Camera mode + * @param zoomLevel Current zoom level (0.0 to 100.0, NaN if not known) + * @param focusLevel Current focus level (0.0 to 100.0, NaN if not known) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_settings_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t mode_id,float zoomLevel,float focusLevel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint8_t(buf, 4, mode_id); + _mav_put_float(buf, 5, zoomLevel); + _mav_put_float(buf, 9, focusLevel); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); +#else + mavlink_camera_settings_t packet; + packet.time_boot_ms = time_boot_ms; + packet.mode_id = mode_id; + packet.zoomLevel = zoomLevel; + packet.focusLevel = focusLevel; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_SETTINGS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); +} + +/** + * @brief Encode a camera_settings struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_settings C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_settings_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_settings_t* camera_settings) +{ + return mavlink_msg_camera_settings_pack(system_id, component_id, msg, camera_settings->time_boot_ms, camera_settings->mode_id, camera_settings->zoomLevel, camera_settings->focusLevel); +} + +/** + * @brief Encode a camera_settings struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_settings C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_settings_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_settings_t* camera_settings) +{ + return mavlink_msg_camera_settings_pack_chan(system_id, component_id, chan, msg, camera_settings->time_boot_ms, camera_settings->mode_id, camera_settings->zoomLevel, camera_settings->focusLevel); +} + +/** + * @brief Send a camera_settings message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param mode_id Camera mode + * @param zoomLevel Current zoom level (0.0 to 100.0, NaN if not known) + * @param focusLevel Current focus level (0.0 to 100.0, NaN if not known) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_settings_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t mode_id, float zoomLevel, float focusLevel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint8_t(buf, 4, mode_id); + _mav_put_float(buf, 5, zoomLevel); + _mav_put_float(buf, 9, focusLevel); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_SETTINGS, buf, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); +#else + mavlink_camera_settings_t packet; + packet.time_boot_ms = time_boot_ms; + packet.mode_id = mode_id; + packet.zoomLevel = zoomLevel; + packet.focusLevel = focusLevel; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_SETTINGS, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); +#endif +} + +/** + * @brief Send a camera_settings message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_camera_settings_send_struct(mavlink_channel_t chan, const mavlink_camera_settings_t* camera_settings) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_camera_settings_send(chan, camera_settings->time_boot_ms, camera_settings->mode_id, camera_settings->zoomLevel, camera_settings->focusLevel); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_SETTINGS, (const char *)camera_settings, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_settings_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t mode_id, float zoomLevel, float focusLevel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint8_t(buf, 4, mode_id); + _mav_put_float(buf, 5, zoomLevel); + _mav_put_float(buf, 9, focusLevel); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_SETTINGS, buf, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); +#else + mavlink_camera_settings_t *packet = (mavlink_camera_settings_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->mode_id = mode_id; + packet->zoomLevel = zoomLevel; + packet->focusLevel = focusLevel; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_SETTINGS, (const char *)packet, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_SETTINGS UNPACKING + + +/** + * @brief Get field time_boot_ms from camera_settings message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_camera_settings_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field mode_id from camera_settings message + * + * @return Camera mode + */ +static inline uint8_t mavlink_msg_camera_settings_get_mode_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field zoomLevel from camera_settings message + * + * @return Current zoom level (0.0 to 100.0, NaN if not known) + */ +static inline float mavlink_msg_camera_settings_get_zoomLevel(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 5); +} + +/** + * @brief Get field focusLevel from camera_settings message + * + * @return Current focus level (0.0 to 100.0, NaN if not known) + */ +static inline float mavlink_msg_camera_settings_get_focusLevel(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 9); +} + +/** + * @brief Decode a camera_settings message into a struct + * + * @param msg The message to decode + * @param camera_settings C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_settings_decode(const mavlink_message_t* msg, mavlink_camera_settings_t* camera_settings) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + camera_settings->time_boot_ms = mavlink_msg_camera_settings_get_time_boot_ms(msg); + camera_settings->mode_id = mavlink_msg_camera_settings_get_mode_id(msg); + camera_settings->zoomLevel = mavlink_msg_camera_settings_get_zoomLevel(msg); + camera_settings->focusLevel = mavlink_msg_camera_settings_get_focusLevel(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN; + memset(camera_settings, 0, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); + memcpy(camera_settings, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_tracking_geo_status.h b/lib/main/MAVLink/common/mavlink_msg_camera_tracking_geo_status.h new file mode 100644 index 0000000000..f4e22b98f9 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_camera_tracking_geo_status.h @@ -0,0 +1,513 @@ +#pragma once +// MESSAGE CAMERA_TRACKING_GEO_STATUS PACKING + +#define MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS 276 + + +typedef struct __mavlink_camera_tracking_geo_status_t { + int32_t lat; /*< [degE7] Latitude of tracked object*/ + int32_t lon; /*< [degE7] Longitude of tracked object*/ + float alt; /*< [m] Altitude of tracked object(AMSL, WGS84)*/ + float h_acc; /*< [m] Horizontal accuracy. NAN if unknown*/ + float v_acc; /*< [m] Vertical accuracy. NAN if unknown*/ + float vel_n; /*< [m/s] North velocity of tracked object. NAN if unknown*/ + float vel_e; /*< [m/s] East velocity of tracked object. NAN if unknown*/ + float vel_d; /*< [m/s] Down velocity of tracked object. NAN if unknown*/ + float vel_acc; /*< [m/s] Velocity accuracy. NAN if unknown*/ + float dist; /*< [m] Distance between camera and tracked object. NAN if unknown*/ + float hdg; /*< [rad] Heading in radians, in NED. NAN if unknown*/ + float hdg_acc; /*< [rad] Accuracy of heading, in NED. NAN if unknown*/ + uint8_t tracking_status; /*< Current tracking status*/ +} mavlink_camera_tracking_geo_status_t; + +#define MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN 49 +#define MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN 49 +#define MAVLINK_MSG_ID_276_LEN 49 +#define MAVLINK_MSG_ID_276_MIN_LEN 49 + +#define MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC 18 +#define MAVLINK_MSG_ID_276_CRC 18 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_GEO_STATUS { \ + 276, \ + "CAMERA_TRACKING_GEO_STATUS", \ + 13, \ + { { "tracking_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_camera_tracking_geo_status_t, tracking_status) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_camera_tracking_geo_status_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_camera_tracking_geo_status_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_camera_tracking_geo_status_t, alt) }, \ + { "h_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_tracking_geo_status_t, h_acc) }, \ + { "v_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_tracking_geo_status_t, v_acc) }, \ + { "vel_n", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_camera_tracking_geo_status_t, vel_n) }, \ + { "vel_e", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_camera_tracking_geo_status_t, vel_e) }, \ + { "vel_d", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_camera_tracking_geo_status_t, vel_d) }, \ + { "vel_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_camera_tracking_geo_status_t, vel_acc) }, \ + { "dist", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_camera_tracking_geo_status_t, dist) }, \ + { "hdg", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_camera_tracking_geo_status_t, hdg) }, \ + { "hdg_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_camera_tracking_geo_status_t, hdg_acc) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_GEO_STATUS { \ + "CAMERA_TRACKING_GEO_STATUS", \ + 13, \ + { { "tracking_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_camera_tracking_geo_status_t, tracking_status) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_camera_tracking_geo_status_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_camera_tracking_geo_status_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_camera_tracking_geo_status_t, alt) }, \ + { "h_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_tracking_geo_status_t, h_acc) }, \ + { "v_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_tracking_geo_status_t, v_acc) }, \ + { "vel_n", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_camera_tracking_geo_status_t, vel_n) }, \ + { "vel_e", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_camera_tracking_geo_status_t, vel_e) }, \ + { "vel_d", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_camera_tracking_geo_status_t, vel_d) }, \ + { "vel_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_camera_tracking_geo_status_t, vel_acc) }, \ + { "dist", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_camera_tracking_geo_status_t, dist) }, \ + { "hdg", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_camera_tracking_geo_status_t, hdg) }, \ + { "hdg_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_camera_tracking_geo_status_t, hdg_acc) }, \ + } \ +} +#endif + +/** + * @brief Pack a camera_tracking_geo_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param tracking_status Current tracking status + * @param lat [degE7] Latitude of tracked object + * @param lon [degE7] Longitude of tracked object + * @param alt [m] Altitude of tracked object(AMSL, WGS84) + * @param h_acc [m] Horizontal accuracy. NAN if unknown + * @param v_acc [m] Vertical accuracy. NAN if unknown + * @param vel_n [m/s] North velocity of tracked object. NAN if unknown + * @param vel_e [m/s] East velocity of tracked object. NAN if unknown + * @param vel_d [m/s] Down velocity of tracked object. NAN if unknown + * @param vel_acc [m/s] Velocity accuracy. NAN if unknown + * @param dist [m] Distance between camera and tracked object. NAN if unknown + * @param hdg [rad] Heading in radians, in NED. NAN if unknown + * @param hdg_acc [rad] Accuracy of heading, in NED. NAN if unknown + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_tracking_geo_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t tracking_status, int32_t lat, int32_t lon, float alt, float h_acc, float v_acc, float vel_n, float vel_e, float vel_d, float vel_acc, float dist, float hdg, float hdg_acc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, h_acc); + _mav_put_float(buf, 16, v_acc); + _mav_put_float(buf, 20, vel_n); + _mav_put_float(buf, 24, vel_e); + _mav_put_float(buf, 28, vel_d); + _mav_put_float(buf, 32, vel_acc); + _mav_put_float(buf, 36, dist); + _mav_put_float(buf, 40, hdg); + _mav_put_float(buf, 44, hdg_acc); + _mav_put_uint8_t(buf, 48, tracking_status); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); +#else + mavlink_camera_tracking_geo_status_t packet; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_n = vel_n; + packet.vel_e = vel_e; + packet.vel_d = vel_d; + packet.vel_acc = vel_acc; + packet.dist = dist; + packet.hdg = hdg; + packet.hdg_acc = hdg_acc; + packet.tracking_status = tracking_status; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); +} + +/** + * @brief Pack a camera_tracking_geo_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param tracking_status Current tracking status + * @param lat [degE7] Latitude of tracked object + * @param lon [degE7] Longitude of tracked object + * @param alt [m] Altitude of tracked object(AMSL, WGS84) + * @param h_acc [m] Horizontal accuracy. NAN if unknown + * @param v_acc [m] Vertical accuracy. NAN if unknown + * @param vel_n [m/s] North velocity of tracked object. NAN if unknown + * @param vel_e [m/s] East velocity of tracked object. NAN if unknown + * @param vel_d [m/s] Down velocity of tracked object. NAN if unknown + * @param vel_acc [m/s] Velocity accuracy. NAN if unknown + * @param dist [m] Distance between camera and tracked object. NAN if unknown + * @param hdg [rad] Heading in radians, in NED. NAN if unknown + * @param hdg_acc [rad] Accuracy of heading, in NED. NAN if unknown + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_tracking_geo_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t tracking_status,int32_t lat,int32_t lon,float alt,float h_acc,float v_acc,float vel_n,float vel_e,float vel_d,float vel_acc,float dist,float hdg,float hdg_acc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, h_acc); + _mav_put_float(buf, 16, v_acc); + _mav_put_float(buf, 20, vel_n); + _mav_put_float(buf, 24, vel_e); + _mav_put_float(buf, 28, vel_d); + _mav_put_float(buf, 32, vel_acc); + _mav_put_float(buf, 36, dist); + _mav_put_float(buf, 40, hdg); + _mav_put_float(buf, 44, hdg_acc); + _mav_put_uint8_t(buf, 48, tracking_status); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); +#else + mavlink_camera_tracking_geo_status_t packet; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_n = vel_n; + packet.vel_e = vel_e; + packet.vel_d = vel_d; + packet.vel_acc = vel_acc; + packet.dist = dist; + packet.hdg = hdg; + packet.hdg_acc = hdg_acc; + packet.tracking_status = tracking_status; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); +} + +/** + * @brief Encode a camera_tracking_geo_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_tracking_geo_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_tracking_geo_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_tracking_geo_status_t* camera_tracking_geo_status) +{ + return mavlink_msg_camera_tracking_geo_status_pack(system_id, component_id, msg, camera_tracking_geo_status->tracking_status, camera_tracking_geo_status->lat, camera_tracking_geo_status->lon, camera_tracking_geo_status->alt, camera_tracking_geo_status->h_acc, camera_tracking_geo_status->v_acc, camera_tracking_geo_status->vel_n, camera_tracking_geo_status->vel_e, camera_tracking_geo_status->vel_d, camera_tracking_geo_status->vel_acc, camera_tracking_geo_status->dist, camera_tracking_geo_status->hdg, camera_tracking_geo_status->hdg_acc); +} + +/** + * @brief Encode a camera_tracking_geo_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_tracking_geo_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_tracking_geo_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_tracking_geo_status_t* camera_tracking_geo_status) +{ + return mavlink_msg_camera_tracking_geo_status_pack_chan(system_id, component_id, chan, msg, camera_tracking_geo_status->tracking_status, camera_tracking_geo_status->lat, camera_tracking_geo_status->lon, camera_tracking_geo_status->alt, camera_tracking_geo_status->h_acc, camera_tracking_geo_status->v_acc, camera_tracking_geo_status->vel_n, camera_tracking_geo_status->vel_e, camera_tracking_geo_status->vel_d, camera_tracking_geo_status->vel_acc, camera_tracking_geo_status->dist, camera_tracking_geo_status->hdg, camera_tracking_geo_status->hdg_acc); +} + +/** + * @brief Send a camera_tracking_geo_status message + * @param chan MAVLink channel to send the message + * + * @param tracking_status Current tracking status + * @param lat [degE7] Latitude of tracked object + * @param lon [degE7] Longitude of tracked object + * @param alt [m] Altitude of tracked object(AMSL, WGS84) + * @param h_acc [m] Horizontal accuracy. NAN if unknown + * @param v_acc [m] Vertical accuracy. NAN if unknown + * @param vel_n [m/s] North velocity of tracked object. NAN if unknown + * @param vel_e [m/s] East velocity of tracked object. NAN if unknown + * @param vel_d [m/s] Down velocity of tracked object. NAN if unknown + * @param vel_acc [m/s] Velocity accuracy. NAN if unknown + * @param dist [m] Distance between camera and tracked object. NAN if unknown + * @param hdg [rad] Heading in radians, in NED. NAN if unknown + * @param hdg_acc [rad] Accuracy of heading, in NED. NAN if unknown + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_tracking_geo_status_send(mavlink_channel_t chan, uint8_t tracking_status, int32_t lat, int32_t lon, float alt, float h_acc, float v_acc, float vel_n, float vel_e, float vel_d, float vel_acc, float dist, float hdg, float hdg_acc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, h_acc); + _mav_put_float(buf, 16, v_acc); + _mav_put_float(buf, 20, vel_n); + _mav_put_float(buf, 24, vel_e); + _mav_put_float(buf, 28, vel_d); + _mav_put_float(buf, 32, vel_acc); + _mav_put_float(buf, 36, dist); + _mav_put_float(buf, 40, hdg); + _mav_put_float(buf, 44, hdg_acc); + _mav_put_uint8_t(buf, 48, tracking_status); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS, buf, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); +#else + mavlink_camera_tracking_geo_status_t packet; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_n = vel_n; + packet.vel_e = vel_e; + packet.vel_d = vel_d; + packet.vel_acc = vel_acc; + packet.dist = dist; + packet.hdg = hdg; + packet.hdg_acc = hdg_acc; + packet.tracking_status = tracking_status; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); +#endif +} + +/** + * @brief Send a camera_tracking_geo_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_camera_tracking_geo_status_send_struct(mavlink_channel_t chan, const mavlink_camera_tracking_geo_status_t* camera_tracking_geo_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_camera_tracking_geo_status_send(chan, camera_tracking_geo_status->tracking_status, camera_tracking_geo_status->lat, camera_tracking_geo_status->lon, camera_tracking_geo_status->alt, camera_tracking_geo_status->h_acc, camera_tracking_geo_status->v_acc, camera_tracking_geo_status->vel_n, camera_tracking_geo_status->vel_e, camera_tracking_geo_status->vel_d, camera_tracking_geo_status->vel_acc, camera_tracking_geo_status->dist, camera_tracking_geo_status->hdg, camera_tracking_geo_status->hdg_acc); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS, (const char *)camera_tracking_geo_status, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_tracking_geo_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t tracking_status, int32_t lat, int32_t lon, float alt, float h_acc, float v_acc, float vel_n, float vel_e, float vel_d, float vel_acc, float dist, float hdg, float hdg_acc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, h_acc); + _mav_put_float(buf, 16, v_acc); + _mav_put_float(buf, 20, vel_n); + _mav_put_float(buf, 24, vel_e); + _mav_put_float(buf, 28, vel_d); + _mav_put_float(buf, 32, vel_acc); + _mav_put_float(buf, 36, dist); + _mav_put_float(buf, 40, hdg); + _mav_put_float(buf, 44, hdg_acc); + _mav_put_uint8_t(buf, 48, tracking_status); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS, buf, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); +#else + mavlink_camera_tracking_geo_status_t *packet = (mavlink_camera_tracking_geo_status_t *)msgbuf; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->h_acc = h_acc; + packet->v_acc = v_acc; + packet->vel_n = vel_n; + packet->vel_e = vel_e; + packet->vel_d = vel_d; + packet->vel_acc = vel_acc; + packet->dist = dist; + packet->hdg = hdg; + packet->hdg_acc = hdg_acc; + packet->tracking_status = tracking_status; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS, (const char *)packet, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_TRACKING_GEO_STATUS UNPACKING + + +/** + * @brief Get field tracking_status from camera_tracking_geo_status message + * + * @return Current tracking status + */ +static inline uint8_t mavlink_msg_camera_tracking_geo_status_get_tracking_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 48); +} + +/** + * @brief Get field lat from camera_tracking_geo_status message + * + * @return [degE7] Latitude of tracked object + */ +static inline int32_t mavlink_msg_camera_tracking_geo_status_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field lon from camera_tracking_geo_status message + * + * @return [degE7] Longitude of tracked object + */ +static inline int32_t mavlink_msg_camera_tracking_geo_status_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field alt from camera_tracking_geo_status message + * + * @return [m] Altitude of tracked object(AMSL, WGS84) + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field h_acc from camera_tracking_geo_status message + * + * @return [m] Horizontal accuracy. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_h_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field v_acc from camera_tracking_geo_status message + * + * @return [m] Vertical accuracy. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_v_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field vel_n from camera_tracking_geo_status message + * + * @return [m/s] North velocity of tracked object. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_vel_n(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field vel_e from camera_tracking_geo_status message + * + * @return [m/s] East velocity of tracked object. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_vel_e(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field vel_d from camera_tracking_geo_status message + * + * @return [m/s] Down velocity of tracked object. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_vel_d(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field vel_acc from camera_tracking_geo_status message + * + * @return [m/s] Velocity accuracy. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_vel_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field dist from camera_tracking_geo_status message + * + * @return [m] Distance between camera and tracked object. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_dist(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field hdg from camera_tracking_geo_status message + * + * @return [rad] Heading in radians, in NED. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_hdg(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field hdg_acc from camera_tracking_geo_status message + * + * @return [rad] Accuracy of heading, in NED. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_hdg_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Decode a camera_tracking_geo_status message into a struct + * + * @param msg The message to decode + * @param camera_tracking_geo_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_tracking_geo_status_decode(const mavlink_message_t* msg, mavlink_camera_tracking_geo_status_t* camera_tracking_geo_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + camera_tracking_geo_status->lat = mavlink_msg_camera_tracking_geo_status_get_lat(msg); + camera_tracking_geo_status->lon = mavlink_msg_camera_tracking_geo_status_get_lon(msg); + camera_tracking_geo_status->alt = mavlink_msg_camera_tracking_geo_status_get_alt(msg); + camera_tracking_geo_status->h_acc = mavlink_msg_camera_tracking_geo_status_get_h_acc(msg); + camera_tracking_geo_status->v_acc = mavlink_msg_camera_tracking_geo_status_get_v_acc(msg); + camera_tracking_geo_status->vel_n = mavlink_msg_camera_tracking_geo_status_get_vel_n(msg); + camera_tracking_geo_status->vel_e = mavlink_msg_camera_tracking_geo_status_get_vel_e(msg); + camera_tracking_geo_status->vel_d = mavlink_msg_camera_tracking_geo_status_get_vel_d(msg); + camera_tracking_geo_status->vel_acc = mavlink_msg_camera_tracking_geo_status_get_vel_acc(msg); + camera_tracking_geo_status->dist = mavlink_msg_camera_tracking_geo_status_get_dist(msg); + camera_tracking_geo_status->hdg = mavlink_msg_camera_tracking_geo_status_get_hdg(msg); + camera_tracking_geo_status->hdg_acc = mavlink_msg_camera_tracking_geo_status_get_hdg_acc(msg); + camera_tracking_geo_status->tracking_status = mavlink_msg_camera_tracking_geo_status_get_tracking_status(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN; + memset(camera_tracking_geo_status, 0, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); + memcpy(camera_tracking_geo_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_tracking_image_status.h b/lib/main/MAVLink/common/mavlink_msg_camera_tracking_image_status.h new file mode 100644 index 0000000000..5f703fce5d --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_camera_tracking_image_status.h @@ -0,0 +1,438 @@ +#pragma once +// MESSAGE CAMERA_TRACKING_IMAGE_STATUS PACKING + +#define MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS 275 + + +typedef struct __mavlink_camera_tracking_image_status_t { + float point_x; /*< Current tracked point x value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is left, 1 is right), NAN if unknown*/ + float point_y; /*< Current tracked point y value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown*/ + float radius; /*< Current tracked radius if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is image left, 1 is image right), NAN if unknown*/ + float rec_top_x; /*< Current tracked rectangle top x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown*/ + float rec_top_y; /*< Current tracked rectangle top y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown*/ + float rec_bottom_x; /*< Current tracked rectangle bottom x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown*/ + float rec_bottom_y; /*< Current tracked rectangle bottom y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown*/ + uint8_t tracking_status; /*< Current tracking status*/ + uint8_t tracking_mode; /*< Current tracking mode*/ + uint8_t target_data; /*< Defines location of target data*/ +} mavlink_camera_tracking_image_status_t; + +#define MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN 31 +#define MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN 31 +#define MAVLINK_MSG_ID_275_LEN 31 +#define MAVLINK_MSG_ID_275_MIN_LEN 31 + +#define MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC 126 +#define MAVLINK_MSG_ID_275_CRC 126 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_IMAGE_STATUS { \ + 275, \ + "CAMERA_TRACKING_IMAGE_STATUS", \ + 10, \ + { { "tracking_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_camera_tracking_image_status_t, tracking_status) }, \ + { "tracking_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_camera_tracking_image_status_t, tracking_mode) }, \ + { "target_data", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_camera_tracking_image_status_t, target_data) }, \ + { "point_x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_camera_tracking_image_status_t, point_x) }, \ + { "point_y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_camera_tracking_image_status_t, point_y) }, \ + { "radius", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_camera_tracking_image_status_t, radius) }, \ + { "rec_top_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_tracking_image_status_t, rec_top_x) }, \ + { "rec_top_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_tracking_image_status_t, rec_top_y) }, \ + { "rec_bottom_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_camera_tracking_image_status_t, rec_bottom_x) }, \ + { "rec_bottom_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_camera_tracking_image_status_t, rec_bottom_y) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_IMAGE_STATUS { \ + "CAMERA_TRACKING_IMAGE_STATUS", \ + 10, \ + { { "tracking_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_camera_tracking_image_status_t, tracking_status) }, \ + { "tracking_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_camera_tracking_image_status_t, tracking_mode) }, \ + { "target_data", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_camera_tracking_image_status_t, target_data) }, \ + { "point_x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_camera_tracking_image_status_t, point_x) }, \ + { "point_y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_camera_tracking_image_status_t, point_y) }, \ + { "radius", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_camera_tracking_image_status_t, radius) }, \ + { "rec_top_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_tracking_image_status_t, rec_top_x) }, \ + { "rec_top_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_tracking_image_status_t, rec_top_y) }, \ + { "rec_bottom_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_camera_tracking_image_status_t, rec_bottom_x) }, \ + { "rec_bottom_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_camera_tracking_image_status_t, rec_bottom_y) }, \ + } \ +} +#endif + +/** + * @brief Pack a camera_tracking_image_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param tracking_status Current tracking status + * @param tracking_mode Current tracking mode + * @param target_data Defines location of target data + * @param point_x Current tracked point x value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param point_y Current tracked point y value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param radius Current tracked radius if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is image left, 1 is image right), NAN if unknown + * @param rec_top_x Current tracked rectangle top x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param rec_top_y Current tracked rectangle top y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param rec_bottom_x Current tracked rectangle bottom x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param rec_bottom_y Current tracked rectangle bottom y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_tracking_image_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t tracking_status, uint8_t tracking_mode, uint8_t target_data, float point_x, float point_y, float radius, float rec_top_x, float rec_top_y, float rec_bottom_x, float rec_bottom_y) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN]; + _mav_put_float(buf, 0, point_x); + _mav_put_float(buf, 4, point_y); + _mav_put_float(buf, 8, radius); + _mav_put_float(buf, 12, rec_top_x); + _mav_put_float(buf, 16, rec_top_y); + _mav_put_float(buf, 20, rec_bottom_x); + _mav_put_float(buf, 24, rec_bottom_y); + _mav_put_uint8_t(buf, 28, tracking_status); + _mav_put_uint8_t(buf, 29, tracking_mode); + _mav_put_uint8_t(buf, 30, target_data); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); +#else + mavlink_camera_tracking_image_status_t packet; + packet.point_x = point_x; + packet.point_y = point_y; + packet.radius = radius; + packet.rec_top_x = rec_top_x; + packet.rec_top_y = rec_top_y; + packet.rec_bottom_x = rec_bottom_x; + packet.rec_bottom_y = rec_bottom_y; + packet.tracking_status = tracking_status; + packet.tracking_mode = tracking_mode; + packet.target_data = target_data; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); +} + +/** + * @brief Pack a camera_tracking_image_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param tracking_status Current tracking status + * @param tracking_mode Current tracking mode + * @param target_data Defines location of target data + * @param point_x Current tracked point x value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param point_y Current tracked point y value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param radius Current tracked radius if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is image left, 1 is image right), NAN if unknown + * @param rec_top_x Current tracked rectangle top x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param rec_top_y Current tracked rectangle top y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param rec_bottom_x Current tracked rectangle bottom x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param rec_bottom_y Current tracked rectangle bottom y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_tracking_image_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t tracking_status,uint8_t tracking_mode,uint8_t target_data,float point_x,float point_y,float radius,float rec_top_x,float rec_top_y,float rec_bottom_x,float rec_bottom_y) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN]; + _mav_put_float(buf, 0, point_x); + _mav_put_float(buf, 4, point_y); + _mav_put_float(buf, 8, radius); + _mav_put_float(buf, 12, rec_top_x); + _mav_put_float(buf, 16, rec_top_y); + _mav_put_float(buf, 20, rec_bottom_x); + _mav_put_float(buf, 24, rec_bottom_y); + _mav_put_uint8_t(buf, 28, tracking_status); + _mav_put_uint8_t(buf, 29, tracking_mode); + _mav_put_uint8_t(buf, 30, target_data); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); +#else + mavlink_camera_tracking_image_status_t packet; + packet.point_x = point_x; + packet.point_y = point_y; + packet.radius = radius; + packet.rec_top_x = rec_top_x; + packet.rec_top_y = rec_top_y; + packet.rec_bottom_x = rec_bottom_x; + packet.rec_bottom_y = rec_bottom_y; + packet.tracking_status = tracking_status; + packet.tracking_mode = tracking_mode; + packet.target_data = target_data; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); +} + +/** + * @brief Encode a camera_tracking_image_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_tracking_image_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_tracking_image_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_tracking_image_status_t* camera_tracking_image_status) +{ + return mavlink_msg_camera_tracking_image_status_pack(system_id, component_id, msg, camera_tracking_image_status->tracking_status, camera_tracking_image_status->tracking_mode, camera_tracking_image_status->target_data, camera_tracking_image_status->point_x, camera_tracking_image_status->point_y, camera_tracking_image_status->radius, camera_tracking_image_status->rec_top_x, camera_tracking_image_status->rec_top_y, camera_tracking_image_status->rec_bottom_x, camera_tracking_image_status->rec_bottom_y); +} + +/** + * @brief Encode a camera_tracking_image_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_tracking_image_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_tracking_image_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_tracking_image_status_t* camera_tracking_image_status) +{ + return mavlink_msg_camera_tracking_image_status_pack_chan(system_id, component_id, chan, msg, camera_tracking_image_status->tracking_status, camera_tracking_image_status->tracking_mode, camera_tracking_image_status->target_data, camera_tracking_image_status->point_x, camera_tracking_image_status->point_y, camera_tracking_image_status->radius, camera_tracking_image_status->rec_top_x, camera_tracking_image_status->rec_top_y, camera_tracking_image_status->rec_bottom_x, camera_tracking_image_status->rec_bottom_y); +} + +/** + * @brief Send a camera_tracking_image_status message + * @param chan MAVLink channel to send the message + * + * @param tracking_status Current tracking status + * @param tracking_mode Current tracking mode + * @param target_data Defines location of target data + * @param point_x Current tracked point x value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param point_y Current tracked point y value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param radius Current tracked radius if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is image left, 1 is image right), NAN if unknown + * @param rec_top_x Current tracked rectangle top x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param rec_top_y Current tracked rectangle top y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param rec_bottom_x Current tracked rectangle bottom x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param rec_bottom_y Current tracked rectangle bottom y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_tracking_image_status_send(mavlink_channel_t chan, uint8_t tracking_status, uint8_t tracking_mode, uint8_t target_data, float point_x, float point_y, float radius, float rec_top_x, float rec_top_y, float rec_bottom_x, float rec_bottom_y) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN]; + _mav_put_float(buf, 0, point_x); + _mav_put_float(buf, 4, point_y); + _mav_put_float(buf, 8, radius); + _mav_put_float(buf, 12, rec_top_x); + _mav_put_float(buf, 16, rec_top_y); + _mav_put_float(buf, 20, rec_bottom_x); + _mav_put_float(buf, 24, rec_bottom_y); + _mav_put_uint8_t(buf, 28, tracking_status); + _mav_put_uint8_t(buf, 29, tracking_mode); + _mav_put_uint8_t(buf, 30, target_data); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, buf, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); +#else + mavlink_camera_tracking_image_status_t packet; + packet.point_x = point_x; + packet.point_y = point_y; + packet.radius = radius; + packet.rec_top_x = rec_top_x; + packet.rec_top_y = rec_top_y; + packet.rec_bottom_x = rec_bottom_x; + packet.rec_bottom_y = rec_bottom_y; + packet.tracking_status = tracking_status; + packet.tracking_mode = tracking_mode; + packet.target_data = target_data; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); +#endif +} + +/** + * @brief Send a camera_tracking_image_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_camera_tracking_image_status_send_struct(mavlink_channel_t chan, const mavlink_camera_tracking_image_status_t* camera_tracking_image_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_camera_tracking_image_status_send(chan, camera_tracking_image_status->tracking_status, camera_tracking_image_status->tracking_mode, camera_tracking_image_status->target_data, camera_tracking_image_status->point_x, camera_tracking_image_status->point_y, camera_tracking_image_status->radius, camera_tracking_image_status->rec_top_x, camera_tracking_image_status->rec_top_y, camera_tracking_image_status->rec_bottom_x, camera_tracking_image_status->rec_bottom_y); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, (const char *)camera_tracking_image_status, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_tracking_image_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t tracking_status, uint8_t tracking_mode, uint8_t target_data, float point_x, float point_y, float radius, float rec_top_x, float rec_top_y, float rec_bottom_x, float rec_bottom_y) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, point_x); + _mav_put_float(buf, 4, point_y); + _mav_put_float(buf, 8, radius); + _mav_put_float(buf, 12, rec_top_x); + _mav_put_float(buf, 16, rec_top_y); + _mav_put_float(buf, 20, rec_bottom_x); + _mav_put_float(buf, 24, rec_bottom_y); + _mav_put_uint8_t(buf, 28, tracking_status); + _mav_put_uint8_t(buf, 29, tracking_mode); + _mav_put_uint8_t(buf, 30, target_data); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, buf, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); +#else + mavlink_camera_tracking_image_status_t *packet = (mavlink_camera_tracking_image_status_t *)msgbuf; + packet->point_x = point_x; + packet->point_y = point_y; + packet->radius = radius; + packet->rec_top_x = rec_top_x; + packet->rec_top_y = rec_top_y; + packet->rec_bottom_x = rec_bottom_x; + packet->rec_bottom_y = rec_bottom_y; + packet->tracking_status = tracking_status; + packet->tracking_mode = tracking_mode; + packet->target_data = target_data; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, (const char *)packet, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_TRACKING_IMAGE_STATUS UNPACKING + + +/** + * @brief Get field tracking_status from camera_tracking_image_status message + * + * @return Current tracking status + */ +static inline uint8_t mavlink_msg_camera_tracking_image_status_get_tracking_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 28); +} + +/** + * @brief Get field tracking_mode from camera_tracking_image_status message + * + * @return Current tracking mode + */ +static inline uint8_t mavlink_msg_camera_tracking_image_status_get_tracking_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 29); +} + +/** + * @brief Get field target_data from camera_tracking_image_status message + * + * @return Defines location of target data + */ +static inline uint8_t mavlink_msg_camera_tracking_image_status_get_target_data(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 30); +} + +/** + * @brief Get field point_x from camera_tracking_image_status message + * + * @return Current tracked point x value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is left, 1 is right), NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_image_status_get_point_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field point_y from camera_tracking_image_status message + * + * @return Current tracked point y value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_image_status_get_point_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field radius from camera_tracking_image_status message + * + * @return Current tracked radius if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is image left, 1 is image right), NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_image_status_get_radius(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field rec_top_x from camera_tracking_image_status message + * + * @return Current tracked rectangle top x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_image_status_get_rec_top_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field rec_top_y from camera_tracking_image_status message + * + * @return Current tracked rectangle top y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_image_status_get_rec_top_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field rec_bottom_x from camera_tracking_image_status message + * + * @return Current tracked rectangle bottom x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_image_status_get_rec_bottom_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field rec_bottom_y from camera_tracking_image_status message + * + * @return Current tracked rectangle bottom y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_image_status_get_rec_bottom_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a camera_tracking_image_status message into a struct + * + * @param msg The message to decode + * @param camera_tracking_image_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_tracking_image_status_decode(const mavlink_message_t* msg, mavlink_camera_tracking_image_status_t* camera_tracking_image_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + camera_tracking_image_status->point_x = mavlink_msg_camera_tracking_image_status_get_point_x(msg); + camera_tracking_image_status->point_y = mavlink_msg_camera_tracking_image_status_get_point_y(msg); + camera_tracking_image_status->radius = mavlink_msg_camera_tracking_image_status_get_radius(msg); + camera_tracking_image_status->rec_top_x = mavlink_msg_camera_tracking_image_status_get_rec_top_x(msg); + camera_tracking_image_status->rec_top_y = mavlink_msg_camera_tracking_image_status_get_rec_top_y(msg); + camera_tracking_image_status->rec_bottom_x = mavlink_msg_camera_tracking_image_status_get_rec_bottom_x(msg); + camera_tracking_image_status->rec_bottom_y = mavlink_msg_camera_tracking_image_status_get_rec_bottom_y(msg); + camera_tracking_image_status->tracking_status = mavlink_msg_camera_tracking_image_status_get_tracking_status(msg); + camera_tracking_image_status->tracking_mode = mavlink_msg_camera_tracking_image_status_get_tracking_mode(msg); + camera_tracking_image_status->target_data = mavlink_msg_camera_tracking_image_status_get_target_data(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN; + memset(camera_tracking_image_status, 0, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); + memcpy(camera_tracking_image_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_cellular_config.h b/lib/main/MAVLink/common/mavlink_msg_cellular_config.h new file mode 100644 index 0000000000..5b8a1fb763 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_cellular_config.h @@ -0,0 +1,383 @@ +#pragma once +// MESSAGE CELLULAR_CONFIG PACKING + +#define MAVLINK_MSG_ID_CELLULAR_CONFIG 336 + + +typedef struct __mavlink_cellular_config_t { + uint8_t enable_lte; /*< Enable/disable LTE. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response.*/ + uint8_t enable_pin; /*< Enable/disable PIN on the SIM card. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response.*/ + char pin[16]; /*< PIN sent to the SIM card. Blank when PIN is disabled. Empty when message is sent back as a response.*/ + char new_pin[16]; /*< New PIN when changing the PIN. Blank to leave it unchanged. Empty when message is sent back as a response.*/ + char apn[32]; /*< Name of the cellular APN. Blank to leave it unchanged. Current APN when sent back as a response.*/ + char puk[16]; /*< Required PUK code in case the user failed to authenticate 3 times with the PIN. Empty when message is sent back as a response.*/ + uint8_t roaming; /*< Enable/disable roaming. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response.*/ + uint8_t response; /*< Message acceptance response (sent back to GS).*/ +} mavlink_cellular_config_t; + +#define MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN 84 +#define MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN 84 +#define MAVLINK_MSG_ID_336_LEN 84 +#define MAVLINK_MSG_ID_336_MIN_LEN 84 + +#define MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC 245 +#define MAVLINK_MSG_ID_336_CRC 245 + +#define MAVLINK_MSG_CELLULAR_CONFIG_FIELD_PIN_LEN 16 +#define MAVLINK_MSG_CELLULAR_CONFIG_FIELD_NEW_PIN_LEN 16 +#define MAVLINK_MSG_CELLULAR_CONFIG_FIELD_APN_LEN 32 +#define MAVLINK_MSG_CELLULAR_CONFIG_FIELD_PUK_LEN 16 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CELLULAR_CONFIG { \ + 336, \ + "CELLULAR_CONFIG", \ + 8, \ + { { "enable_lte", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_cellular_config_t, enable_lte) }, \ + { "enable_pin", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_cellular_config_t, enable_pin) }, \ + { "pin", NULL, MAVLINK_TYPE_CHAR, 16, 2, offsetof(mavlink_cellular_config_t, pin) }, \ + { "new_pin", NULL, MAVLINK_TYPE_CHAR, 16, 18, offsetof(mavlink_cellular_config_t, new_pin) }, \ + { "apn", NULL, MAVLINK_TYPE_CHAR, 32, 34, offsetof(mavlink_cellular_config_t, apn) }, \ + { "puk", NULL, MAVLINK_TYPE_CHAR, 16, 66, offsetof(mavlink_cellular_config_t, puk) }, \ + { "roaming", NULL, MAVLINK_TYPE_UINT8_T, 0, 82, offsetof(mavlink_cellular_config_t, roaming) }, \ + { "response", NULL, MAVLINK_TYPE_UINT8_T, 0, 83, offsetof(mavlink_cellular_config_t, response) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CELLULAR_CONFIG { \ + "CELLULAR_CONFIG", \ + 8, \ + { { "enable_lte", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_cellular_config_t, enable_lte) }, \ + { "enable_pin", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_cellular_config_t, enable_pin) }, \ + { "pin", NULL, MAVLINK_TYPE_CHAR, 16, 2, offsetof(mavlink_cellular_config_t, pin) }, \ + { "new_pin", NULL, MAVLINK_TYPE_CHAR, 16, 18, offsetof(mavlink_cellular_config_t, new_pin) }, \ + { "apn", NULL, MAVLINK_TYPE_CHAR, 32, 34, offsetof(mavlink_cellular_config_t, apn) }, \ + { "puk", NULL, MAVLINK_TYPE_CHAR, 16, 66, offsetof(mavlink_cellular_config_t, puk) }, \ + { "roaming", NULL, MAVLINK_TYPE_UINT8_T, 0, 82, offsetof(mavlink_cellular_config_t, roaming) }, \ + { "response", NULL, MAVLINK_TYPE_UINT8_T, 0, 83, offsetof(mavlink_cellular_config_t, response) }, \ + } \ +} +#endif + +/** + * @brief Pack a cellular_config message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param enable_lte Enable/disable LTE. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param enable_pin Enable/disable PIN on the SIM card. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param pin PIN sent to the SIM card. Blank when PIN is disabled. Empty when message is sent back as a response. + * @param new_pin New PIN when changing the PIN. Blank to leave it unchanged. Empty when message is sent back as a response. + * @param apn Name of the cellular APN. Blank to leave it unchanged. Current APN when sent back as a response. + * @param puk Required PUK code in case the user failed to authenticate 3 times with the PIN. Empty when message is sent back as a response. + * @param roaming Enable/disable roaming. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param response Message acceptance response (sent back to GS). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_cellular_config_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t enable_lte, uint8_t enable_pin, const char *pin, const char *new_pin, const char *apn, const char *puk, uint8_t roaming, uint8_t response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN]; + _mav_put_uint8_t(buf, 0, enable_lte); + _mav_put_uint8_t(buf, 1, enable_pin); + _mav_put_uint8_t(buf, 82, roaming); + _mav_put_uint8_t(buf, 83, response); + _mav_put_char_array(buf, 2, pin, 16); + _mav_put_char_array(buf, 18, new_pin, 16); + _mav_put_char_array(buf, 34, apn, 32); + _mav_put_char_array(buf, 66, puk, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN); +#else + mavlink_cellular_config_t packet; + packet.enable_lte = enable_lte; + packet.enable_pin = enable_pin; + packet.roaming = roaming; + packet.response = response; + mav_array_memcpy(packet.pin, pin, sizeof(char)*16); + mav_array_memcpy(packet.new_pin, new_pin, sizeof(char)*16); + mav_array_memcpy(packet.apn, apn, sizeof(char)*32); + mav_array_memcpy(packet.puk, puk, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CELLULAR_CONFIG; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); +} + +/** + * @brief Pack a cellular_config message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param enable_lte Enable/disable LTE. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param enable_pin Enable/disable PIN on the SIM card. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param pin PIN sent to the SIM card. Blank when PIN is disabled. Empty when message is sent back as a response. + * @param new_pin New PIN when changing the PIN. Blank to leave it unchanged. Empty when message is sent back as a response. + * @param apn Name of the cellular APN. Blank to leave it unchanged. Current APN when sent back as a response. + * @param puk Required PUK code in case the user failed to authenticate 3 times with the PIN. Empty when message is sent back as a response. + * @param roaming Enable/disable roaming. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param response Message acceptance response (sent back to GS). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_cellular_config_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t enable_lte,uint8_t enable_pin,const char *pin,const char *new_pin,const char *apn,const char *puk,uint8_t roaming,uint8_t response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN]; + _mav_put_uint8_t(buf, 0, enable_lte); + _mav_put_uint8_t(buf, 1, enable_pin); + _mav_put_uint8_t(buf, 82, roaming); + _mav_put_uint8_t(buf, 83, response); + _mav_put_char_array(buf, 2, pin, 16); + _mav_put_char_array(buf, 18, new_pin, 16); + _mav_put_char_array(buf, 34, apn, 32); + _mav_put_char_array(buf, 66, puk, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN); +#else + mavlink_cellular_config_t packet; + packet.enable_lte = enable_lte; + packet.enable_pin = enable_pin; + packet.roaming = roaming; + packet.response = response; + mav_array_memcpy(packet.pin, pin, sizeof(char)*16); + mav_array_memcpy(packet.new_pin, new_pin, sizeof(char)*16); + mav_array_memcpy(packet.apn, apn, sizeof(char)*32); + mav_array_memcpy(packet.puk, puk, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CELLULAR_CONFIG; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); +} + +/** + * @brief Encode a cellular_config struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param cellular_config C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cellular_config_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cellular_config_t* cellular_config) +{ + return mavlink_msg_cellular_config_pack(system_id, component_id, msg, cellular_config->enable_lte, cellular_config->enable_pin, cellular_config->pin, cellular_config->new_pin, cellular_config->apn, cellular_config->puk, cellular_config->roaming, cellular_config->response); +} + +/** + * @brief Encode a cellular_config struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param cellular_config C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cellular_config_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cellular_config_t* cellular_config) +{ + return mavlink_msg_cellular_config_pack_chan(system_id, component_id, chan, msg, cellular_config->enable_lte, cellular_config->enable_pin, cellular_config->pin, cellular_config->new_pin, cellular_config->apn, cellular_config->puk, cellular_config->roaming, cellular_config->response); +} + +/** + * @brief Send a cellular_config message + * @param chan MAVLink channel to send the message + * + * @param enable_lte Enable/disable LTE. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param enable_pin Enable/disable PIN on the SIM card. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param pin PIN sent to the SIM card. Blank when PIN is disabled. Empty when message is sent back as a response. + * @param new_pin New PIN when changing the PIN. Blank to leave it unchanged. Empty when message is sent back as a response. + * @param apn Name of the cellular APN. Blank to leave it unchanged. Current APN when sent back as a response. + * @param puk Required PUK code in case the user failed to authenticate 3 times with the PIN. Empty when message is sent back as a response. + * @param roaming Enable/disable roaming. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param response Message acceptance response (sent back to GS). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_cellular_config_send(mavlink_channel_t chan, uint8_t enable_lte, uint8_t enable_pin, const char *pin, const char *new_pin, const char *apn, const char *puk, uint8_t roaming, uint8_t response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN]; + _mav_put_uint8_t(buf, 0, enable_lte); + _mav_put_uint8_t(buf, 1, enable_pin); + _mav_put_uint8_t(buf, 82, roaming); + _mav_put_uint8_t(buf, 83, response); + _mav_put_char_array(buf, 2, pin, 16); + _mav_put_char_array(buf, 18, new_pin, 16); + _mav_put_char_array(buf, 34, apn, 32); + _mav_put_char_array(buf, 66, puk, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_CONFIG, buf, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); +#else + mavlink_cellular_config_t packet; + packet.enable_lte = enable_lte; + packet.enable_pin = enable_pin; + packet.roaming = roaming; + packet.response = response; + mav_array_memcpy(packet.pin, pin, sizeof(char)*16); + mav_array_memcpy(packet.new_pin, new_pin, sizeof(char)*16); + mav_array_memcpy(packet.apn, apn, sizeof(char)*32); + mav_array_memcpy(packet.puk, puk, sizeof(char)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_CONFIG, (const char *)&packet, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); +#endif +} + +/** + * @brief Send a cellular_config message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_cellular_config_send_struct(mavlink_channel_t chan, const mavlink_cellular_config_t* cellular_config) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_cellular_config_send(chan, cellular_config->enable_lte, cellular_config->enable_pin, cellular_config->pin, cellular_config->new_pin, cellular_config->apn, cellular_config->puk, cellular_config->roaming, cellular_config->response); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_CONFIG, (const char *)cellular_config, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_cellular_config_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t enable_lte, uint8_t enable_pin, const char *pin, const char *new_pin, const char *apn, const char *puk, uint8_t roaming, uint8_t response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, enable_lte); + _mav_put_uint8_t(buf, 1, enable_pin); + _mav_put_uint8_t(buf, 82, roaming); + _mav_put_uint8_t(buf, 83, response); + _mav_put_char_array(buf, 2, pin, 16); + _mav_put_char_array(buf, 18, new_pin, 16); + _mav_put_char_array(buf, 34, apn, 32); + _mav_put_char_array(buf, 66, puk, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_CONFIG, buf, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); +#else + mavlink_cellular_config_t *packet = (mavlink_cellular_config_t *)msgbuf; + packet->enable_lte = enable_lte; + packet->enable_pin = enable_pin; + packet->roaming = roaming; + packet->response = response; + mav_array_memcpy(packet->pin, pin, sizeof(char)*16); + mav_array_memcpy(packet->new_pin, new_pin, sizeof(char)*16); + mav_array_memcpy(packet->apn, apn, sizeof(char)*32); + mav_array_memcpy(packet->puk, puk, sizeof(char)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_CONFIG, (const char *)packet, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CELLULAR_CONFIG UNPACKING + + +/** + * @brief Get field enable_lte from cellular_config message + * + * @return Enable/disable LTE. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + */ +static inline uint8_t mavlink_msg_cellular_config_get_enable_lte(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field enable_pin from cellular_config message + * + * @return Enable/disable PIN on the SIM card. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + */ +static inline uint8_t mavlink_msg_cellular_config_get_enable_pin(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field pin from cellular_config message + * + * @return PIN sent to the SIM card. Blank when PIN is disabled. Empty when message is sent back as a response. + */ +static inline uint16_t mavlink_msg_cellular_config_get_pin(const mavlink_message_t* msg, char *pin) +{ + return _MAV_RETURN_char_array(msg, pin, 16, 2); +} + +/** + * @brief Get field new_pin from cellular_config message + * + * @return New PIN when changing the PIN. Blank to leave it unchanged. Empty when message is sent back as a response. + */ +static inline uint16_t mavlink_msg_cellular_config_get_new_pin(const mavlink_message_t* msg, char *new_pin) +{ + return _MAV_RETURN_char_array(msg, new_pin, 16, 18); +} + +/** + * @brief Get field apn from cellular_config message + * + * @return Name of the cellular APN. Blank to leave it unchanged. Current APN when sent back as a response. + */ +static inline uint16_t mavlink_msg_cellular_config_get_apn(const mavlink_message_t* msg, char *apn) +{ + return _MAV_RETURN_char_array(msg, apn, 32, 34); +} + +/** + * @brief Get field puk from cellular_config message + * + * @return Required PUK code in case the user failed to authenticate 3 times with the PIN. Empty when message is sent back as a response. + */ +static inline uint16_t mavlink_msg_cellular_config_get_puk(const mavlink_message_t* msg, char *puk) +{ + return _MAV_RETURN_char_array(msg, puk, 16, 66); +} + +/** + * @brief Get field roaming from cellular_config message + * + * @return Enable/disable roaming. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + */ +static inline uint8_t mavlink_msg_cellular_config_get_roaming(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 82); +} + +/** + * @brief Get field response from cellular_config message + * + * @return Message acceptance response (sent back to GS). + */ +static inline uint8_t mavlink_msg_cellular_config_get_response(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 83); +} + +/** + * @brief Decode a cellular_config message into a struct + * + * @param msg The message to decode + * @param cellular_config C-struct to decode the message contents into + */ +static inline void mavlink_msg_cellular_config_decode(const mavlink_message_t* msg, mavlink_cellular_config_t* cellular_config) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + cellular_config->enable_lte = mavlink_msg_cellular_config_get_enable_lte(msg); + cellular_config->enable_pin = mavlink_msg_cellular_config_get_enable_pin(msg); + mavlink_msg_cellular_config_get_pin(msg, cellular_config->pin); + mavlink_msg_cellular_config_get_new_pin(msg, cellular_config->new_pin); + mavlink_msg_cellular_config_get_apn(msg, cellular_config->apn); + mavlink_msg_cellular_config_get_puk(msg, cellular_config->puk); + cellular_config->roaming = mavlink_msg_cellular_config_get_roaming(msg); + cellular_config->response = mavlink_msg_cellular_config_get_response(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN? msg->len : MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN; + memset(cellular_config, 0, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN); + memcpy(cellular_config, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_cellular_status.h b/lib/main/MAVLink/common/mavlink_msg_cellular_status.h new file mode 100644 index 0000000000..5b1c195805 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_cellular_status.h @@ -0,0 +1,363 @@ +#pragma once +// MESSAGE CELLULAR_STATUS PACKING + +#define MAVLINK_MSG_ID_CELLULAR_STATUS 334 + + +typedef struct __mavlink_cellular_status_t { + uint16_t mcc; /*< Mobile country code. If unknown, set to UINT16_MAX*/ + uint16_t mnc; /*< Mobile network code. If unknown, set to UINT16_MAX*/ + uint16_t lac; /*< Location area code. If unknown, set to 0*/ + uint8_t status; /*< Cellular modem status*/ + uint8_t failure_reason; /*< Failure reason when status in in CELLUAR_STATUS_FAILED*/ + uint8_t type; /*< Cellular network radio type: gsm, cdma, lte...*/ + uint8_t quality; /*< Signal quality in percent. If unknown, set to UINT8_MAX*/ +} mavlink_cellular_status_t; + +#define MAVLINK_MSG_ID_CELLULAR_STATUS_LEN 10 +#define MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN 10 +#define MAVLINK_MSG_ID_334_LEN 10 +#define MAVLINK_MSG_ID_334_MIN_LEN 10 + +#define MAVLINK_MSG_ID_CELLULAR_STATUS_CRC 72 +#define MAVLINK_MSG_ID_334_CRC 72 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CELLULAR_STATUS { \ + 334, \ + "CELLULAR_STATUS", \ + 7, \ + { { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_cellular_status_t, status) }, \ + { "failure_reason", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_cellular_status_t, failure_reason) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_cellular_status_t, type) }, \ + { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_cellular_status_t, quality) }, \ + { "mcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_cellular_status_t, mcc) }, \ + { "mnc", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_cellular_status_t, mnc) }, \ + { "lac", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_cellular_status_t, lac) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CELLULAR_STATUS { \ + "CELLULAR_STATUS", \ + 7, \ + { { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_cellular_status_t, status) }, \ + { "failure_reason", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_cellular_status_t, failure_reason) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_cellular_status_t, type) }, \ + { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_cellular_status_t, quality) }, \ + { "mcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_cellular_status_t, mcc) }, \ + { "mnc", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_cellular_status_t, mnc) }, \ + { "lac", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_cellular_status_t, lac) }, \ + } \ +} +#endif + +/** + * @brief Pack a cellular_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param status Cellular modem status + * @param failure_reason Failure reason when status in in CELLUAR_STATUS_FAILED + * @param type Cellular network radio type: gsm, cdma, lte... + * @param quality Signal quality in percent. If unknown, set to UINT8_MAX + * @param mcc Mobile country code. If unknown, set to UINT16_MAX + * @param mnc Mobile network code. If unknown, set to UINT16_MAX + * @param lac Location area code. If unknown, set to 0 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_cellular_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t status, uint8_t failure_reason, uint8_t type, uint8_t quality, uint16_t mcc, uint16_t mnc, uint16_t lac) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CELLULAR_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, mcc); + _mav_put_uint16_t(buf, 2, mnc); + _mav_put_uint16_t(buf, 4, lac); + _mav_put_uint8_t(buf, 6, status); + _mav_put_uint8_t(buf, 7, failure_reason); + _mav_put_uint8_t(buf, 8, type); + _mav_put_uint8_t(buf, 9, quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN); +#else + mavlink_cellular_status_t packet; + packet.mcc = mcc; + packet.mnc = mnc; + packet.lac = lac; + packet.status = status; + packet.failure_reason = failure_reason; + packet.type = type; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CELLULAR_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_CRC); +} + +/** + * @brief Pack a cellular_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param status Cellular modem status + * @param failure_reason Failure reason when status in in CELLUAR_STATUS_FAILED + * @param type Cellular network radio type: gsm, cdma, lte... + * @param quality Signal quality in percent. If unknown, set to UINT8_MAX + * @param mcc Mobile country code. If unknown, set to UINT16_MAX + * @param mnc Mobile network code. If unknown, set to UINT16_MAX + * @param lac Location area code. If unknown, set to 0 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_cellular_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t status,uint8_t failure_reason,uint8_t type,uint8_t quality,uint16_t mcc,uint16_t mnc,uint16_t lac) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CELLULAR_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, mcc); + _mav_put_uint16_t(buf, 2, mnc); + _mav_put_uint16_t(buf, 4, lac); + _mav_put_uint8_t(buf, 6, status); + _mav_put_uint8_t(buf, 7, failure_reason); + _mav_put_uint8_t(buf, 8, type); + _mav_put_uint8_t(buf, 9, quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN); +#else + mavlink_cellular_status_t packet; + packet.mcc = mcc; + packet.mnc = mnc; + packet.lac = lac; + packet.status = status; + packet.failure_reason = failure_reason; + packet.type = type; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CELLULAR_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_CRC); +} + +/** + * @brief Encode a cellular_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param cellular_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cellular_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cellular_status_t* cellular_status) +{ + return mavlink_msg_cellular_status_pack(system_id, component_id, msg, cellular_status->status, cellular_status->failure_reason, cellular_status->type, cellular_status->quality, cellular_status->mcc, cellular_status->mnc, cellular_status->lac); +} + +/** + * @brief Encode a cellular_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param cellular_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cellular_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cellular_status_t* cellular_status) +{ + return mavlink_msg_cellular_status_pack_chan(system_id, component_id, chan, msg, cellular_status->status, cellular_status->failure_reason, cellular_status->type, cellular_status->quality, cellular_status->mcc, cellular_status->mnc, cellular_status->lac); +} + +/** + * @brief Send a cellular_status message + * @param chan MAVLink channel to send the message + * + * @param status Cellular modem status + * @param failure_reason Failure reason when status in in CELLUAR_STATUS_FAILED + * @param type Cellular network radio type: gsm, cdma, lte... + * @param quality Signal quality in percent. If unknown, set to UINT8_MAX + * @param mcc Mobile country code. If unknown, set to UINT16_MAX + * @param mnc Mobile network code. If unknown, set to UINT16_MAX + * @param lac Location area code. If unknown, set to 0 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_cellular_status_send(mavlink_channel_t chan, uint8_t status, uint8_t failure_reason, uint8_t type, uint8_t quality, uint16_t mcc, uint16_t mnc, uint16_t lac) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CELLULAR_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, mcc); + _mav_put_uint16_t(buf, 2, mnc); + _mav_put_uint16_t(buf, 4, lac); + _mav_put_uint8_t(buf, 6, status); + _mav_put_uint8_t(buf, 7, failure_reason); + _mav_put_uint8_t(buf, 8, type); + _mav_put_uint8_t(buf, 9, quality); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_STATUS, buf, MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_CRC); +#else + mavlink_cellular_status_t packet; + packet.mcc = mcc; + packet.mnc = mnc; + packet.lac = lac; + packet.status = status; + packet.failure_reason = failure_reason; + packet.type = type; + packet.quality = quality; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_STATUS, (const char *)&packet, MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_CRC); +#endif +} + +/** + * @brief Send a cellular_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_cellular_status_send_struct(mavlink_channel_t chan, const mavlink_cellular_status_t* cellular_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_cellular_status_send(chan, cellular_status->status, cellular_status->failure_reason, cellular_status->type, cellular_status->quality, cellular_status->mcc, cellular_status->mnc, cellular_status->lac); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_STATUS, (const char *)cellular_status, MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CELLULAR_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_cellular_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t status, uint8_t failure_reason, uint8_t type, uint8_t quality, uint16_t mcc, uint16_t mnc, uint16_t lac) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, mcc); + _mav_put_uint16_t(buf, 2, mnc); + _mav_put_uint16_t(buf, 4, lac); + _mav_put_uint8_t(buf, 6, status); + _mav_put_uint8_t(buf, 7, failure_reason); + _mav_put_uint8_t(buf, 8, type); + _mav_put_uint8_t(buf, 9, quality); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_STATUS, buf, MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_CRC); +#else + mavlink_cellular_status_t *packet = (mavlink_cellular_status_t *)msgbuf; + packet->mcc = mcc; + packet->mnc = mnc; + packet->lac = lac; + packet->status = status; + packet->failure_reason = failure_reason; + packet->type = type; + packet->quality = quality; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_STATUS, (const char *)packet, MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CELLULAR_STATUS UNPACKING + + +/** + * @brief Get field status from cellular_status message + * + * @return Cellular modem status + */ +static inline uint8_t mavlink_msg_cellular_status_get_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field failure_reason from cellular_status message + * + * @return Failure reason when status in in CELLUAR_STATUS_FAILED + */ +static inline uint8_t mavlink_msg_cellular_status_get_failure_reason(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 7); +} + +/** + * @brief Get field type from cellular_status message + * + * @return Cellular network radio type: gsm, cdma, lte... + */ +static inline uint8_t mavlink_msg_cellular_status_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field quality from cellular_status message + * + * @return Signal quality in percent. If unknown, set to UINT8_MAX + */ +static inline uint8_t mavlink_msg_cellular_status_get_quality(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 9); +} + +/** + * @brief Get field mcc from cellular_status message + * + * @return Mobile country code. If unknown, set to UINT16_MAX + */ +static inline uint16_t mavlink_msg_cellular_status_get_mcc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field mnc from cellular_status message + * + * @return Mobile network code. If unknown, set to UINT16_MAX + */ +static inline uint16_t mavlink_msg_cellular_status_get_mnc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field lac from cellular_status message + * + * @return Location area code. If unknown, set to 0 + */ +static inline uint16_t mavlink_msg_cellular_status_get_lac(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Decode a cellular_status message into a struct + * + * @param msg The message to decode + * @param cellular_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_cellular_status_decode(const mavlink_message_t* msg, mavlink_cellular_status_t* cellular_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + cellular_status->mcc = mavlink_msg_cellular_status_get_mcc(msg); + cellular_status->mnc = mavlink_msg_cellular_status_get_mnc(msg); + cellular_status->lac = mavlink_msg_cellular_status_get_lac(msg); + cellular_status->status = mavlink_msg_cellular_status_get_status(msg); + cellular_status->failure_reason = mavlink_msg_cellular_status_get_failure_reason(msg); + cellular_status->type = mavlink_msg_cellular_status_get_type(msg); + cellular_status->quality = mavlink_msg_cellular_status_get_quality(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CELLULAR_STATUS_LEN? msg->len : MAVLINK_MSG_ID_CELLULAR_STATUS_LEN; + memset(cellular_status, 0, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN); + memcpy(cellular_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_command_ack.h b/lib/main/MAVLink/common/mavlink_msg_command_ack.h index 52991543c5..58dd1fc6df 100755 --- a/lib/main/MAVLink/common/mavlink_msg_command_ack.h +++ b/lib/main/MAVLink/common/mavlink_msg_command_ack.h @@ -7,11 +7,15 @@ typedef struct __mavlink_command_ack_t { uint16_t command; /*< Command ID (of acknowledged command).*/ uint8_t result; /*< Result of command.*/ + uint8_t progress; /*< WIP: Also used as result_param1, it can be set with an enum containing the errors reasons of why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS (255 if the progress is unknown).*/ + int32_t result_param2; /*< WIP: Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied.*/ + uint8_t target_system; /*< WIP: System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement.*/ + uint8_t target_component; /*< WIP: Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement.*/ } mavlink_command_ack_t; -#define MAVLINK_MSG_ID_COMMAND_ACK_LEN 3 +#define MAVLINK_MSG_ID_COMMAND_ACK_LEN 10 #define MAVLINK_MSG_ID_COMMAND_ACK_MIN_LEN 3 -#define MAVLINK_MSG_ID_77_LEN 3 +#define MAVLINK_MSG_ID_77_LEN 10 #define MAVLINK_MSG_ID_77_MIN_LEN 3 #define MAVLINK_MSG_ID_COMMAND_ACK_CRC 143 @@ -23,17 +27,25 @@ typedef struct __mavlink_command_ack_t { #define MAVLINK_MESSAGE_INFO_COMMAND_ACK { \ 77, \ "COMMAND_ACK", \ - 2, \ + 6, \ { { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_command_ack_t, command) }, \ { "result", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_command_ack_t, result) }, \ + { "progress", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_command_ack_t, progress) }, \ + { "result_param2", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_command_ack_t, result_param2) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_command_ack_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_command_ack_t, target_component) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_COMMAND_ACK { \ "COMMAND_ACK", \ - 2, \ + 6, \ { { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_command_ack_t, command) }, \ { "result", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_command_ack_t, result) }, \ + { "progress", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_command_ack_t, progress) }, \ + { "result_param2", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_command_ack_t, result_param2) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_command_ack_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_command_ack_t, target_component) }, \ } \ } #endif @@ -46,21 +58,33 @@ typedef struct __mavlink_command_ack_t { * * @param command Command ID (of acknowledged command). * @param result Result of command. + * @param progress WIP: Also used as result_param1, it can be set with an enum containing the errors reasons of why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS (255 if the progress is unknown). + * @param result_param2 WIP: Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. + * @param target_system WIP: System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + * @param target_component WIP: Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t command, uint8_t result) + uint16_t command, uint8_t result, uint8_t progress, int32_t result_param2, uint8_t target_system, uint8_t target_component) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_COMMAND_ACK_LEN]; _mav_put_uint16_t(buf, 0, command); _mav_put_uint8_t(buf, 2, result); + _mav_put_uint8_t(buf, 3, progress); + _mav_put_int32_t(buf, 4, result_param2); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN); #else mavlink_command_ack_t packet; packet.command = command; packet.result = result; + packet.progress = progress; + packet.result_param2 = result_param2; + packet.target_system = target_system; + packet.target_component = target_component; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN); #endif @@ -77,22 +101,34 @@ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t c * @param msg The MAVLink message to compress the data into * @param command Command ID (of acknowledged command). * @param result Result of command. + * @param progress WIP: Also used as result_param1, it can be set with an enum containing the errors reasons of why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS (255 if the progress is unknown). + * @param result_param2 WIP: Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. + * @param target_system WIP: System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + * @param target_component WIP: Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint16_t command,uint8_t result) + uint16_t command,uint8_t result,uint8_t progress,int32_t result_param2,uint8_t target_system,uint8_t target_component) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_COMMAND_ACK_LEN]; _mav_put_uint16_t(buf, 0, command); _mav_put_uint8_t(buf, 2, result); + _mav_put_uint8_t(buf, 3, progress); + _mav_put_int32_t(buf, 4, result_param2); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN); #else mavlink_command_ack_t packet; packet.command = command; packet.result = result; + packet.progress = progress; + packet.result_param2 = result_param2; + packet.target_system = target_system; + packet.target_component = target_component; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN); #endif @@ -111,7 +147,7 @@ static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_command_ack_t* command_ack) { - return mavlink_msg_command_ack_pack(system_id, component_id, msg, command_ack->command, command_ack->result); + return mavlink_msg_command_ack_pack(system_id, component_id, msg, command_ack->command, command_ack->result, command_ack->progress, command_ack->result_param2, command_ack->target_system, command_ack->target_component); } /** @@ -125,7 +161,7 @@ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_command_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_command_ack_t* command_ack) { - return mavlink_msg_command_ack_pack_chan(system_id, component_id, chan, msg, command_ack->command, command_ack->result); + return mavlink_msg_command_ack_pack_chan(system_id, component_id, chan, msg, command_ack->command, command_ack->result, command_ack->progress, command_ack->result_param2, command_ack->target_system, command_ack->target_component); } /** @@ -134,21 +170,33 @@ static inline uint16_t mavlink_msg_command_ack_encode_chan(uint8_t system_id, ui * * @param command Command ID (of acknowledged command). * @param result Result of command. + * @param progress WIP: Also used as result_param1, it can be set with an enum containing the errors reasons of why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS (255 if the progress is unknown). + * @param result_param2 WIP: Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. + * @param target_system WIP: System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + * @param target_component WIP: Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, uint16_t command, uint8_t result) +static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, uint16_t command, uint8_t result, uint8_t progress, int32_t result_param2, uint8_t target_system, uint8_t target_component) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_COMMAND_ACK_LEN]; _mav_put_uint16_t(buf, 0, command); _mav_put_uint8_t(buf, 2, result); + _mav_put_uint8_t(buf, 3, progress); + _mav_put_int32_t(buf, 4, result_param2); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_MIN_LEN, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); #else mavlink_command_ack_t packet; packet.command = command; packet.result = result; + packet.progress = progress; + packet.result_param2 = result_param2; + packet.target_system = target_system; + packet.target_component = target_component; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)&packet, MAVLINK_MSG_ID_COMMAND_ACK_MIN_LEN, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); #endif @@ -162,7 +210,7 @@ static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, uint16_t static inline void mavlink_msg_command_ack_send_struct(mavlink_channel_t chan, const mavlink_command_ack_t* command_ack) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_command_ack_send(chan, command_ack->command, command_ack->result); + mavlink_msg_command_ack_send(chan, command_ack->command, command_ack->result, command_ack->progress, command_ack->result_param2, command_ack->target_system, command_ack->target_component); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)command_ack, MAVLINK_MSG_ID_COMMAND_ACK_MIN_LEN, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); #endif @@ -176,18 +224,26 @@ static inline void mavlink_msg_command_ack_send_struct(mavlink_channel_t chan, c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_command_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t command, uint8_t result) +static inline void mavlink_msg_command_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t command, uint8_t result, uint8_t progress, int32_t result_param2, uint8_t target_system, uint8_t target_component) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint16_t(buf, 0, command); _mav_put_uint8_t(buf, 2, result); + _mav_put_uint8_t(buf, 3, progress); + _mav_put_int32_t(buf, 4, result_param2); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_MIN_LEN, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); #else mavlink_command_ack_t *packet = (mavlink_command_ack_t *)msgbuf; packet->command = command; packet->result = result; + packet->progress = progress; + packet->result_param2 = result_param2; + packet->target_system = target_system; + packet->target_component = target_component; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_COMMAND_ACK_MIN_LEN, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); #endif @@ -219,6 +275,46 @@ static inline uint8_t mavlink_msg_command_ack_get_result(const mavlink_message_t return _MAV_RETURN_uint8_t(msg, 2); } +/** + * @brief Get field progress from command_ack message + * + * @return WIP: Also used as result_param1, it can be set with an enum containing the errors reasons of why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS (255 if the progress is unknown). + */ +static inline uint8_t mavlink_msg_command_ack_get_progress(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field result_param2 from command_ack message + * + * @return WIP: Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. + */ +static inline int32_t mavlink_msg_command_ack_get_result_param2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field target_system from command_ack message + * + * @return WIP: System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + */ +static inline uint8_t mavlink_msg_command_ack_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field target_component from command_ack message + * + * @return WIP: Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + */ +static inline uint8_t mavlink_msg_command_ack_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 9); +} + /** * @brief Decode a command_ack message into a struct * @@ -230,6 +326,10 @@ static inline void mavlink_msg_command_ack_decode(const mavlink_message_t* msg, #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS command_ack->command = mavlink_msg_command_ack_get_command(msg); command_ack->result = mavlink_msg_command_ack_get_result(msg); + command_ack->progress = mavlink_msg_command_ack_get_progress(msg); + command_ack->result_param2 = mavlink_msg_command_ack_get_result_param2(msg); + command_ack->target_system = mavlink_msg_command_ack_get_target_system(msg); + command_ack->target_component = mavlink_msg_command_ack_get_target_component(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_COMMAND_ACK_LEN? msg->len : MAVLINK_MSG_ID_COMMAND_ACK_LEN; memset(command_ack, 0, MAVLINK_MSG_ID_COMMAND_ACK_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_command_int.h b/lib/main/MAVLink/common/mavlink_msg_command_int.h index dcd298a1c9..a23f50895c 100755 --- a/lib/main/MAVLink/common/mavlink_msg_command_int.h +++ b/lib/main/MAVLink/common/mavlink_msg_command_int.h @@ -16,8 +16,8 @@ typedef struct __mavlink_command_int_t { uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ uint8_t frame; /*< The coordinate system of the COMMAND.*/ - uint8_t current; /*< false:0, true:1*/ - uint8_t autocontinue; /*< autocontinue to next wp*/ + uint8_t current; /*< Not used.*/ + uint8_t autocontinue; /*< Not used (set 0).*/ } mavlink_command_int_t; #define MAVLINK_MSG_ID_COMMAND_INT_LEN 35 @@ -81,8 +81,8 @@ typedef struct __mavlink_command_int_t { * @param target_component Component ID * @param frame The coordinate system of the COMMAND. * @param command The scheduled action for the mission item. - * @param current false:0, true:1 - * @param autocontinue autocontinue to next wp + * @param current Not used. + * @param autocontinue Not used (set 0). * @param param1 PARAM1, see MAV_CMD enum * @param param2 PARAM2, see MAV_CMD enum * @param param3 PARAM3, see MAV_CMD enum @@ -145,8 +145,8 @@ static inline uint16_t mavlink_msg_command_int_pack(uint8_t system_id, uint8_t c * @param target_component Component ID * @param frame The coordinate system of the COMMAND. * @param command The scheduled action for the mission item. - * @param current false:0, true:1 - * @param autocontinue autocontinue to next wp + * @param current Not used. + * @param autocontinue Not used (set 0). * @param param1 PARAM1, see MAV_CMD enum * @param param2 PARAM2, see MAV_CMD enum * @param param3 PARAM3, see MAV_CMD enum @@ -235,8 +235,8 @@ static inline uint16_t mavlink_msg_command_int_encode_chan(uint8_t system_id, ui * @param target_component Component ID * @param frame The coordinate system of the COMMAND. * @param command The scheduled action for the mission item. - * @param current false:0, true:1 - * @param autocontinue autocontinue to next wp + * @param current Not used. + * @param autocontinue Not used (set 0). * @param param1 PARAM1, see MAV_CMD enum * @param param2 PARAM2, see MAV_CMD enum * @param param3 PARAM3, see MAV_CMD enum @@ -396,7 +396,7 @@ static inline uint16_t mavlink_msg_command_int_get_command(const mavlink_message /** * @brief Get field current from command_int message * - * @return false:0, true:1 + * @return Not used. */ static inline uint8_t mavlink_msg_command_int_get_current(const mavlink_message_t* msg) { @@ -406,7 +406,7 @@ static inline uint8_t mavlink_msg_command_int_get_current(const mavlink_message_ /** * @brief Get field autocontinue from command_int message * - * @return autocontinue to next wp + * @return Not used (set 0). */ static inline uint8_t mavlink_msg_command_int_get_autocontinue(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_component_information.h b/lib/main/MAVLink/common/mavlink_msg_component_information.h new file mode 100644 index 0000000000..a916e6bee6 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_component_information.h @@ -0,0 +1,331 @@ +#pragma once +// MESSAGE COMPONENT_INFORMATION PACKING + +#define MAVLINK_MSG_ID_COMPONENT_INFORMATION 395 + + +typedef struct __mavlink_component_information_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + uint32_t metadata_type; /*< The type of metadata being requested.*/ + uint32_t metadata_uid; /*< Unique uid for this metadata which a gcs can use for created cached metadata and understanding whether it's cache it up to date or whether it needs to download new data.*/ + uint32_t translation_uid; /*< Unique uid for the translation file associated with the metadata.*/ + char metadata_uri[70]; /*< Component definition URI. If prefix mavlinkftp:// file is downloaded from vehicle over mavlink ftp protocol. If prefix http[s]:// file is downloaded over internet. Files are json format. They can end in .gz to indicate file is in gzip format.*/ + char translation_uri[70]; /*< The translations for strings within the metadata file. If null the either do not exist or may be included in the metadata file itself. The translations specified here supercede any which may be in the metadata file itself. The uri format is the same as component_metadata_uri . Files are in Json Translation spec format. Empty string indicates no tranlsation file.*/ +} mavlink_component_information_t; + +#define MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN 156 +#define MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN 156 +#define MAVLINK_MSG_ID_395_LEN 156 +#define MAVLINK_MSG_ID_395_MIN_LEN 156 + +#define MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC 163 +#define MAVLINK_MSG_ID_395_CRC 163 + +#define MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_METADATA_URI_LEN 70 +#define MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_TRANSLATION_URI_LEN 70 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_COMPONENT_INFORMATION { \ + 395, \ + "COMPONENT_INFORMATION", \ + 6, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_component_information_t, time_boot_ms) }, \ + { "metadata_type", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_component_information_t, metadata_type) }, \ + { "metadata_uid", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_component_information_t, metadata_uid) }, \ + { "metadata_uri", NULL, MAVLINK_TYPE_CHAR, 70, 16, offsetof(mavlink_component_information_t, metadata_uri) }, \ + { "translation_uid", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_component_information_t, translation_uid) }, \ + { "translation_uri", NULL, MAVLINK_TYPE_CHAR, 70, 86, offsetof(mavlink_component_information_t, translation_uri) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_COMPONENT_INFORMATION { \ + "COMPONENT_INFORMATION", \ + 6, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_component_information_t, time_boot_ms) }, \ + { "metadata_type", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_component_information_t, metadata_type) }, \ + { "metadata_uid", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_component_information_t, metadata_uid) }, \ + { "metadata_uri", NULL, MAVLINK_TYPE_CHAR, 70, 16, offsetof(mavlink_component_information_t, metadata_uri) }, \ + { "translation_uid", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_component_information_t, translation_uid) }, \ + { "translation_uri", NULL, MAVLINK_TYPE_CHAR, 70, 86, offsetof(mavlink_component_information_t, translation_uri) }, \ + } \ +} +#endif + +/** + * @brief Pack a component_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param metadata_type The type of metadata being requested. + * @param metadata_uid Unique uid for this metadata which a gcs can use for created cached metadata and understanding whether it's cache it up to date or whether it needs to download new data. + * @param metadata_uri Component definition URI. If prefix mavlinkftp:// file is downloaded from vehicle over mavlink ftp protocol. If prefix http[s]:// file is downloaded over internet. Files are json format. They can end in .gz to indicate file is in gzip format. + * @param translation_uid Unique uid for the translation file associated with the metadata. + * @param translation_uri The translations for strings within the metadata file. If null the either do not exist or may be included in the metadata file itself. The translations specified here supercede any which may be in the metadata file itself. The uri format is the same as component_metadata_uri . Files are in Json Translation spec format. Empty string indicates no tranlsation file. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_component_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint32_t metadata_type, uint32_t metadata_uid, const char *metadata_uri, uint32_t translation_uid, const char *translation_uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, metadata_type); + _mav_put_uint32_t(buf, 8, metadata_uid); + _mav_put_uint32_t(buf, 12, translation_uid); + _mav_put_char_array(buf, 16, metadata_uri, 70); + _mav_put_char_array(buf, 86, translation_uri, 70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); +#else + mavlink_component_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.metadata_type = metadata_type; + packet.metadata_uid = metadata_uid; + packet.translation_uid = translation_uid; + mav_array_memcpy(packet.metadata_uri, metadata_uri, sizeof(char)*70); + mav_array_memcpy(packet.translation_uri, translation_uri, sizeof(char)*70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMPONENT_INFORMATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); +} + +/** + * @brief Pack a component_information message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param metadata_type The type of metadata being requested. + * @param metadata_uid Unique uid for this metadata which a gcs can use for created cached metadata and understanding whether it's cache it up to date or whether it needs to download new data. + * @param metadata_uri Component definition URI. If prefix mavlinkftp:// file is downloaded from vehicle over mavlink ftp protocol. If prefix http[s]:// file is downloaded over internet. Files are json format. They can end in .gz to indicate file is in gzip format. + * @param translation_uid Unique uid for the translation file associated with the metadata. + * @param translation_uri The translations for strings within the metadata file. If null the either do not exist or may be included in the metadata file itself. The translations specified here supercede any which may be in the metadata file itself. The uri format is the same as component_metadata_uri . Files are in Json Translation spec format. Empty string indicates no tranlsation file. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_component_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint32_t metadata_type,uint32_t metadata_uid,const char *metadata_uri,uint32_t translation_uid,const char *translation_uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, metadata_type); + _mav_put_uint32_t(buf, 8, metadata_uid); + _mav_put_uint32_t(buf, 12, translation_uid); + _mav_put_char_array(buf, 16, metadata_uri, 70); + _mav_put_char_array(buf, 86, translation_uri, 70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); +#else + mavlink_component_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.metadata_type = metadata_type; + packet.metadata_uid = metadata_uid; + packet.translation_uid = translation_uid; + mav_array_memcpy(packet.metadata_uri, metadata_uri, sizeof(char)*70); + mav_array_memcpy(packet.translation_uri, translation_uri, sizeof(char)*70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMPONENT_INFORMATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); +} + +/** + * @brief Encode a component_information struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param component_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_component_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_component_information_t* component_information) +{ + return mavlink_msg_component_information_pack(system_id, component_id, msg, component_information->time_boot_ms, component_information->metadata_type, component_information->metadata_uid, component_information->metadata_uri, component_information->translation_uid, component_information->translation_uri); +} + +/** + * @brief Encode a component_information struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param component_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_component_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_component_information_t* component_information) +{ + return mavlink_msg_component_information_pack_chan(system_id, component_id, chan, msg, component_information->time_boot_ms, component_information->metadata_type, component_information->metadata_uid, component_information->metadata_uri, component_information->translation_uid, component_information->translation_uri); +} + +/** + * @brief Send a component_information message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param metadata_type The type of metadata being requested. + * @param metadata_uid Unique uid for this metadata which a gcs can use for created cached metadata and understanding whether it's cache it up to date or whether it needs to download new data. + * @param metadata_uri Component definition URI. If prefix mavlinkftp:// file is downloaded from vehicle over mavlink ftp protocol. If prefix http[s]:// file is downloaded over internet. Files are json format. They can end in .gz to indicate file is in gzip format. + * @param translation_uid Unique uid for the translation file associated with the metadata. + * @param translation_uri The translations for strings within the metadata file. If null the either do not exist or may be included in the metadata file itself. The translations specified here supercede any which may be in the metadata file itself. The uri format is the same as component_metadata_uri . Files are in Json Translation spec format. Empty string indicates no tranlsation file. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_component_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t metadata_type, uint32_t metadata_uid, const char *metadata_uri, uint32_t translation_uid, const char *translation_uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, metadata_type); + _mav_put_uint32_t(buf, 8, metadata_uid); + _mav_put_uint32_t(buf, 12, translation_uid); + _mav_put_char_array(buf, 16, metadata_uri, 70); + _mav_put_char_array(buf, 86, translation_uri, 70); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION, buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); +#else + mavlink_component_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.metadata_type = metadata_type; + packet.metadata_uid = metadata_uid; + packet.translation_uid = translation_uid; + mav_array_memcpy(packet.metadata_uri, metadata_uri, sizeof(char)*70); + mav_array_memcpy(packet.translation_uri, translation_uri, sizeof(char)*70); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); +#endif +} + +/** + * @brief Send a component_information message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_component_information_send_struct(mavlink_channel_t chan, const mavlink_component_information_t* component_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_component_information_send(chan, component_information->time_boot_ms, component_information->metadata_type, component_information->metadata_uid, component_information->metadata_uri, component_information->translation_uid, component_information->translation_uri); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION, (const char *)component_information, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_component_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t metadata_type, uint32_t metadata_uid, const char *metadata_uri, uint32_t translation_uid, const char *translation_uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, metadata_type); + _mav_put_uint32_t(buf, 8, metadata_uid); + _mav_put_uint32_t(buf, 12, translation_uid); + _mav_put_char_array(buf, 16, metadata_uri, 70); + _mav_put_char_array(buf, 86, translation_uri, 70); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION, buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); +#else + mavlink_component_information_t *packet = (mavlink_component_information_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->metadata_type = metadata_type; + packet->metadata_uid = metadata_uid; + packet->translation_uid = translation_uid; + mav_array_memcpy(packet->metadata_uri, metadata_uri, sizeof(char)*70); + mav_array_memcpy(packet->translation_uri, translation_uri, sizeof(char)*70); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE COMPONENT_INFORMATION UNPACKING + + +/** + * @brief Get field time_boot_ms from component_information message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_component_information_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field metadata_type from component_information message + * + * @return The type of metadata being requested. + */ +static inline uint32_t mavlink_msg_component_information_get_metadata_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field metadata_uid from component_information message + * + * @return Unique uid for this metadata which a gcs can use for created cached metadata and understanding whether it's cache it up to date or whether it needs to download new data. + */ +static inline uint32_t mavlink_msg_component_information_get_metadata_uid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field metadata_uri from component_information message + * + * @return Component definition URI. If prefix mavlinkftp:// file is downloaded from vehicle over mavlink ftp protocol. If prefix http[s]:// file is downloaded over internet. Files are json format. They can end in .gz to indicate file is in gzip format. + */ +static inline uint16_t mavlink_msg_component_information_get_metadata_uri(const mavlink_message_t* msg, char *metadata_uri) +{ + return _MAV_RETURN_char_array(msg, metadata_uri, 70, 16); +} + +/** + * @brief Get field translation_uid from component_information message + * + * @return Unique uid for the translation file associated with the metadata. + */ +static inline uint32_t mavlink_msg_component_information_get_translation_uid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 12); +} + +/** + * @brief Get field translation_uri from component_information message + * + * @return The translations for strings within the metadata file. If null the either do not exist or may be included in the metadata file itself. The translations specified here supercede any which may be in the metadata file itself. The uri format is the same as component_metadata_uri . Files are in Json Translation spec format. Empty string indicates no tranlsation file. + */ +static inline uint16_t mavlink_msg_component_information_get_translation_uri(const mavlink_message_t* msg, char *translation_uri) +{ + return _MAV_RETURN_char_array(msg, translation_uri, 70, 86); +} + +/** + * @brief Decode a component_information message into a struct + * + * @param msg The message to decode + * @param component_information C-struct to decode the message contents into + */ +static inline void mavlink_msg_component_information_decode(const mavlink_message_t* msg, mavlink_component_information_t* component_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + component_information->time_boot_ms = mavlink_msg_component_information_get_time_boot_ms(msg); + component_information->metadata_type = mavlink_msg_component_information_get_metadata_type(msg); + component_information->metadata_uid = mavlink_msg_component_information_get_metadata_uid(msg); + component_information->translation_uid = mavlink_msg_component_information_get_translation_uid(msg); + mavlink_msg_component_information_get_metadata_uri(msg, component_information->metadata_uri); + mavlink_msg_component_information_get_translation_uri(msg, component_information->translation_uri); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN; + memset(component_information, 0, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); + memcpy(component_information, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_debug_float_array.h b/lib/main/MAVLink/common/mavlink_msg_debug_float_array.h new file mode 100644 index 0000000000..c3a8b31fb8 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_debug_float_array.h @@ -0,0 +1,281 @@ +#pragma once +// MESSAGE DEBUG_FLOAT_ARRAY PACKING + +#define MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY 350 + + +typedef struct __mavlink_debug_float_array_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + uint16_t array_id; /*< Unique ID used to discriminate between arrays*/ + char name[10]; /*< Name, for human-friendly display in a Ground Control Station*/ + float data[58]; /*< data*/ +} mavlink_debug_float_array_t; + +#define MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN 252 +#define MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN 20 +#define MAVLINK_MSG_ID_350_LEN 252 +#define MAVLINK_MSG_ID_350_MIN_LEN 20 + +#define MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC 232 +#define MAVLINK_MSG_ID_350_CRC 232 + +#define MAVLINK_MSG_DEBUG_FLOAT_ARRAY_FIELD_NAME_LEN 10 +#define MAVLINK_MSG_DEBUG_FLOAT_ARRAY_FIELD_DATA_LEN 58 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_DEBUG_FLOAT_ARRAY { \ + 350, \ + "DEBUG_FLOAT_ARRAY", \ + 4, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_debug_float_array_t, time_usec) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 10, 10, offsetof(mavlink_debug_float_array_t, name) }, \ + { "array_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_debug_float_array_t, array_id) }, \ + { "data", NULL, MAVLINK_TYPE_FLOAT, 58, 20, offsetof(mavlink_debug_float_array_t, data) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_DEBUG_FLOAT_ARRAY { \ + "DEBUG_FLOAT_ARRAY", \ + 4, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_debug_float_array_t, time_usec) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 10, 10, offsetof(mavlink_debug_float_array_t, name) }, \ + { "array_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_debug_float_array_t, array_id) }, \ + { "data", NULL, MAVLINK_TYPE_FLOAT, 58, 20, offsetof(mavlink_debug_float_array_t, data) }, \ + } \ +} +#endif + +/** + * @brief Pack a debug_float_array message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param name Name, for human-friendly display in a Ground Control Station + * @param array_id Unique ID used to discriminate between arrays + * @param data data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_debug_float_array_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, const char *name, uint16_t array_id, const float *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, array_id); + _mav_put_char_array(buf, 10, name, 10); + _mav_put_float_array(buf, 20, data, 58); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN); +#else + mavlink_debug_float_array_t packet; + packet.time_usec = time_usec; + packet.array_id = array_id; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + mav_array_memcpy(packet.data, data, sizeof(float)*58); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); +} + +/** + * @brief Pack a debug_float_array message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param name Name, for human-friendly display in a Ground Control Station + * @param array_id Unique ID used to discriminate between arrays + * @param data data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_debug_float_array_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,const char *name,uint16_t array_id,const float *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, array_id); + _mav_put_char_array(buf, 10, name, 10); + _mav_put_float_array(buf, 20, data, 58); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN); +#else + mavlink_debug_float_array_t packet; + packet.time_usec = time_usec; + packet.array_id = array_id; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + mav_array_memcpy(packet.data, data, sizeof(float)*58); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); +} + +/** + * @brief Encode a debug_float_array struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param debug_float_array C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_float_array_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_debug_float_array_t* debug_float_array) +{ + return mavlink_msg_debug_float_array_pack(system_id, component_id, msg, debug_float_array->time_usec, debug_float_array->name, debug_float_array->array_id, debug_float_array->data); +} + +/** + * @brief Encode a debug_float_array struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param debug_float_array C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_float_array_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_debug_float_array_t* debug_float_array) +{ + return mavlink_msg_debug_float_array_pack_chan(system_id, component_id, chan, msg, debug_float_array->time_usec, debug_float_array->name, debug_float_array->array_id, debug_float_array->data); +} + +/** + * @brief Send a debug_float_array message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param name Name, for human-friendly display in a Ground Control Station + * @param array_id Unique ID used to discriminate between arrays + * @param data data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_debug_float_array_send(mavlink_channel_t chan, uint64_t time_usec, const char *name, uint16_t array_id, const float *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, array_id); + _mav_put_char_array(buf, 10, name, 10); + _mav_put_float_array(buf, 20, data, 58); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY, buf, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); +#else + mavlink_debug_float_array_t packet; + packet.time_usec = time_usec; + packet.array_id = array_id; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + mav_array_memcpy(packet.data, data, sizeof(float)*58); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY, (const char *)&packet, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); +#endif +} + +/** + * @brief Send a debug_float_array message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_debug_float_array_send_struct(mavlink_channel_t chan, const mavlink_debug_float_array_t* debug_float_array) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_debug_float_array_send(chan, debug_float_array->time_usec, debug_float_array->name, debug_float_array->array_id, debug_float_array->data); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY, (const char *)debug_float_array, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); +#endif +} + +#if MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_debug_float_array_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, const char *name, uint16_t array_id, const float *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, array_id); + _mav_put_char_array(buf, 10, name, 10); + _mav_put_float_array(buf, 20, data, 58); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY, buf, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); +#else + mavlink_debug_float_array_t *packet = (mavlink_debug_float_array_t *)msgbuf; + packet->time_usec = time_usec; + packet->array_id = array_id; + mav_array_memcpy(packet->name, name, sizeof(char)*10); + mav_array_memcpy(packet->data, data, sizeof(float)*58); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY, (const char *)packet, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); +#endif +} +#endif + +#endif + +// MESSAGE DEBUG_FLOAT_ARRAY UNPACKING + + +/** + * @brief Get field time_usec from debug_float_array message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_debug_float_array_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field name from debug_float_array message + * + * @return Name, for human-friendly display in a Ground Control Station + */ +static inline uint16_t mavlink_msg_debug_float_array_get_name(const mavlink_message_t* msg, char *name) +{ + return _MAV_RETURN_char_array(msg, name, 10, 10); +} + +/** + * @brief Get field array_id from debug_float_array message + * + * @return Unique ID used to discriminate between arrays + */ +static inline uint16_t mavlink_msg_debug_float_array_get_array_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field data from debug_float_array message + * + * @return data + */ +static inline uint16_t mavlink_msg_debug_float_array_get_data(const mavlink_message_t* msg, float *data) +{ + return _MAV_RETURN_float_array(msg, data, 58, 20); +} + +/** + * @brief Decode a debug_float_array message into a struct + * + * @param msg The message to decode + * @param debug_float_array C-struct to decode the message contents into + */ +static inline void mavlink_msg_debug_float_array_decode(const mavlink_message_t* msg, mavlink_debug_float_array_t* debug_float_array) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + debug_float_array->time_usec = mavlink_msg_debug_float_array_get_time_usec(msg); + debug_float_array->array_id = mavlink_msg_debug_float_array_get_array_id(msg); + mavlink_msg_debug_float_array_get_name(msg, debug_float_array->name); + mavlink_msg_debug_float_array_get_data(msg, debug_float_array->data); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN? msg->len : MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN; + memset(debug_float_array, 0, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN); + memcpy(debug_float_array, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_distance_sensor.h b/lib/main/MAVLink/common/mavlink_msg_distance_sensor.h index a5b0107be3..7e4e850408 100755 --- a/lib/main/MAVLink/common/mavlink_msg_distance_sensor.h +++ b/lib/main/MAVLink/common/mavlink_msg_distance_sensor.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_DISTANCE_SENSOR 132 - +MAVPACKED( typedef struct __mavlink_distance_sensor_t { uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ uint16_t min_distance; /*< [cm] Minimum distance the sensor can measure*/ @@ -13,23 +13,27 @@ typedef struct __mavlink_distance_sensor_t { uint8_t id; /*< Onboard ID of the sensor*/ uint8_t orientation; /*< Direction the sensor faces. downward-facing: ROTATION_PITCH_270, upward-facing: ROTATION_PITCH_90, backward-facing: ROTATION_PITCH_180, forward-facing: ROTATION_NONE, left-facing: ROTATION_YAW_90, right-facing: ROTATION_YAW_270*/ uint8_t covariance; /*< [cm^2] Measurement variance. Max standard deviation is 6cm. 255 if unknown.*/ -} mavlink_distance_sensor_t; + float horizontal_fov; /*< [rad] Horizontal Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0.*/ + float vertical_fov; /*< [rad] Vertical Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0.*/ + float quaternion[4]; /*< Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. Set it to 0 if invalid."*/ + uint8_t signal_quality; /*< [%] Signal quality of the sensor. Specific to each sensor type, representing the relation of the signal strength with the target reflectivity, distance, size or aspect, but normalised as a percentage. 0 = unknown/unset signal quality, 1 = invalid signal, 100 = perfect signal.*/ +}) mavlink_distance_sensor_t; -#define MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN 14 +#define MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN 39 #define MAVLINK_MSG_ID_DISTANCE_SENSOR_MIN_LEN 14 -#define MAVLINK_MSG_ID_132_LEN 14 +#define MAVLINK_MSG_ID_132_LEN 39 #define MAVLINK_MSG_ID_132_MIN_LEN 14 #define MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC 85 #define MAVLINK_MSG_ID_132_CRC 85 - +#define MAVLINK_MSG_DISTANCE_SENSOR_FIELD_QUATERNION_LEN 4 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR { \ 132, \ "DISTANCE_SENSOR", \ - 8, \ + 12, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_distance_sensor_t, time_boot_ms) }, \ { "min_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_distance_sensor_t, min_distance) }, \ { "max_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_distance_sensor_t, max_distance) }, \ @@ -38,12 +42,16 @@ typedef struct __mavlink_distance_sensor_t { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_distance_sensor_t, id) }, \ { "orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_distance_sensor_t, orientation) }, \ { "covariance", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_distance_sensor_t, covariance) }, \ + { "horizontal_fov", NULL, MAVLINK_TYPE_FLOAT, 0, 14, offsetof(mavlink_distance_sensor_t, horizontal_fov) }, \ + { "vertical_fov", NULL, MAVLINK_TYPE_FLOAT, 0, 18, offsetof(mavlink_distance_sensor_t, vertical_fov) }, \ + { "quaternion", NULL, MAVLINK_TYPE_FLOAT, 4, 22, offsetof(mavlink_distance_sensor_t, quaternion) }, \ + { "signal_quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_distance_sensor_t, signal_quality) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR { \ "DISTANCE_SENSOR", \ - 8, \ + 12, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_distance_sensor_t, time_boot_ms) }, \ { "min_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_distance_sensor_t, min_distance) }, \ { "max_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_distance_sensor_t, max_distance) }, \ @@ -52,6 +60,10 @@ typedef struct __mavlink_distance_sensor_t { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_distance_sensor_t, id) }, \ { "orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_distance_sensor_t, orientation) }, \ { "covariance", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_distance_sensor_t, covariance) }, \ + { "horizontal_fov", NULL, MAVLINK_TYPE_FLOAT, 0, 14, offsetof(mavlink_distance_sensor_t, horizontal_fov) }, \ + { "vertical_fov", NULL, MAVLINK_TYPE_FLOAT, 0, 18, offsetof(mavlink_distance_sensor_t, vertical_fov) }, \ + { "quaternion", NULL, MAVLINK_TYPE_FLOAT, 4, 22, offsetof(mavlink_distance_sensor_t, quaternion) }, \ + { "signal_quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_distance_sensor_t, signal_quality) }, \ } \ } #endif @@ -70,10 +82,14 @@ typedef struct __mavlink_distance_sensor_t { * @param id Onboard ID of the sensor * @param orientation Direction the sensor faces. downward-facing: ROTATION_PITCH_270, upward-facing: ROTATION_PITCH_90, backward-facing: ROTATION_PITCH_180, forward-facing: ROTATION_NONE, left-facing: ROTATION_YAW_90, right-facing: ROTATION_YAW_270 * @param covariance [cm^2] Measurement variance. Max standard deviation is 6cm. 255 if unknown. + * @param horizontal_fov [rad] Horizontal Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + * @param vertical_fov [rad] Vertical Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + * @param quaternion Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. Set it to 0 if invalid." + * @param signal_quality [%] Signal quality of the sensor. Specific to each sensor type, representing the relation of the signal strength with the target reflectivity, distance, size or aspect, but normalised as a percentage. 0 = unknown/unset signal quality, 1 = invalid signal, 100 = perfect signal. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance) + uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance, float horizontal_fov, float vertical_fov, const float *quaternion, uint8_t signal_quality) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; @@ -85,7 +101,10 @@ static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8 _mav_put_uint8_t(buf, 11, id); _mav_put_uint8_t(buf, 12, orientation); _mav_put_uint8_t(buf, 13, covariance); - + _mav_put_float(buf, 14, horizontal_fov); + _mav_put_float(buf, 18, vertical_fov); + _mav_put_uint8_t(buf, 38, signal_quality); + _mav_put_float_array(buf, 22, quaternion, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); #else mavlink_distance_sensor_t packet; @@ -97,7 +116,10 @@ static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8 packet.id = id; packet.orientation = orientation; packet.covariance = covariance; - + packet.horizontal_fov = horizontal_fov; + packet.vertical_fov = vertical_fov; + packet.signal_quality = signal_quality; + mav_array_memcpy(packet.quaternion, quaternion, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); #endif @@ -119,11 +141,15 @@ static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8 * @param id Onboard ID of the sensor * @param orientation Direction the sensor faces. downward-facing: ROTATION_PITCH_270, upward-facing: ROTATION_PITCH_90, backward-facing: ROTATION_PITCH_180, forward-facing: ROTATION_NONE, left-facing: ROTATION_YAW_90, right-facing: ROTATION_YAW_270 * @param covariance [cm^2] Measurement variance. Max standard deviation is 6cm. 255 if unknown. + * @param horizontal_fov [rad] Horizontal Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + * @param vertical_fov [rad] Vertical Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + * @param quaternion Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. Set it to 0 if invalid." + * @param signal_quality [%] Signal quality of the sensor. Specific to each sensor type, representing the relation of the signal strength with the target reflectivity, distance, size or aspect, but normalised as a percentage. 0 = unknown/unset signal quality, 1 = invalid signal, 100 = perfect signal. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_distance_sensor_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,uint16_t min_distance,uint16_t max_distance,uint16_t current_distance,uint8_t type,uint8_t id,uint8_t orientation,uint8_t covariance) + uint32_t time_boot_ms,uint16_t min_distance,uint16_t max_distance,uint16_t current_distance,uint8_t type,uint8_t id,uint8_t orientation,uint8_t covariance,float horizontal_fov,float vertical_fov,const float *quaternion,uint8_t signal_quality) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; @@ -135,7 +161,10 @@ static inline uint16_t mavlink_msg_distance_sensor_pack_chan(uint8_t system_id, _mav_put_uint8_t(buf, 11, id); _mav_put_uint8_t(buf, 12, orientation); _mav_put_uint8_t(buf, 13, covariance); - + _mav_put_float(buf, 14, horizontal_fov); + _mav_put_float(buf, 18, vertical_fov); + _mav_put_uint8_t(buf, 38, signal_quality); + _mav_put_float_array(buf, 22, quaternion, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); #else mavlink_distance_sensor_t packet; @@ -147,7 +176,10 @@ static inline uint16_t mavlink_msg_distance_sensor_pack_chan(uint8_t system_id, packet.id = id; packet.orientation = orientation; packet.covariance = covariance; - + packet.horizontal_fov = horizontal_fov; + packet.vertical_fov = vertical_fov; + packet.signal_quality = signal_quality; + mav_array_memcpy(packet.quaternion, quaternion, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); #endif @@ -165,7 +197,7 @@ static inline uint16_t mavlink_msg_distance_sensor_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_distance_sensor_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor) { - return mavlink_msg_distance_sensor_pack(system_id, component_id, msg, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance); + return mavlink_msg_distance_sensor_pack(system_id, component_id, msg, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance, distance_sensor->horizontal_fov, distance_sensor->vertical_fov, distance_sensor->quaternion, distance_sensor->signal_quality); } /** @@ -179,7 +211,7 @@ static inline uint16_t mavlink_msg_distance_sensor_encode(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_distance_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor) { - return mavlink_msg_distance_sensor_pack_chan(system_id, component_id, chan, msg, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance); + return mavlink_msg_distance_sensor_pack_chan(system_id, component_id, chan, msg, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance, distance_sensor->horizontal_fov, distance_sensor->vertical_fov, distance_sensor->quaternion, distance_sensor->signal_quality); } /** @@ -194,10 +226,14 @@ static inline uint16_t mavlink_msg_distance_sensor_encode_chan(uint8_t system_id * @param id Onboard ID of the sensor * @param orientation Direction the sensor faces. downward-facing: ROTATION_PITCH_270, upward-facing: ROTATION_PITCH_90, backward-facing: ROTATION_PITCH_180, forward-facing: ROTATION_NONE, left-facing: ROTATION_YAW_90, right-facing: ROTATION_YAW_270 * @param covariance [cm^2] Measurement variance. Max standard deviation is 6cm. 255 if unknown. + * @param horizontal_fov [rad] Horizontal Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + * @param vertical_fov [rad] Vertical Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + * @param quaternion Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. Set it to 0 if invalid." + * @param signal_quality [%] Signal quality of the sensor. Specific to each sensor type, representing the relation of the signal strength with the target reflectivity, distance, size or aspect, but normalised as a percentage. 0 = unknown/unset signal quality, 1 = invalid signal, 100 = perfect signal. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance) +static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance, float horizontal_fov, float vertical_fov, const float *quaternion, uint8_t signal_quality) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; @@ -209,7 +245,10 @@ static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint _mav_put_uint8_t(buf, 11, id); _mav_put_uint8_t(buf, 12, orientation); _mav_put_uint8_t(buf, 13, covariance); - + _mav_put_float(buf, 14, horizontal_fov); + _mav_put_float(buf, 18, vertical_fov); + _mav_put_uint8_t(buf, 38, signal_quality); + _mav_put_float_array(buf, 22, quaternion, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_MIN_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); #else mavlink_distance_sensor_t packet; @@ -221,7 +260,10 @@ static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint packet.id = id; packet.orientation = orientation; packet.covariance = covariance; - + packet.horizontal_fov = horizontal_fov; + packet.vertical_fov = vertical_fov; + packet.signal_quality = signal_quality; + mav_array_memcpy(packet.quaternion, quaternion, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_MIN_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); #endif } @@ -234,7 +276,7 @@ static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint static inline void mavlink_msg_distance_sensor_send_struct(mavlink_channel_t chan, const mavlink_distance_sensor_t* distance_sensor) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_distance_sensor_send(chan, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance); + mavlink_msg_distance_sensor_send(chan, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance, distance_sensor->horizontal_fov, distance_sensor->vertical_fov, distance_sensor->quaternion, distance_sensor->signal_quality); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)distance_sensor, MAVLINK_MSG_ID_DISTANCE_SENSOR_MIN_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); #endif @@ -248,7 +290,7 @@ static inline void mavlink_msg_distance_sensor_send_struct(mavlink_channel_t cha is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_distance_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance) +static inline void mavlink_msg_distance_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance, float horizontal_fov, float vertical_fov, const float *quaternion, uint8_t signal_quality) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -260,7 +302,10 @@ static inline void mavlink_msg_distance_sensor_send_buf(mavlink_message_t *msgbu _mav_put_uint8_t(buf, 11, id); _mav_put_uint8_t(buf, 12, orientation); _mav_put_uint8_t(buf, 13, covariance); - + _mav_put_float(buf, 14, horizontal_fov); + _mav_put_float(buf, 18, vertical_fov); + _mav_put_uint8_t(buf, 38, signal_quality); + _mav_put_float_array(buf, 22, quaternion, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_MIN_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); #else mavlink_distance_sensor_t *packet = (mavlink_distance_sensor_t *)msgbuf; @@ -272,7 +317,10 @@ static inline void mavlink_msg_distance_sensor_send_buf(mavlink_message_t *msgbu packet->id = id; packet->orientation = orientation; packet->covariance = covariance; - + packet->horizontal_fov = horizontal_fov; + packet->vertical_fov = vertical_fov; + packet->signal_quality = signal_quality; + mav_array_memcpy(packet->quaternion, quaternion, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_MIN_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); #endif } @@ -363,6 +411,46 @@ static inline uint8_t mavlink_msg_distance_sensor_get_covariance(const mavlink_m return _MAV_RETURN_uint8_t(msg, 13); } +/** + * @brief Get field horizontal_fov from distance_sensor message + * + * @return [rad] Horizontal Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + */ +static inline float mavlink_msg_distance_sensor_get_horizontal_fov(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 14); +} + +/** + * @brief Get field vertical_fov from distance_sensor message + * + * @return [rad] Vertical Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + */ +static inline float mavlink_msg_distance_sensor_get_vertical_fov(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 18); +} + +/** + * @brief Get field quaternion from distance_sensor message + * + * @return Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. Set it to 0 if invalid." + */ +static inline uint16_t mavlink_msg_distance_sensor_get_quaternion(const mavlink_message_t* msg, float *quaternion) +{ + return _MAV_RETURN_float_array(msg, quaternion, 4, 22); +} + +/** + * @brief Get field signal_quality from distance_sensor message + * + * @return [%] Signal quality of the sensor. Specific to each sensor type, representing the relation of the signal strength with the target reflectivity, distance, size or aspect, but normalised as a percentage. 0 = unknown/unset signal quality, 1 = invalid signal, 100 = perfect signal. + */ +static inline uint8_t mavlink_msg_distance_sensor_get_signal_quality(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 38); +} + /** * @brief Decode a distance_sensor message into a struct * @@ -380,6 +468,10 @@ static inline void mavlink_msg_distance_sensor_decode(const mavlink_message_t* m distance_sensor->id = mavlink_msg_distance_sensor_get_id(msg); distance_sensor->orientation = mavlink_msg_distance_sensor_get_orientation(msg); distance_sensor->covariance = mavlink_msg_distance_sensor_get_covariance(msg); + distance_sensor->horizontal_fov = mavlink_msg_distance_sensor_get_horizontal_fov(msg); + distance_sensor->vertical_fov = mavlink_msg_distance_sensor_get_vertical_fov(msg); + mavlink_msg_distance_sensor_get_quaternion(msg, distance_sensor->quaternion); + distance_sensor->signal_quality = mavlink_msg_distance_sensor_get_signal_quality(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN? msg->len : MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN; memset(distance_sensor, 0, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_efi_status.h b/lib/main/MAVLink/common/mavlink_msg_efi_status.h new file mode 100644 index 0000000000..d631024044 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_efi_status.h @@ -0,0 +1,613 @@ +#pragma once +// MESSAGE EFI_STATUS PACKING + +#define MAVLINK_MSG_ID_EFI_STATUS 225 + + +typedef struct __mavlink_efi_status_t { + float ecu_index; /*< ECU index*/ + float rpm; /*< RPM*/ + float fuel_consumed; /*< [cm^3] Fuel consumed*/ + float fuel_flow; /*< [cm^3/min] Fuel flow rate*/ + float engine_load; /*< [%] Engine load*/ + float throttle_position; /*< [%] Throttle position*/ + float spark_dwell_time; /*< [ms] Spark dwell time*/ + float barometric_pressure; /*< [kPa] Barometric pressure*/ + float intake_manifold_pressure; /*< [kPa] Intake manifold pressure(*/ + float intake_manifold_temperature; /*< [degC] Intake manifold temperature*/ + float cylinder_head_temperature; /*< [degC] Cylinder head temperature*/ + float ignition_timing; /*< [deg] Ignition timing (Crank angle degrees)*/ + float injection_time; /*< [ms] Injection time*/ + float exhaust_gas_temperature; /*< [degC] Exhaust gas temperature*/ + float throttle_out; /*< [%] Output throttle*/ + float pt_compensation; /*< Pressure/temperature compensation*/ + uint8_t health; /*< EFI health status*/ +} mavlink_efi_status_t; + +#define MAVLINK_MSG_ID_EFI_STATUS_LEN 65 +#define MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN 65 +#define MAVLINK_MSG_ID_225_LEN 65 +#define MAVLINK_MSG_ID_225_MIN_LEN 65 + +#define MAVLINK_MSG_ID_EFI_STATUS_CRC 208 +#define MAVLINK_MSG_ID_225_CRC 208 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_EFI_STATUS { \ + 225, \ + "EFI_STATUS", \ + 17, \ + { { "health", NULL, MAVLINK_TYPE_UINT8_T, 0, 64, offsetof(mavlink_efi_status_t, health) }, \ + { "ecu_index", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_efi_status_t, ecu_index) }, \ + { "rpm", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_efi_status_t, rpm) }, \ + { "fuel_consumed", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_efi_status_t, fuel_consumed) }, \ + { "fuel_flow", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_efi_status_t, fuel_flow) }, \ + { "engine_load", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_efi_status_t, engine_load) }, \ + { "throttle_position", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_efi_status_t, throttle_position) }, \ + { "spark_dwell_time", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_efi_status_t, spark_dwell_time) }, \ + { "barometric_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_efi_status_t, barometric_pressure) }, \ + { "intake_manifold_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_efi_status_t, intake_manifold_pressure) }, \ + { "intake_manifold_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_efi_status_t, intake_manifold_temperature) }, \ + { "cylinder_head_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_efi_status_t, cylinder_head_temperature) }, \ + { "ignition_timing", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_efi_status_t, ignition_timing) }, \ + { "injection_time", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_efi_status_t, injection_time) }, \ + { "exhaust_gas_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_efi_status_t, exhaust_gas_temperature) }, \ + { "throttle_out", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_efi_status_t, throttle_out) }, \ + { "pt_compensation", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_efi_status_t, pt_compensation) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_EFI_STATUS { \ + "EFI_STATUS", \ + 17, \ + { { "health", NULL, MAVLINK_TYPE_UINT8_T, 0, 64, offsetof(mavlink_efi_status_t, health) }, \ + { "ecu_index", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_efi_status_t, ecu_index) }, \ + { "rpm", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_efi_status_t, rpm) }, \ + { "fuel_consumed", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_efi_status_t, fuel_consumed) }, \ + { "fuel_flow", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_efi_status_t, fuel_flow) }, \ + { "engine_load", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_efi_status_t, engine_load) }, \ + { "throttle_position", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_efi_status_t, throttle_position) }, \ + { "spark_dwell_time", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_efi_status_t, spark_dwell_time) }, \ + { "barometric_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_efi_status_t, barometric_pressure) }, \ + { "intake_manifold_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_efi_status_t, intake_manifold_pressure) }, \ + { "intake_manifold_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_efi_status_t, intake_manifold_temperature) }, \ + { "cylinder_head_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_efi_status_t, cylinder_head_temperature) }, \ + { "ignition_timing", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_efi_status_t, ignition_timing) }, \ + { "injection_time", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_efi_status_t, injection_time) }, \ + { "exhaust_gas_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_efi_status_t, exhaust_gas_temperature) }, \ + { "throttle_out", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_efi_status_t, throttle_out) }, \ + { "pt_compensation", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_efi_status_t, pt_compensation) }, \ + } \ +} +#endif + +/** + * @brief Pack a efi_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param health EFI health status + * @param ecu_index ECU index + * @param rpm RPM + * @param fuel_consumed [cm^3] Fuel consumed + * @param fuel_flow [cm^3/min] Fuel flow rate + * @param engine_load [%] Engine load + * @param throttle_position [%] Throttle position + * @param spark_dwell_time [ms] Spark dwell time + * @param barometric_pressure [kPa] Barometric pressure + * @param intake_manifold_pressure [kPa] Intake manifold pressure( + * @param intake_manifold_temperature [degC] Intake manifold temperature + * @param cylinder_head_temperature [degC] Cylinder head temperature + * @param ignition_timing [deg] Ignition timing (Crank angle degrees) + * @param injection_time [ms] Injection time + * @param exhaust_gas_temperature [degC] Exhaust gas temperature + * @param throttle_out [%] Output throttle + * @param pt_compensation Pressure/temperature compensation + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_efi_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t health, float ecu_index, float rpm, float fuel_consumed, float fuel_flow, float engine_load, float throttle_position, float spark_dwell_time, float barometric_pressure, float intake_manifold_pressure, float intake_manifold_temperature, float cylinder_head_temperature, float ignition_timing, float injection_time, float exhaust_gas_temperature, float throttle_out, float pt_compensation) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_EFI_STATUS_LEN]; + _mav_put_float(buf, 0, ecu_index); + _mav_put_float(buf, 4, rpm); + _mav_put_float(buf, 8, fuel_consumed); + _mav_put_float(buf, 12, fuel_flow); + _mav_put_float(buf, 16, engine_load); + _mav_put_float(buf, 20, throttle_position); + _mav_put_float(buf, 24, spark_dwell_time); + _mav_put_float(buf, 28, barometric_pressure); + _mav_put_float(buf, 32, intake_manifold_pressure); + _mav_put_float(buf, 36, intake_manifold_temperature); + _mav_put_float(buf, 40, cylinder_head_temperature); + _mav_put_float(buf, 44, ignition_timing); + _mav_put_float(buf, 48, injection_time); + _mav_put_float(buf, 52, exhaust_gas_temperature); + _mav_put_float(buf, 56, throttle_out); + _mav_put_float(buf, 60, pt_compensation); + _mav_put_uint8_t(buf, 64, health); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_EFI_STATUS_LEN); +#else + mavlink_efi_status_t packet; + packet.ecu_index = ecu_index; + packet.rpm = rpm; + packet.fuel_consumed = fuel_consumed; + packet.fuel_flow = fuel_flow; + packet.engine_load = engine_load; + packet.throttle_position = throttle_position; + packet.spark_dwell_time = spark_dwell_time; + packet.barometric_pressure = barometric_pressure; + packet.intake_manifold_pressure = intake_manifold_pressure; + packet.intake_manifold_temperature = intake_manifold_temperature; + packet.cylinder_head_temperature = cylinder_head_temperature; + packet.ignition_timing = ignition_timing; + packet.injection_time = injection_time; + packet.exhaust_gas_temperature = exhaust_gas_temperature; + packet.throttle_out = throttle_out; + packet.pt_compensation = pt_compensation; + packet.health = health; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_EFI_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_EFI_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); +} + +/** + * @brief Pack a efi_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param health EFI health status + * @param ecu_index ECU index + * @param rpm RPM + * @param fuel_consumed [cm^3] Fuel consumed + * @param fuel_flow [cm^3/min] Fuel flow rate + * @param engine_load [%] Engine load + * @param throttle_position [%] Throttle position + * @param spark_dwell_time [ms] Spark dwell time + * @param barometric_pressure [kPa] Barometric pressure + * @param intake_manifold_pressure [kPa] Intake manifold pressure( + * @param intake_manifold_temperature [degC] Intake manifold temperature + * @param cylinder_head_temperature [degC] Cylinder head temperature + * @param ignition_timing [deg] Ignition timing (Crank angle degrees) + * @param injection_time [ms] Injection time + * @param exhaust_gas_temperature [degC] Exhaust gas temperature + * @param throttle_out [%] Output throttle + * @param pt_compensation Pressure/temperature compensation + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_efi_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t health,float ecu_index,float rpm,float fuel_consumed,float fuel_flow,float engine_load,float throttle_position,float spark_dwell_time,float barometric_pressure,float intake_manifold_pressure,float intake_manifold_temperature,float cylinder_head_temperature,float ignition_timing,float injection_time,float exhaust_gas_temperature,float throttle_out,float pt_compensation) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_EFI_STATUS_LEN]; + _mav_put_float(buf, 0, ecu_index); + _mav_put_float(buf, 4, rpm); + _mav_put_float(buf, 8, fuel_consumed); + _mav_put_float(buf, 12, fuel_flow); + _mav_put_float(buf, 16, engine_load); + _mav_put_float(buf, 20, throttle_position); + _mav_put_float(buf, 24, spark_dwell_time); + _mav_put_float(buf, 28, barometric_pressure); + _mav_put_float(buf, 32, intake_manifold_pressure); + _mav_put_float(buf, 36, intake_manifold_temperature); + _mav_put_float(buf, 40, cylinder_head_temperature); + _mav_put_float(buf, 44, ignition_timing); + _mav_put_float(buf, 48, injection_time); + _mav_put_float(buf, 52, exhaust_gas_temperature); + _mav_put_float(buf, 56, throttle_out); + _mav_put_float(buf, 60, pt_compensation); + _mav_put_uint8_t(buf, 64, health); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_EFI_STATUS_LEN); +#else + mavlink_efi_status_t packet; + packet.ecu_index = ecu_index; + packet.rpm = rpm; + packet.fuel_consumed = fuel_consumed; + packet.fuel_flow = fuel_flow; + packet.engine_load = engine_load; + packet.throttle_position = throttle_position; + packet.spark_dwell_time = spark_dwell_time; + packet.barometric_pressure = barometric_pressure; + packet.intake_manifold_pressure = intake_manifold_pressure; + packet.intake_manifold_temperature = intake_manifold_temperature; + packet.cylinder_head_temperature = cylinder_head_temperature; + packet.ignition_timing = ignition_timing; + packet.injection_time = injection_time; + packet.exhaust_gas_temperature = exhaust_gas_temperature; + packet.throttle_out = throttle_out; + packet.pt_compensation = pt_compensation; + packet.health = health; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_EFI_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_EFI_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); +} + +/** + * @brief Encode a efi_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param efi_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_efi_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_efi_status_t* efi_status) +{ + return mavlink_msg_efi_status_pack(system_id, component_id, msg, efi_status->health, efi_status->ecu_index, efi_status->rpm, efi_status->fuel_consumed, efi_status->fuel_flow, efi_status->engine_load, efi_status->throttle_position, efi_status->spark_dwell_time, efi_status->barometric_pressure, efi_status->intake_manifold_pressure, efi_status->intake_manifold_temperature, efi_status->cylinder_head_temperature, efi_status->ignition_timing, efi_status->injection_time, efi_status->exhaust_gas_temperature, efi_status->throttle_out, efi_status->pt_compensation); +} + +/** + * @brief Encode a efi_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param efi_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_efi_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_efi_status_t* efi_status) +{ + return mavlink_msg_efi_status_pack_chan(system_id, component_id, chan, msg, efi_status->health, efi_status->ecu_index, efi_status->rpm, efi_status->fuel_consumed, efi_status->fuel_flow, efi_status->engine_load, efi_status->throttle_position, efi_status->spark_dwell_time, efi_status->barometric_pressure, efi_status->intake_manifold_pressure, efi_status->intake_manifold_temperature, efi_status->cylinder_head_temperature, efi_status->ignition_timing, efi_status->injection_time, efi_status->exhaust_gas_temperature, efi_status->throttle_out, efi_status->pt_compensation); +} + +/** + * @brief Send a efi_status message + * @param chan MAVLink channel to send the message + * + * @param health EFI health status + * @param ecu_index ECU index + * @param rpm RPM + * @param fuel_consumed [cm^3] Fuel consumed + * @param fuel_flow [cm^3/min] Fuel flow rate + * @param engine_load [%] Engine load + * @param throttle_position [%] Throttle position + * @param spark_dwell_time [ms] Spark dwell time + * @param barometric_pressure [kPa] Barometric pressure + * @param intake_manifold_pressure [kPa] Intake manifold pressure( + * @param intake_manifold_temperature [degC] Intake manifold temperature + * @param cylinder_head_temperature [degC] Cylinder head temperature + * @param ignition_timing [deg] Ignition timing (Crank angle degrees) + * @param injection_time [ms] Injection time + * @param exhaust_gas_temperature [degC] Exhaust gas temperature + * @param throttle_out [%] Output throttle + * @param pt_compensation Pressure/temperature compensation + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_efi_status_send(mavlink_channel_t chan, uint8_t health, float ecu_index, float rpm, float fuel_consumed, float fuel_flow, float engine_load, float throttle_position, float spark_dwell_time, float barometric_pressure, float intake_manifold_pressure, float intake_manifold_temperature, float cylinder_head_temperature, float ignition_timing, float injection_time, float exhaust_gas_temperature, float throttle_out, float pt_compensation) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_EFI_STATUS_LEN]; + _mav_put_float(buf, 0, ecu_index); + _mav_put_float(buf, 4, rpm); + _mav_put_float(buf, 8, fuel_consumed); + _mav_put_float(buf, 12, fuel_flow); + _mav_put_float(buf, 16, engine_load); + _mav_put_float(buf, 20, throttle_position); + _mav_put_float(buf, 24, spark_dwell_time); + _mav_put_float(buf, 28, barometric_pressure); + _mav_put_float(buf, 32, intake_manifold_pressure); + _mav_put_float(buf, 36, intake_manifold_temperature); + _mav_put_float(buf, 40, cylinder_head_temperature); + _mav_put_float(buf, 44, ignition_timing); + _mav_put_float(buf, 48, injection_time); + _mav_put_float(buf, 52, exhaust_gas_temperature); + _mav_put_float(buf, 56, throttle_out); + _mav_put_float(buf, 60, pt_compensation); + _mav_put_uint8_t(buf, 64, health); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EFI_STATUS, buf, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); +#else + mavlink_efi_status_t packet; + packet.ecu_index = ecu_index; + packet.rpm = rpm; + packet.fuel_consumed = fuel_consumed; + packet.fuel_flow = fuel_flow; + packet.engine_load = engine_load; + packet.throttle_position = throttle_position; + packet.spark_dwell_time = spark_dwell_time; + packet.barometric_pressure = barometric_pressure; + packet.intake_manifold_pressure = intake_manifold_pressure; + packet.intake_manifold_temperature = intake_manifold_temperature; + packet.cylinder_head_temperature = cylinder_head_temperature; + packet.ignition_timing = ignition_timing; + packet.injection_time = injection_time; + packet.exhaust_gas_temperature = exhaust_gas_temperature; + packet.throttle_out = throttle_out; + packet.pt_compensation = pt_compensation; + packet.health = health; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EFI_STATUS, (const char *)&packet, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); +#endif +} + +/** + * @brief Send a efi_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_efi_status_send_struct(mavlink_channel_t chan, const mavlink_efi_status_t* efi_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_efi_status_send(chan, efi_status->health, efi_status->ecu_index, efi_status->rpm, efi_status->fuel_consumed, efi_status->fuel_flow, efi_status->engine_load, efi_status->throttle_position, efi_status->spark_dwell_time, efi_status->barometric_pressure, efi_status->intake_manifold_pressure, efi_status->intake_manifold_temperature, efi_status->cylinder_head_temperature, efi_status->ignition_timing, efi_status->injection_time, efi_status->exhaust_gas_temperature, efi_status->throttle_out, efi_status->pt_compensation); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EFI_STATUS, (const char *)efi_status, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_EFI_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_efi_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t health, float ecu_index, float rpm, float fuel_consumed, float fuel_flow, float engine_load, float throttle_position, float spark_dwell_time, float barometric_pressure, float intake_manifold_pressure, float intake_manifold_temperature, float cylinder_head_temperature, float ignition_timing, float injection_time, float exhaust_gas_temperature, float throttle_out, float pt_compensation) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, ecu_index); + _mav_put_float(buf, 4, rpm); + _mav_put_float(buf, 8, fuel_consumed); + _mav_put_float(buf, 12, fuel_flow); + _mav_put_float(buf, 16, engine_load); + _mav_put_float(buf, 20, throttle_position); + _mav_put_float(buf, 24, spark_dwell_time); + _mav_put_float(buf, 28, barometric_pressure); + _mav_put_float(buf, 32, intake_manifold_pressure); + _mav_put_float(buf, 36, intake_manifold_temperature); + _mav_put_float(buf, 40, cylinder_head_temperature); + _mav_put_float(buf, 44, ignition_timing); + _mav_put_float(buf, 48, injection_time); + _mav_put_float(buf, 52, exhaust_gas_temperature); + _mav_put_float(buf, 56, throttle_out); + _mav_put_float(buf, 60, pt_compensation); + _mav_put_uint8_t(buf, 64, health); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EFI_STATUS, buf, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); +#else + mavlink_efi_status_t *packet = (mavlink_efi_status_t *)msgbuf; + packet->ecu_index = ecu_index; + packet->rpm = rpm; + packet->fuel_consumed = fuel_consumed; + packet->fuel_flow = fuel_flow; + packet->engine_load = engine_load; + packet->throttle_position = throttle_position; + packet->spark_dwell_time = spark_dwell_time; + packet->barometric_pressure = barometric_pressure; + packet->intake_manifold_pressure = intake_manifold_pressure; + packet->intake_manifold_temperature = intake_manifold_temperature; + packet->cylinder_head_temperature = cylinder_head_temperature; + packet->ignition_timing = ignition_timing; + packet->injection_time = injection_time; + packet->exhaust_gas_temperature = exhaust_gas_temperature; + packet->throttle_out = throttle_out; + packet->pt_compensation = pt_compensation; + packet->health = health; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EFI_STATUS, (const char *)packet, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE EFI_STATUS UNPACKING + + +/** + * @brief Get field health from efi_status message + * + * @return EFI health status + */ +static inline uint8_t mavlink_msg_efi_status_get_health(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 64); +} + +/** + * @brief Get field ecu_index from efi_status message + * + * @return ECU index + */ +static inline float mavlink_msg_efi_status_get_ecu_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field rpm from efi_status message + * + * @return RPM + */ +static inline float mavlink_msg_efi_status_get_rpm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field fuel_consumed from efi_status message + * + * @return [cm^3] Fuel consumed + */ +static inline float mavlink_msg_efi_status_get_fuel_consumed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field fuel_flow from efi_status message + * + * @return [cm^3/min] Fuel flow rate + */ +static inline float mavlink_msg_efi_status_get_fuel_flow(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field engine_load from efi_status message + * + * @return [%] Engine load + */ +static inline float mavlink_msg_efi_status_get_engine_load(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field throttle_position from efi_status message + * + * @return [%] Throttle position + */ +static inline float mavlink_msg_efi_status_get_throttle_position(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field spark_dwell_time from efi_status message + * + * @return [ms] Spark dwell time + */ +static inline float mavlink_msg_efi_status_get_spark_dwell_time(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field barometric_pressure from efi_status message + * + * @return [kPa] Barometric pressure + */ +static inline float mavlink_msg_efi_status_get_barometric_pressure(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field intake_manifold_pressure from efi_status message + * + * @return [kPa] Intake manifold pressure( + */ +static inline float mavlink_msg_efi_status_get_intake_manifold_pressure(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field intake_manifold_temperature from efi_status message + * + * @return [degC] Intake manifold temperature + */ +static inline float mavlink_msg_efi_status_get_intake_manifold_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field cylinder_head_temperature from efi_status message + * + * @return [degC] Cylinder head temperature + */ +static inline float mavlink_msg_efi_status_get_cylinder_head_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field ignition_timing from efi_status message + * + * @return [deg] Ignition timing (Crank angle degrees) + */ +static inline float mavlink_msg_efi_status_get_ignition_timing(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field injection_time from efi_status message + * + * @return [ms] Injection time + */ +static inline float mavlink_msg_efi_status_get_injection_time(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 48); +} + +/** + * @brief Get field exhaust_gas_temperature from efi_status message + * + * @return [degC] Exhaust gas temperature + */ +static inline float mavlink_msg_efi_status_get_exhaust_gas_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 52); +} + +/** + * @brief Get field throttle_out from efi_status message + * + * @return [%] Output throttle + */ +static inline float mavlink_msg_efi_status_get_throttle_out(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 56); +} + +/** + * @brief Get field pt_compensation from efi_status message + * + * @return Pressure/temperature compensation + */ +static inline float mavlink_msg_efi_status_get_pt_compensation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 60); +} + +/** + * @brief Decode a efi_status message into a struct + * + * @param msg The message to decode + * @param efi_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_efi_status_decode(const mavlink_message_t* msg, mavlink_efi_status_t* efi_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + efi_status->ecu_index = mavlink_msg_efi_status_get_ecu_index(msg); + efi_status->rpm = mavlink_msg_efi_status_get_rpm(msg); + efi_status->fuel_consumed = mavlink_msg_efi_status_get_fuel_consumed(msg); + efi_status->fuel_flow = mavlink_msg_efi_status_get_fuel_flow(msg); + efi_status->engine_load = mavlink_msg_efi_status_get_engine_load(msg); + efi_status->throttle_position = mavlink_msg_efi_status_get_throttle_position(msg); + efi_status->spark_dwell_time = mavlink_msg_efi_status_get_spark_dwell_time(msg); + efi_status->barometric_pressure = mavlink_msg_efi_status_get_barometric_pressure(msg); + efi_status->intake_manifold_pressure = mavlink_msg_efi_status_get_intake_manifold_pressure(msg); + efi_status->intake_manifold_temperature = mavlink_msg_efi_status_get_intake_manifold_temperature(msg); + efi_status->cylinder_head_temperature = mavlink_msg_efi_status_get_cylinder_head_temperature(msg); + efi_status->ignition_timing = mavlink_msg_efi_status_get_ignition_timing(msg); + efi_status->injection_time = mavlink_msg_efi_status_get_injection_time(msg); + efi_status->exhaust_gas_temperature = mavlink_msg_efi_status_get_exhaust_gas_temperature(msg); + efi_status->throttle_out = mavlink_msg_efi_status_get_throttle_out(msg); + efi_status->pt_compensation = mavlink_msg_efi_status_get_pt_compensation(msg); + efi_status->health = mavlink_msg_efi_status_get_health(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_EFI_STATUS_LEN? msg->len : MAVLINK_MSG_ID_EFI_STATUS_LEN; + memset(efi_status, 0, MAVLINK_MSG_ID_EFI_STATUS_LEN); + memcpy(efi_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_esc_info.h b/lib/main/MAVLink/common/mavlink_msg_esc_info.h new file mode 100644 index 0000000000..d8a8bbcfe1 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_esc_info.h @@ -0,0 +1,407 @@ +#pragma once +// MESSAGE ESC_INFO PACKING + +#define MAVLINK_MSG_ID_ESC_INFO 290 + + +typedef struct __mavlink_esc_info_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number.*/ + uint32_t error_count[4]; /*< Number of reported errors by each ESC since boot.*/ + uint16_t counter; /*< Counter of data packets received.*/ + uint16_t failure_flags[4]; /*< Bitmap of ESC failure flags.*/ + uint8_t index; /*< Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4.*/ + uint8_t count; /*< Total number of ESCs in all messages of this type. Message fields with an index higher than this should be ignored because they contain invalid data.*/ + uint8_t connection_type; /*< Connection type protocol for all ESC.*/ + uint8_t info; /*< Information regarding online/offline status of each ESC.*/ + uint8_t temperature[4]; /*< [degC] Temperature measured by each ESC. UINT8_MAX if data not supplied by ESC.*/ +} mavlink_esc_info_t; + +#define MAVLINK_MSG_ID_ESC_INFO_LEN 42 +#define MAVLINK_MSG_ID_ESC_INFO_MIN_LEN 42 +#define MAVLINK_MSG_ID_290_LEN 42 +#define MAVLINK_MSG_ID_290_MIN_LEN 42 + +#define MAVLINK_MSG_ID_ESC_INFO_CRC 221 +#define MAVLINK_MSG_ID_290_CRC 221 + +#define MAVLINK_MSG_ESC_INFO_FIELD_ERROR_COUNT_LEN 4 +#define MAVLINK_MSG_ESC_INFO_FIELD_FAILURE_FLAGS_LEN 4 +#define MAVLINK_MSG_ESC_INFO_FIELD_TEMPERATURE_LEN 4 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_ESC_INFO { \ + 290, \ + "ESC_INFO", \ + 9, \ + { { "index", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_esc_info_t, index) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_esc_info_t, time_usec) }, \ + { "counter", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_esc_info_t, counter) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_esc_info_t, count) }, \ + { "connection_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_esc_info_t, connection_type) }, \ + { "info", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_esc_info_t, info) }, \ + { "failure_flags", NULL, MAVLINK_TYPE_UINT16_T, 4, 26, offsetof(mavlink_esc_info_t, failure_flags) }, \ + { "error_count", NULL, MAVLINK_TYPE_UINT32_T, 4, 8, offsetof(mavlink_esc_info_t, error_count) }, \ + { "temperature", NULL, MAVLINK_TYPE_UINT8_T, 4, 38, offsetof(mavlink_esc_info_t, temperature) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_ESC_INFO { \ + "ESC_INFO", \ + 9, \ + { { "index", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_esc_info_t, index) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_esc_info_t, time_usec) }, \ + { "counter", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_esc_info_t, counter) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_esc_info_t, count) }, \ + { "connection_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_esc_info_t, connection_type) }, \ + { "info", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_esc_info_t, info) }, \ + { "failure_flags", NULL, MAVLINK_TYPE_UINT16_T, 4, 26, offsetof(mavlink_esc_info_t, failure_flags) }, \ + { "error_count", NULL, MAVLINK_TYPE_UINT32_T, 4, 8, offsetof(mavlink_esc_info_t, error_count) }, \ + { "temperature", NULL, MAVLINK_TYPE_UINT8_T, 4, 38, offsetof(mavlink_esc_info_t, temperature) }, \ + } \ +} +#endif + +/** + * @brief Pack a esc_info message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param index Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + * @param counter Counter of data packets received. + * @param count Total number of ESCs in all messages of this type. Message fields with an index higher than this should be ignored because they contain invalid data. + * @param connection_type Connection type protocol for all ESC. + * @param info Information regarding online/offline status of each ESC. + * @param failure_flags Bitmap of ESC failure flags. + * @param error_count Number of reported errors by each ESC since boot. + * @param temperature [degC] Temperature measured by each ESC. UINT8_MAX if data not supplied by ESC. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_esc_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t index, uint64_t time_usec, uint16_t counter, uint8_t count, uint8_t connection_type, uint8_t info, const uint16_t *failure_flags, const uint32_t *error_count, const uint8_t *temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ESC_INFO_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 24, counter); + _mav_put_uint8_t(buf, 34, index); + _mav_put_uint8_t(buf, 35, count); + _mav_put_uint8_t(buf, 36, connection_type); + _mav_put_uint8_t(buf, 37, info); + _mav_put_uint32_t_array(buf, 8, error_count, 4); + _mav_put_uint16_t_array(buf, 26, failure_flags, 4); + _mav_put_uint8_t_array(buf, 38, temperature, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ESC_INFO_LEN); +#else + mavlink_esc_info_t packet; + packet.time_usec = time_usec; + packet.counter = counter; + packet.index = index; + packet.count = count; + packet.connection_type = connection_type; + packet.info = info; + mav_array_memcpy(packet.error_count, error_count, sizeof(uint32_t)*4); + mav_array_memcpy(packet.failure_flags, failure_flags, sizeof(uint16_t)*4); + mav_array_memcpy(packet.temperature, temperature, sizeof(uint8_t)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ESC_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ESC_INFO; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); +} + +/** + * @brief Pack a esc_info message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param index Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + * @param counter Counter of data packets received. + * @param count Total number of ESCs in all messages of this type. Message fields with an index higher than this should be ignored because they contain invalid data. + * @param connection_type Connection type protocol for all ESC. + * @param info Information regarding online/offline status of each ESC. + * @param failure_flags Bitmap of ESC failure flags. + * @param error_count Number of reported errors by each ESC since boot. + * @param temperature [degC] Temperature measured by each ESC. UINT8_MAX if data not supplied by ESC. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_esc_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t index,uint64_t time_usec,uint16_t counter,uint8_t count,uint8_t connection_type,uint8_t info,const uint16_t *failure_flags,const uint32_t *error_count,const uint8_t *temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ESC_INFO_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 24, counter); + _mav_put_uint8_t(buf, 34, index); + _mav_put_uint8_t(buf, 35, count); + _mav_put_uint8_t(buf, 36, connection_type); + _mav_put_uint8_t(buf, 37, info); + _mav_put_uint32_t_array(buf, 8, error_count, 4); + _mav_put_uint16_t_array(buf, 26, failure_flags, 4); + _mav_put_uint8_t_array(buf, 38, temperature, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ESC_INFO_LEN); +#else + mavlink_esc_info_t packet; + packet.time_usec = time_usec; + packet.counter = counter; + packet.index = index; + packet.count = count; + packet.connection_type = connection_type; + packet.info = info; + mav_array_memcpy(packet.error_count, error_count, sizeof(uint32_t)*4); + mav_array_memcpy(packet.failure_flags, failure_flags, sizeof(uint16_t)*4); + mav_array_memcpy(packet.temperature, temperature, sizeof(uint8_t)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ESC_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ESC_INFO; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); +} + +/** + * @brief Encode a esc_info struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param esc_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_esc_info_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_esc_info_t* esc_info) +{ + return mavlink_msg_esc_info_pack(system_id, component_id, msg, esc_info->index, esc_info->time_usec, esc_info->counter, esc_info->count, esc_info->connection_type, esc_info->info, esc_info->failure_flags, esc_info->error_count, esc_info->temperature); +} + +/** + * @brief Encode a esc_info struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param esc_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_esc_info_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_esc_info_t* esc_info) +{ + return mavlink_msg_esc_info_pack_chan(system_id, component_id, chan, msg, esc_info->index, esc_info->time_usec, esc_info->counter, esc_info->count, esc_info->connection_type, esc_info->info, esc_info->failure_flags, esc_info->error_count, esc_info->temperature); +} + +/** + * @brief Send a esc_info message + * @param chan MAVLink channel to send the message + * + * @param index Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + * @param counter Counter of data packets received. + * @param count Total number of ESCs in all messages of this type. Message fields with an index higher than this should be ignored because they contain invalid data. + * @param connection_type Connection type protocol for all ESC. + * @param info Information regarding online/offline status of each ESC. + * @param failure_flags Bitmap of ESC failure flags. + * @param error_count Number of reported errors by each ESC since boot. + * @param temperature [degC] Temperature measured by each ESC. UINT8_MAX if data not supplied by ESC. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_esc_info_send(mavlink_channel_t chan, uint8_t index, uint64_t time_usec, uint16_t counter, uint8_t count, uint8_t connection_type, uint8_t info, const uint16_t *failure_flags, const uint32_t *error_count, const uint8_t *temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ESC_INFO_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 24, counter); + _mav_put_uint8_t(buf, 34, index); + _mav_put_uint8_t(buf, 35, count); + _mav_put_uint8_t(buf, 36, connection_type); + _mav_put_uint8_t(buf, 37, info); + _mav_put_uint32_t_array(buf, 8, error_count, 4); + _mav_put_uint16_t_array(buf, 26, failure_flags, 4); + _mav_put_uint8_t_array(buf, 38, temperature, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_INFO, buf, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); +#else + mavlink_esc_info_t packet; + packet.time_usec = time_usec; + packet.counter = counter; + packet.index = index; + packet.count = count; + packet.connection_type = connection_type; + packet.info = info; + mav_array_memcpy(packet.error_count, error_count, sizeof(uint32_t)*4); + mav_array_memcpy(packet.failure_flags, failure_flags, sizeof(uint16_t)*4); + mav_array_memcpy(packet.temperature, temperature, sizeof(uint8_t)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_INFO, (const char *)&packet, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); +#endif +} + +/** + * @brief Send a esc_info message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_esc_info_send_struct(mavlink_channel_t chan, const mavlink_esc_info_t* esc_info) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_esc_info_send(chan, esc_info->index, esc_info->time_usec, esc_info->counter, esc_info->count, esc_info->connection_type, esc_info->info, esc_info->failure_flags, esc_info->error_count, esc_info->temperature); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_INFO, (const char *)esc_info, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); +#endif +} + +#if MAVLINK_MSG_ID_ESC_INFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_esc_info_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t index, uint64_t time_usec, uint16_t counter, uint8_t count, uint8_t connection_type, uint8_t info, const uint16_t *failure_flags, const uint32_t *error_count, const uint8_t *temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 24, counter); + _mav_put_uint8_t(buf, 34, index); + _mav_put_uint8_t(buf, 35, count); + _mav_put_uint8_t(buf, 36, connection_type); + _mav_put_uint8_t(buf, 37, info); + _mav_put_uint32_t_array(buf, 8, error_count, 4); + _mav_put_uint16_t_array(buf, 26, failure_flags, 4); + _mav_put_uint8_t_array(buf, 38, temperature, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_INFO, buf, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); +#else + mavlink_esc_info_t *packet = (mavlink_esc_info_t *)msgbuf; + packet->time_usec = time_usec; + packet->counter = counter; + packet->index = index; + packet->count = count; + packet->connection_type = connection_type; + packet->info = info; + mav_array_memcpy(packet->error_count, error_count, sizeof(uint32_t)*4); + mav_array_memcpy(packet->failure_flags, failure_flags, sizeof(uint16_t)*4); + mav_array_memcpy(packet->temperature, temperature, sizeof(uint8_t)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_INFO, (const char *)packet, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); +#endif +} +#endif + +#endif + +// MESSAGE ESC_INFO UNPACKING + + +/** + * @brief Get field index from esc_info message + * + * @return Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + */ +static inline uint8_t mavlink_msg_esc_info_get_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field time_usec from esc_info message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + */ +static inline uint64_t mavlink_msg_esc_info_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field counter from esc_info message + * + * @return Counter of data packets received. + */ +static inline uint16_t mavlink_msg_esc_info_get_counter(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field count from esc_info message + * + * @return Total number of ESCs in all messages of this type. Message fields with an index higher than this should be ignored because they contain invalid data. + */ +static inline uint8_t mavlink_msg_esc_info_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 35); +} + +/** + * @brief Get field connection_type from esc_info message + * + * @return Connection type protocol for all ESC. + */ +static inline uint8_t mavlink_msg_esc_info_get_connection_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 36); +} + +/** + * @brief Get field info from esc_info message + * + * @return Information regarding online/offline status of each ESC. + */ +static inline uint8_t mavlink_msg_esc_info_get_info(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 37); +} + +/** + * @brief Get field failure_flags from esc_info message + * + * @return Bitmap of ESC failure flags. + */ +static inline uint16_t mavlink_msg_esc_info_get_failure_flags(const mavlink_message_t* msg, uint16_t *failure_flags) +{ + return _MAV_RETURN_uint16_t_array(msg, failure_flags, 4, 26); +} + +/** + * @brief Get field error_count from esc_info message + * + * @return Number of reported errors by each ESC since boot. + */ +static inline uint16_t mavlink_msg_esc_info_get_error_count(const mavlink_message_t* msg, uint32_t *error_count) +{ + return _MAV_RETURN_uint32_t_array(msg, error_count, 4, 8); +} + +/** + * @brief Get field temperature from esc_info message + * + * @return [degC] Temperature measured by each ESC. UINT8_MAX if data not supplied by ESC. + */ +static inline uint16_t mavlink_msg_esc_info_get_temperature(const mavlink_message_t* msg, uint8_t *temperature) +{ + return _MAV_RETURN_uint8_t_array(msg, temperature, 4, 38); +} + +/** + * @brief Decode a esc_info message into a struct + * + * @param msg The message to decode + * @param esc_info C-struct to decode the message contents into + */ +static inline void mavlink_msg_esc_info_decode(const mavlink_message_t* msg, mavlink_esc_info_t* esc_info) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + esc_info->time_usec = mavlink_msg_esc_info_get_time_usec(msg); + mavlink_msg_esc_info_get_error_count(msg, esc_info->error_count); + esc_info->counter = mavlink_msg_esc_info_get_counter(msg); + mavlink_msg_esc_info_get_failure_flags(msg, esc_info->failure_flags); + esc_info->index = mavlink_msg_esc_info_get_index(msg); + esc_info->count = mavlink_msg_esc_info_get_count(msg); + esc_info->connection_type = mavlink_msg_esc_info_get_connection_type(msg); + esc_info->info = mavlink_msg_esc_info_get_info(msg); + mavlink_msg_esc_info_get_temperature(msg, esc_info->temperature); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_ESC_INFO_LEN? msg->len : MAVLINK_MSG_ID_ESC_INFO_LEN; + memset(esc_info, 0, MAVLINK_MSG_ID_ESC_INFO_LEN); + memcpy(esc_info, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_esc_status.h b/lib/main/MAVLink/common/mavlink_msg_esc_status.h new file mode 100644 index 0000000000..3a5fefe20a --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_esc_status.h @@ -0,0 +1,307 @@ +#pragma once +// MESSAGE ESC_STATUS PACKING + +#define MAVLINK_MSG_ID_ESC_STATUS 291 + + +typedef struct __mavlink_esc_status_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number.*/ + int32_t rpm[4]; /*< [rpm] Reported motor RPM from each ESC (negative for reverse rotation).*/ + float voltage[4]; /*< [V] Voltage measured from each ESC.*/ + float current[4]; /*< [A] Current measured from each ESC.*/ + uint8_t index; /*< Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4.*/ +} mavlink_esc_status_t; + +#define MAVLINK_MSG_ID_ESC_STATUS_LEN 57 +#define MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN 57 +#define MAVLINK_MSG_ID_291_LEN 57 +#define MAVLINK_MSG_ID_291_MIN_LEN 57 + +#define MAVLINK_MSG_ID_ESC_STATUS_CRC 10 +#define MAVLINK_MSG_ID_291_CRC 10 + +#define MAVLINK_MSG_ESC_STATUS_FIELD_RPM_LEN 4 +#define MAVLINK_MSG_ESC_STATUS_FIELD_VOLTAGE_LEN 4 +#define MAVLINK_MSG_ESC_STATUS_FIELD_CURRENT_LEN 4 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_ESC_STATUS { \ + 291, \ + "ESC_STATUS", \ + 5, \ + { { "index", NULL, MAVLINK_TYPE_UINT8_T, 0, 56, offsetof(mavlink_esc_status_t, index) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_esc_status_t, time_usec) }, \ + { "rpm", NULL, MAVLINK_TYPE_INT32_T, 4, 8, offsetof(mavlink_esc_status_t, rpm) }, \ + { "voltage", NULL, MAVLINK_TYPE_FLOAT, 4, 24, offsetof(mavlink_esc_status_t, voltage) }, \ + { "current", NULL, MAVLINK_TYPE_FLOAT, 4, 40, offsetof(mavlink_esc_status_t, current) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_ESC_STATUS { \ + "ESC_STATUS", \ + 5, \ + { { "index", NULL, MAVLINK_TYPE_UINT8_T, 0, 56, offsetof(mavlink_esc_status_t, index) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_esc_status_t, time_usec) }, \ + { "rpm", NULL, MAVLINK_TYPE_INT32_T, 4, 8, offsetof(mavlink_esc_status_t, rpm) }, \ + { "voltage", NULL, MAVLINK_TYPE_FLOAT, 4, 24, offsetof(mavlink_esc_status_t, voltage) }, \ + { "current", NULL, MAVLINK_TYPE_FLOAT, 4, 40, offsetof(mavlink_esc_status_t, current) }, \ + } \ +} +#endif + +/** + * @brief Pack a esc_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param index Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + * @param rpm [rpm] Reported motor RPM from each ESC (negative for reverse rotation). + * @param voltage [V] Voltage measured from each ESC. + * @param current [A] Current measured from each ESC. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_esc_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t index, uint64_t time_usec, const int32_t *rpm, const float *voltage, const float *current) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ESC_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 56, index); + _mav_put_int32_t_array(buf, 8, rpm, 4); + _mav_put_float_array(buf, 24, voltage, 4); + _mav_put_float_array(buf, 40, current, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ESC_STATUS_LEN); +#else + mavlink_esc_status_t packet; + packet.time_usec = time_usec; + packet.index = index; + mav_array_memcpy(packet.rpm, rpm, sizeof(int32_t)*4); + mav_array_memcpy(packet.voltage, voltage, sizeof(float)*4); + mav_array_memcpy(packet.current, current, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ESC_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ESC_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); +} + +/** + * @brief Pack a esc_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param index Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + * @param rpm [rpm] Reported motor RPM from each ESC (negative for reverse rotation). + * @param voltage [V] Voltage measured from each ESC. + * @param current [A] Current measured from each ESC. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_esc_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t index,uint64_t time_usec,const int32_t *rpm,const float *voltage,const float *current) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ESC_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 56, index); + _mav_put_int32_t_array(buf, 8, rpm, 4); + _mav_put_float_array(buf, 24, voltage, 4); + _mav_put_float_array(buf, 40, current, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ESC_STATUS_LEN); +#else + mavlink_esc_status_t packet; + packet.time_usec = time_usec; + packet.index = index; + mav_array_memcpy(packet.rpm, rpm, sizeof(int32_t)*4); + mav_array_memcpy(packet.voltage, voltage, sizeof(float)*4); + mav_array_memcpy(packet.current, current, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ESC_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ESC_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); +} + +/** + * @brief Encode a esc_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param esc_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_esc_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_esc_status_t* esc_status) +{ + return mavlink_msg_esc_status_pack(system_id, component_id, msg, esc_status->index, esc_status->time_usec, esc_status->rpm, esc_status->voltage, esc_status->current); +} + +/** + * @brief Encode a esc_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param esc_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_esc_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_esc_status_t* esc_status) +{ + return mavlink_msg_esc_status_pack_chan(system_id, component_id, chan, msg, esc_status->index, esc_status->time_usec, esc_status->rpm, esc_status->voltage, esc_status->current); +} + +/** + * @brief Send a esc_status message + * @param chan MAVLink channel to send the message + * + * @param index Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + * @param rpm [rpm] Reported motor RPM from each ESC (negative for reverse rotation). + * @param voltage [V] Voltage measured from each ESC. + * @param current [A] Current measured from each ESC. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_esc_status_send(mavlink_channel_t chan, uint8_t index, uint64_t time_usec, const int32_t *rpm, const float *voltage, const float *current) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ESC_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 56, index); + _mav_put_int32_t_array(buf, 8, rpm, 4); + _mav_put_float_array(buf, 24, voltage, 4); + _mav_put_float_array(buf, 40, current, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_STATUS, buf, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); +#else + mavlink_esc_status_t packet; + packet.time_usec = time_usec; + packet.index = index; + mav_array_memcpy(packet.rpm, rpm, sizeof(int32_t)*4); + mav_array_memcpy(packet.voltage, voltage, sizeof(float)*4); + mav_array_memcpy(packet.current, current, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); +#endif +} + +/** + * @brief Send a esc_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_esc_status_send_struct(mavlink_channel_t chan, const mavlink_esc_status_t* esc_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_esc_status_send(chan, esc_status->index, esc_status->time_usec, esc_status->rpm, esc_status->voltage, esc_status->current); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_STATUS, (const char *)esc_status, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_ESC_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_esc_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t index, uint64_t time_usec, const int32_t *rpm, const float *voltage, const float *current) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 56, index); + _mav_put_int32_t_array(buf, 8, rpm, 4); + _mav_put_float_array(buf, 24, voltage, 4); + _mav_put_float_array(buf, 40, current, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_STATUS, buf, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); +#else + mavlink_esc_status_t *packet = (mavlink_esc_status_t *)msgbuf; + packet->time_usec = time_usec; + packet->index = index; + mav_array_memcpy(packet->rpm, rpm, sizeof(int32_t)*4); + mav_array_memcpy(packet->voltage, voltage, sizeof(float)*4); + mav_array_memcpy(packet->current, current, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_STATUS, (const char *)packet, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE ESC_STATUS UNPACKING + + +/** + * @brief Get field index from esc_status message + * + * @return Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + */ +static inline uint8_t mavlink_msg_esc_status_get_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 56); +} + +/** + * @brief Get field time_usec from esc_status message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + */ +static inline uint64_t mavlink_msg_esc_status_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field rpm from esc_status message + * + * @return [rpm] Reported motor RPM from each ESC (negative for reverse rotation). + */ +static inline uint16_t mavlink_msg_esc_status_get_rpm(const mavlink_message_t* msg, int32_t *rpm) +{ + return _MAV_RETURN_int32_t_array(msg, rpm, 4, 8); +} + +/** + * @brief Get field voltage from esc_status message + * + * @return [V] Voltage measured from each ESC. + */ +static inline uint16_t mavlink_msg_esc_status_get_voltage(const mavlink_message_t* msg, float *voltage) +{ + return _MAV_RETURN_float_array(msg, voltage, 4, 24); +} + +/** + * @brief Get field current from esc_status message + * + * @return [A] Current measured from each ESC. + */ +static inline uint16_t mavlink_msg_esc_status_get_current(const mavlink_message_t* msg, float *current) +{ + return _MAV_RETURN_float_array(msg, current, 4, 40); +} + +/** + * @brief Decode a esc_status message into a struct + * + * @param msg The message to decode + * @param esc_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_esc_status_decode(const mavlink_message_t* msg, mavlink_esc_status_t* esc_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + esc_status->time_usec = mavlink_msg_esc_status_get_time_usec(msg); + mavlink_msg_esc_status_get_rpm(msg, esc_status->rpm); + mavlink_msg_esc_status_get_voltage(msg, esc_status->voltage); + mavlink_msg_esc_status_get_current(msg, esc_status->current); + esc_status->index = mavlink_msg_esc_status_get_index(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_ESC_STATUS_LEN? msg->len : MAVLINK_MSG_ID_ESC_STATUS_LEN; + memset(esc_status, 0, MAVLINK_MSG_ID_ESC_STATUS_LEN); + memcpy(esc_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_fence_status.h b/lib/main/MAVLink/common/mavlink_msg_fence_status.h index d15f768d9b..08ba1555f2 100644 --- a/lib/main/MAVLink/common/mavlink_msg_fence_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_fence_status.h @@ -9,11 +9,12 @@ typedef struct __mavlink_fence_status_t { uint16_t breach_count; /*< Number of fence breaches.*/ uint8_t breach_status; /*< Breach status (0 if currently inside fence, 1 if outside).*/ uint8_t breach_type; /*< Last breach type.*/ + uint8_t breach_mitigation; /*< Active action to prevent fence breach*/ } mavlink_fence_status_t; -#define MAVLINK_MSG_ID_FENCE_STATUS_LEN 8 +#define MAVLINK_MSG_ID_FENCE_STATUS_LEN 9 #define MAVLINK_MSG_ID_FENCE_STATUS_MIN_LEN 8 -#define MAVLINK_MSG_ID_162_LEN 8 +#define MAVLINK_MSG_ID_162_LEN 9 #define MAVLINK_MSG_ID_162_MIN_LEN 8 #define MAVLINK_MSG_ID_FENCE_STATUS_CRC 189 @@ -25,21 +26,23 @@ typedef struct __mavlink_fence_status_t { #define MAVLINK_MESSAGE_INFO_FENCE_STATUS { \ 162, \ "FENCE_STATUS", \ - 4, \ + 5, \ { { "breach_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_fence_status_t, breach_status) }, \ { "breach_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_fence_status_t, breach_count) }, \ { "breach_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_fence_status_t, breach_type) }, \ { "breach_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_fence_status_t, breach_time) }, \ + { "breach_mitigation", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_fence_status_t, breach_mitigation) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_FENCE_STATUS { \ "FENCE_STATUS", \ - 4, \ + 5, \ { { "breach_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_fence_status_t, breach_status) }, \ { "breach_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_fence_status_t, breach_count) }, \ { "breach_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_fence_status_t, breach_type) }, \ { "breach_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_fence_status_t, breach_time) }, \ + { "breach_mitigation", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_fence_status_t, breach_mitigation) }, \ } \ } #endif @@ -54,10 +57,11 @@ typedef struct __mavlink_fence_status_t { * @param breach_count Number of fence breaches. * @param breach_type Last breach type. * @param breach_time [ms] Time (since boot) of last breach. + * @param breach_mitigation Active action to prevent fence breach * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time) + uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time, uint8_t breach_mitigation) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN]; @@ -65,6 +69,7 @@ static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t _mav_put_uint16_t(buf, 4, breach_count); _mav_put_uint8_t(buf, 6, breach_status); _mav_put_uint8_t(buf, 7, breach_type); + _mav_put_uint8_t(buf, 8, breach_mitigation); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); #else @@ -73,6 +78,7 @@ static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t packet.breach_count = breach_count; packet.breach_status = breach_status; packet.breach_type = breach_type; + packet.breach_mitigation = breach_mitigation; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); #endif @@ -91,11 +97,12 @@ static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t * @param breach_count Number of fence breaches. * @param breach_type Last breach type. * @param breach_time [ms] Time (since boot) of last breach. + * @param breach_mitigation Active action to prevent fence breach * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t breach_status,uint16_t breach_count,uint8_t breach_type,uint32_t breach_time) + uint8_t breach_status,uint16_t breach_count,uint8_t breach_type,uint32_t breach_time,uint8_t breach_mitigation) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN]; @@ -103,6 +110,7 @@ static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uin _mav_put_uint16_t(buf, 4, breach_count); _mav_put_uint8_t(buf, 6, breach_status); _mav_put_uint8_t(buf, 7, breach_type); + _mav_put_uint8_t(buf, 8, breach_mitigation); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); #else @@ -111,6 +119,7 @@ static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uin packet.breach_count = breach_count; packet.breach_status = breach_status; packet.breach_type = breach_type; + packet.breach_mitigation = breach_mitigation; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); #endif @@ -129,7 +138,7 @@ static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_fence_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status) { - return mavlink_msg_fence_status_pack(system_id, component_id, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time); + return mavlink_msg_fence_status_pack(system_id, component_id, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time, fence_status->breach_mitigation); } /** @@ -143,7 +152,7 @@ static inline uint16_t mavlink_msg_fence_status_encode(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_fence_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status) { - return mavlink_msg_fence_status_pack_chan(system_id, component_id, chan, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time); + return mavlink_msg_fence_status_pack_chan(system_id, component_id, chan, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time, fence_status->breach_mitigation); } /** @@ -154,10 +163,11 @@ static inline uint16_t mavlink_msg_fence_status_encode_chan(uint8_t system_id, u * @param breach_count Number of fence breaches. * @param breach_type Last breach type. * @param breach_time [ms] Time (since boot) of last breach. + * @param breach_mitigation Active action to prevent fence breach */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time) +static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time, uint8_t breach_mitigation) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN]; @@ -165,6 +175,7 @@ static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t _mav_put_uint16_t(buf, 4, breach_count); _mav_put_uint8_t(buf, 6, breach_status); _mav_put_uint8_t(buf, 7, breach_type); + _mav_put_uint8_t(buf, 8, breach_mitigation); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_MIN_LEN, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); #else @@ -173,6 +184,7 @@ static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t packet.breach_count = breach_count; packet.breach_status = breach_status; packet.breach_type = breach_type; + packet.breach_mitigation = breach_mitigation; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_FENCE_STATUS_MIN_LEN, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); #endif @@ -186,7 +198,7 @@ static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t static inline void mavlink_msg_fence_status_send_struct(mavlink_channel_t chan, const mavlink_fence_status_t* fence_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_fence_status_send(chan, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time); + mavlink_msg_fence_status_send(chan, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time, fence_status->breach_mitigation); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)fence_status, MAVLINK_MSG_ID_FENCE_STATUS_MIN_LEN, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); #endif @@ -200,7 +212,7 @@ static inline void mavlink_msg_fence_status_send_struct(mavlink_channel_t chan, is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_fence_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time) +static inline void mavlink_msg_fence_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time, uint8_t breach_mitigation) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +220,7 @@ static inline void mavlink_msg_fence_status_send_buf(mavlink_message_t *msgbuf, _mav_put_uint16_t(buf, 4, breach_count); _mav_put_uint8_t(buf, 6, breach_status); _mav_put_uint8_t(buf, 7, breach_type); + _mav_put_uint8_t(buf, 8, breach_mitigation); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_MIN_LEN, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); #else @@ -216,6 +229,7 @@ static inline void mavlink_msg_fence_status_send_buf(mavlink_message_t *msgbuf, packet->breach_count = breach_count; packet->breach_status = breach_status; packet->breach_type = breach_type; + packet->breach_mitigation = breach_mitigation; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)packet, MAVLINK_MSG_ID_FENCE_STATUS_MIN_LEN, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); #endif @@ -267,6 +281,16 @@ static inline uint32_t mavlink_msg_fence_status_get_breach_time(const mavlink_me return _MAV_RETURN_uint32_t(msg, 0); } +/** + * @brief Get field breach_mitigation from fence_status message + * + * @return Active action to prevent fence breach + */ +static inline uint8_t mavlink_msg_fence_status_get_breach_mitigation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + /** * @brief Decode a fence_status message into a struct * @@ -280,6 +304,7 @@ static inline void mavlink_msg_fence_status_decode(const mavlink_message_t* msg, fence_status->breach_count = mavlink_msg_fence_status_get_breach_count(msg); fence_status->breach_status = mavlink_msg_fence_status_get_breach_status(msg); fence_status->breach_type = mavlink_msg_fence_status_get_breach_type(msg); + fence_status->breach_mitigation = mavlink_msg_fence_status_get_breach_mitigation(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_FENCE_STATUS_LEN? msg->len : MAVLINK_MSG_ID_FENCE_STATUS_LEN; memset(fence_status, 0, MAVLINK_MSG_ID_FENCE_STATUS_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_flight_information.h b/lib/main/MAVLink/common/mavlink_msg_flight_information.h new file mode 100644 index 0000000000..78513ff593 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_flight_information.h @@ -0,0 +1,288 @@ +#pragma once +// MESSAGE FLIGHT_INFORMATION PACKING + +#define MAVLINK_MSG_ID_FLIGHT_INFORMATION 264 + + +typedef struct __mavlink_flight_information_t { + uint64_t arming_time_utc; /*< [us] Timestamp at arming (time since UNIX epoch) in UTC, 0 for unknown*/ + uint64_t takeoff_time_utc; /*< [us] Timestamp at takeoff (time since UNIX epoch) in UTC, 0 for unknown*/ + uint64_t flight_uuid; /*< Universally unique identifier (UUID) of flight, should correspond to name of log files*/ + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ +} mavlink_flight_information_t; + +#define MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN 28 +#define MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN 28 +#define MAVLINK_MSG_ID_264_LEN 28 +#define MAVLINK_MSG_ID_264_MIN_LEN 28 + +#define MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC 49 +#define MAVLINK_MSG_ID_264_CRC 49 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_FLIGHT_INFORMATION { \ + 264, \ + "FLIGHT_INFORMATION", \ + 4, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 24, offsetof(mavlink_flight_information_t, time_boot_ms) }, \ + { "arming_time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_flight_information_t, arming_time_utc) }, \ + { "takeoff_time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_flight_information_t, takeoff_time_utc) }, \ + { "flight_uuid", NULL, MAVLINK_TYPE_UINT64_T, 0, 16, offsetof(mavlink_flight_information_t, flight_uuid) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_FLIGHT_INFORMATION { \ + "FLIGHT_INFORMATION", \ + 4, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 24, offsetof(mavlink_flight_information_t, time_boot_ms) }, \ + { "arming_time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_flight_information_t, arming_time_utc) }, \ + { "takeoff_time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_flight_information_t, takeoff_time_utc) }, \ + { "flight_uuid", NULL, MAVLINK_TYPE_UINT64_T, 0, 16, offsetof(mavlink_flight_information_t, flight_uuid) }, \ + } \ +} +#endif + +/** + * @brief Pack a flight_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param arming_time_utc [us] Timestamp at arming (time since UNIX epoch) in UTC, 0 for unknown + * @param takeoff_time_utc [us] Timestamp at takeoff (time since UNIX epoch) in UTC, 0 for unknown + * @param flight_uuid Universally unique identifier (UUID) of flight, should correspond to name of log files + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_flight_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint64_t arming_time_utc, uint64_t takeoff_time_utc, uint64_t flight_uuid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN]; + _mav_put_uint64_t(buf, 0, arming_time_utc); + _mav_put_uint64_t(buf, 8, takeoff_time_utc); + _mav_put_uint64_t(buf, 16, flight_uuid); + _mav_put_uint32_t(buf, 24, time_boot_ms); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); +#else + mavlink_flight_information_t packet; + packet.arming_time_utc = arming_time_utc; + packet.takeoff_time_utc = takeoff_time_utc; + packet.flight_uuid = flight_uuid; + packet.time_boot_ms = time_boot_ms; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FLIGHT_INFORMATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); +} + +/** + * @brief Pack a flight_information message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param arming_time_utc [us] Timestamp at arming (time since UNIX epoch) in UTC, 0 for unknown + * @param takeoff_time_utc [us] Timestamp at takeoff (time since UNIX epoch) in UTC, 0 for unknown + * @param flight_uuid Universally unique identifier (UUID) of flight, should correspond to name of log files + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_flight_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint64_t arming_time_utc,uint64_t takeoff_time_utc,uint64_t flight_uuid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN]; + _mav_put_uint64_t(buf, 0, arming_time_utc); + _mav_put_uint64_t(buf, 8, takeoff_time_utc); + _mav_put_uint64_t(buf, 16, flight_uuid); + _mav_put_uint32_t(buf, 24, time_boot_ms); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); +#else + mavlink_flight_information_t packet; + packet.arming_time_utc = arming_time_utc; + packet.takeoff_time_utc = takeoff_time_utc; + packet.flight_uuid = flight_uuid; + packet.time_boot_ms = time_boot_ms; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FLIGHT_INFORMATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); +} + +/** + * @brief Encode a flight_information struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param flight_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flight_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flight_information_t* flight_information) +{ + return mavlink_msg_flight_information_pack(system_id, component_id, msg, flight_information->time_boot_ms, flight_information->arming_time_utc, flight_information->takeoff_time_utc, flight_information->flight_uuid); +} + +/** + * @brief Encode a flight_information struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flight_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flight_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flight_information_t* flight_information) +{ + return mavlink_msg_flight_information_pack_chan(system_id, component_id, chan, msg, flight_information->time_boot_ms, flight_information->arming_time_utc, flight_information->takeoff_time_utc, flight_information->flight_uuid); +} + +/** + * @brief Send a flight_information message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param arming_time_utc [us] Timestamp at arming (time since UNIX epoch) in UTC, 0 for unknown + * @param takeoff_time_utc [us] Timestamp at takeoff (time since UNIX epoch) in UTC, 0 for unknown + * @param flight_uuid Universally unique identifier (UUID) of flight, should correspond to name of log files + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_flight_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t arming_time_utc, uint64_t takeoff_time_utc, uint64_t flight_uuid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN]; + _mav_put_uint64_t(buf, 0, arming_time_utc); + _mav_put_uint64_t(buf, 8, takeoff_time_utc); + _mav_put_uint64_t(buf, 16, flight_uuid); + _mav_put_uint32_t(buf, 24, time_boot_ms); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLIGHT_INFORMATION, buf, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); +#else + mavlink_flight_information_t packet; + packet.arming_time_utc = arming_time_utc; + packet.takeoff_time_utc = takeoff_time_utc; + packet.flight_uuid = flight_uuid; + packet.time_boot_ms = time_boot_ms; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLIGHT_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); +#endif +} + +/** + * @brief Send a flight_information message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_flight_information_send_struct(mavlink_channel_t chan, const mavlink_flight_information_t* flight_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_flight_information_send(chan, flight_information->time_boot_ms, flight_information->arming_time_utc, flight_information->takeoff_time_utc, flight_information->flight_uuid); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLIGHT_INFORMATION, (const char *)flight_information, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flight_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t arming_time_utc, uint64_t takeoff_time_utc, uint64_t flight_uuid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, arming_time_utc); + _mav_put_uint64_t(buf, 8, takeoff_time_utc); + _mav_put_uint64_t(buf, 16, flight_uuid); + _mav_put_uint32_t(buf, 24, time_boot_ms); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLIGHT_INFORMATION, buf, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); +#else + mavlink_flight_information_t *packet = (mavlink_flight_information_t *)msgbuf; + packet->arming_time_utc = arming_time_utc; + packet->takeoff_time_utc = takeoff_time_utc; + packet->flight_uuid = flight_uuid; + packet->time_boot_ms = time_boot_ms; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLIGHT_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE FLIGHT_INFORMATION UNPACKING + + +/** + * @brief Get field time_boot_ms from flight_information message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_flight_information_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 24); +} + +/** + * @brief Get field arming_time_utc from flight_information message + * + * @return [us] Timestamp at arming (time since UNIX epoch) in UTC, 0 for unknown + */ +static inline uint64_t mavlink_msg_flight_information_get_arming_time_utc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field takeoff_time_utc from flight_information message + * + * @return [us] Timestamp at takeoff (time since UNIX epoch) in UTC, 0 for unknown + */ +static inline uint64_t mavlink_msg_flight_information_get_takeoff_time_utc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 8); +} + +/** + * @brief Get field flight_uuid from flight_information message + * + * @return Universally unique identifier (UUID) of flight, should correspond to name of log files + */ +static inline uint64_t mavlink_msg_flight_information_get_flight_uuid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 16); +} + +/** + * @brief Decode a flight_information message into a struct + * + * @param msg The message to decode + * @param flight_information C-struct to decode the message contents into + */ +static inline void mavlink_msg_flight_information_decode(const mavlink_message_t* msg, mavlink_flight_information_t* flight_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + flight_information->arming_time_utc = mavlink_msg_flight_information_get_arming_time_utc(msg); + flight_information->takeoff_time_utc = mavlink_msg_flight_information_get_takeoff_time_utc(msg); + flight_information->flight_uuid = mavlink_msg_flight_information_get_flight_uuid(msg); + flight_information->time_boot_ms = mavlink_msg_flight_information_get_time_boot_ms(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN; + memset(flight_information, 0, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); + memcpy(flight_information, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_generator_status.h b/lib/main/MAVLink/common/mavlink_msg_generator_status.h new file mode 100644 index 0000000000..ef960e13cf --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_generator_status.h @@ -0,0 +1,463 @@ +#pragma once +// MESSAGE GENERATOR_STATUS PACKING + +#define MAVLINK_MSG_ID_GENERATOR_STATUS 373 + + +typedef struct __mavlink_generator_status_t { + uint64_t status; /*< Status flags.*/ + float battery_current; /*< [A] Current into/out of battery. Positive for out. Negative for in. NaN: field not provided.*/ + float load_current; /*< [A] Current going to the UAV. If battery current not available this is the DC current from the generator. Positive for out. Negative for in. NaN: field not provided*/ + float power_generated; /*< [W] The power being generated. NaN: field not provided*/ + float bus_voltage; /*< [V] Voltage of the bus seen at the generator, or battery bus if battery bus is controlled by generator and at a different voltage to main bus.*/ + float bat_current_setpoint; /*< [A] The target battery current. Positive for out. Negative for in. NaN: field not provided*/ + uint32_t runtime; /*< [s] Seconds this generator has run since it was rebooted. UINT32_MAX: field not provided.*/ + int32_t time_until_maintenance; /*< [s] Seconds until this generator requires maintenance. A negative value indicates maintenance is past-due. INT32_MAX: field not provided.*/ + uint16_t generator_speed; /*< [rpm] Speed of electrical generator or alternator. UINT16_MAX: field not provided.*/ + int16_t rectifier_temperature; /*< [degC] The temperature of the rectifier or power converter. INT16_MAX: field not provided.*/ + int16_t generator_temperature; /*< [degC] The temperature of the mechanical motor, fuel cell core or generator. INT16_MAX: field not provided.*/ +} mavlink_generator_status_t; + +#define MAVLINK_MSG_ID_GENERATOR_STATUS_LEN 42 +#define MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN 42 +#define MAVLINK_MSG_ID_373_LEN 42 +#define MAVLINK_MSG_ID_373_MIN_LEN 42 + +#define MAVLINK_MSG_ID_GENERATOR_STATUS_CRC 117 +#define MAVLINK_MSG_ID_373_CRC 117 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GENERATOR_STATUS { \ + 373, \ + "GENERATOR_STATUS", \ + 11, \ + { { "status", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_generator_status_t, status) }, \ + { "generator_speed", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_generator_status_t, generator_speed) }, \ + { "battery_current", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_generator_status_t, battery_current) }, \ + { "load_current", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_generator_status_t, load_current) }, \ + { "power_generated", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_generator_status_t, power_generated) }, \ + { "bus_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_generator_status_t, bus_voltage) }, \ + { "rectifier_temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_generator_status_t, rectifier_temperature) }, \ + { "bat_current_setpoint", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_generator_status_t, bat_current_setpoint) }, \ + { "generator_temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_generator_status_t, generator_temperature) }, \ + { "runtime", NULL, MAVLINK_TYPE_UINT32_T, 0, 28, offsetof(mavlink_generator_status_t, runtime) }, \ + { "time_until_maintenance", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_generator_status_t, time_until_maintenance) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GENERATOR_STATUS { \ + "GENERATOR_STATUS", \ + 11, \ + { { "status", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_generator_status_t, status) }, \ + { "generator_speed", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_generator_status_t, generator_speed) }, \ + { "battery_current", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_generator_status_t, battery_current) }, \ + { "load_current", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_generator_status_t, load_current) }, \ + { "power_generated", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_generator_status_t, power_generated) }, \ + { "bus_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_generator_status_t, bus_voltage) }, \ + { "rectifier_temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_generator_status_t, rectifier_temperature) }, \ + { "bat_current_setpoint", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_generator_status_t, bat_current_setpoint) }, \ + { "generator_temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_generator_status_t, generator_temperature) }, \ + { "runtime", NULL, MAVLINK_TYPE_UINT32_T, 0, 28, offsetof(mavlink_generator_status_t, runtime) }, \ + { "time_until_maintenance", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_generator_status_t, time_until_maintenance) }, \ + } \ +} +#endif + +/** + * @brief Pack a generator_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param status Status flags. + * @param generator_speed [rpm] Speed of electrical generator or alternator. UINT16_MAX: field not provided. + * @param battery_current [A] Current into/out of battery. Positive for out. Negative for in. NaN: field not provided. + * @param load_current [A] Current going to the UAV. If battery current not available this is the DC current from the generator. Positive for out. Negative for in. NaN: field not provided + * @param power_generated [W] The power being generated. NaN: field not provided + * @param bus_voltage [V] Voltage of the bus seen at the generator, or battery bus if battery bus is controlled by generator and at a different voltage to main bus. + * @param rectifier_temperature [degC] The temperature of the rectifier or power converter. INT16_MAX: field not provided. + * @param bat_current_setpoint [A] The target battery current. Positive for out. Negative for in. NaN: field not provided + * @param generator_temperature [degC] The temperature of the mechanical motor, fuel cell core or generator. INT16_MAX: field not provided. + * @param runtime [s] Seconds this generator has run since it was rebooted. UINT32_MAX: field not provided. + * @param time_until_maintenance [s] Seconds until this generator requires maintenance. A negative value indicates maintenance is past-due. INT32_MAX: field not provided. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_generator_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t status, uint16_t generator_speed, float battery_current, float load_current, float power_generated, float bus_voltage, int16_t rectifier_temperature, float bat_current_setpoint, int16_t generator_temperature, uint32_t runtime, int32_t time_until_maintenance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GENERATOR_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, status); + _mav_put_float(buf, 8, battery_current); + _mav_put_float(buf, 12, load_current); + _mav_put_float(buf, 16, power_generated); + _mav_put_float(buf, 20, bus_voltage); + _mav_put_float(buf, 24, bat_current_setpoint); + _mav_put_uint32_t(buf, 28, runtime); + _mav_put_int32_t(buf, 32, time_until_maintenance); + _mav_put_uint16_t(buf, 36, generator_speed); + _mav_put_int16_t(buf, 38, rectifier_temperature); + _mav_put_int16_t(buf, 40, generator_temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN); +#else + mavlink_generator_status_t packet; + packet.status = status; + packet.battery_current = battery_current; + packet.load_current = load_current; + packet.power_generated = power_generated; + packet.bus_voltage = bus_voltage; + packet.bat_current_setpoint = bat_current_setpoint; + packet.runtime = runtime; + packet.time_until_maintenance = time_until_maintenance; + packet.generator_speed = generator_speed; + packet.rectifier_temperature = rectifier_temperature; + packet.generator_temperature = generator_temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GENERATOR_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_CRC); +} + +/** + * @brief Pack a generator_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param status Status flags. + * @param generator_speed [rpm] Speed of electrical generator or alternator. UINT16_MAX: field not provided. + * @param battery_current [A] Current into/out of battery. Positive for out. Negative for in. NaN: field not provided. + * @param load_current [A] Current going to the UAV. If battery current not available this is the DC current from the generator. Positive for out. Negative for in. NaN: field not provided + * @param power_generated [W] The power being generated. NaN: field not provided + * @param bus_voltage [V] Voltage of the bus seen at the generator, or battery bus if battery bus is controlled by generator and at a different voltage to main bus. + * @param rectifier_temperature [degC] The temperature of the rectifier or power converter. INT16_MAX: field not provided. + * @param bat_current_setpoint [A] The target battery current. Positive for out. Negative for in. NaN: field not provided + * @param generator_temperature [degC] The temperature of the mechanical motor, fuel cell core or generator. INT16_MAX: field not provided. + * @param runtime [s] Seconds this generator has run since it was rebooted. UINT32_MAX: field not provided. + * @param time_until_maintenance [s] Seconds until this generator requires maintenance. A negative value indicates maintenance is past-due. INT32_MAX: field not provided. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_generator_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t status,uint16_t generator_speed,float battery_current,float load_current,float power_generated,float bus_voltage,int16_t rectifier_temperature,float bat_current_setpoint,int16_t generator_temperature,uint32_t runtime,int32_t time_until_maintenance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GENERATOR_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, status); + _mav_put_float(buf, 8, battery_current); + _mav_put_float(buf, 12, load_current); + _mav_put_float(buf, 16, power_generated); + _mav_put_float(buf, 20, bus_voltage); + _mav_put_float(buf, 24, bat_current_setpoint); + _mav_put_uint32_t(buf, 28, runtime); + _mav_put_int32_t(buf, 32, time_until_maintenance); + _mav_put_uint16_t(buf, 36, generator_speed); + _mav_put_int16_t(buf, 38, rectifier_temperature); + _mav_put_int16_t(buf, 40, generator_temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN); +#else + mavlink_generator_status_t packet; + packet.status = status; + packet.battery_current = battery_current; + packet.load_current = load_current; + packet.power_generated = power_generated; + packet.bus_voltage = bus_voltage; + packet.bat_current_setpoint = bat_current_setpoint; + packet.runtime = runtime; + packet.time_until_maintenance = time_until_maintenance; + packet.generator_speed = generator_speed; + packet.rectifier_temperature = rectifier_temperature; + packet.generator_temperature = generator_temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GENERATOR_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_CRC); +} + +/** + * @brief Encode a generator_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param generator_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_generator_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_generator_status_t* generator_status) +{ + return mavlink_msg_generator_status_pack(system_id, component_id, msg, generator_status->status, generator_status->generator_speed, generator_status->battery_current, generator_status->load_current, generator_status->power_generated, generator_status->bus_voltage, generator_status->rectifier_temperature, generator_status->bat_current_setpoint, generator_status->generator_temperature, generator_status->runtime, generator_status->time_until_maintenance); +} + +/** + * @brief Encode a generator_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param generator_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_generator_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_generator_status_t* generator_status) +{ + return mavlink_msg_generator_status_pack_chan(system_id, component_id, chan, msg, generator_status->status, generator_status->generator_speed, generator_status->battery_current, generator_status->load_current, generator_status->power_generated, generator_status->bus_voltage, generator_status->rectifier_temperature, generator_status->bat_current_setpoint, generator_status->generator_temperature, generator_status->runtime, generator_status->time_until_maintenance); +} + +/** + * @brief Send a generator_status message + * @param chan MAVLink channel to send the message + * + * @param status Status flags. + * @param generator_speed [rpm] Speed of electrical generator or alternator. UINT16_MAX: field not provided. + * @param battery_current [A] Current into/out of battery. Positive for out. Negative for in. NaN: field not provided. + * @param load_current [A] Current going to the UAV. If battery current not available this is the DC current from the generator. Positive for out. Negative for in. NaN: field not provided + * @param power_generated [W] The power being generated. NaN: field not provided + * @param bus_voltage [V] Voltage of the bus seen at the generator, or battery bus if battery bus is controlled by generator and at a different voltage to main bus. + * @param rectifier_temperature [degC] The temperature of the rectifier or power converter. INT16_MAX: field not provided. + * @param bat_current_setpoint [A] The target battery current. Positive for out. Negative for in. NaN: field not provided + * @param generator_temperature [degC] The temperature of the mechanical motor, fuel cell core or generator. INT16_MAX: field not provided. + * @param runtime [s] Seconds this generator has run since it was rebooted. UINT32_MAX: field not provided. + * @param time_until_maintenance [s] Seconds until this generator requires maintenance. A negative value indicates maintenance is past-due. INT32_MAX: field not provided. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_generator_status_send(mavlink_channel_t chan, uint64_t status, uint16_t generator_speed, float battery_current, float load_current, float power_generated, float bus_voltage, int16_t rectifier_temperature, float bat_current_setpoint, int16_t generator_temperature, uint32_t runtime, int32_t time_until_maintenance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GENERATOR_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, status); + _mav_put_float(buf, 8, battery_current); + _mav_put_float(buf, 12, load_current); + _mav_put_float(buf, 16, power_generated); + _mav_put_float(buf, 20, bus_voltage); + _mav_put_float(buf, 24, bat_current_setpoint); + _mav_put_uint32_t(buf, 28, runtime); + _mav_put_int32_t(buf, 32, time_until_maintenance); + _mav_put_uint16_t(buf, 36, generator_speed); + _mav_put_int16_t(buf, 38, rectifier_temperature); + _mav_put_int16_t(buf, 40, generator_temperature); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GENERATOR_STATUS, buf, MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_CRC); +#else + mavlink_generator_status_t packet; + packet.status = status; + packet.battery_current = battery_current; + packet.load_current = load_current; + packet.power_generated = power_generated; + packet.bus_voltage = bus_voltage; + packet.bat_current_setpoint = bat_current_setpoint; + packet.runtime = runtime; + packet.time_until_maintenance = time_until_maintenance; + packet.generator_speed = generator_speed; + packet.rectifier_temperature = rectifier_temperature; + packet.generator_temperature = generator_temperature; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GENERATOR_STATUS, (const char *)&packet, MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_CRC); +#endif +} + +/** + * @brief Send a generator_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_generator_status_send_struct(mavlink_channel_t chan, const mavlink_generator_status_t* generator_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_generator_status_send(chan, generator_status->status, generator_status->generator_speed, generator_status->battery_current, generator_status->load_current, generator_status->power_generated, generator_status->bus_voltage, generator_status->rectifier_temperature, generator_status->bat_current_setpoint, generator_status->generator_temperature, generator_status->runtime, generator_status->time_until_maintenance); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GENERATOR_STATUS, (const char *)generator_status, MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GENERATOR_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_generator_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t status, uint16_t generator_speed, float battery_current, float load_current, float power_generated, float bus_voltage, int16_t rectifier_temperature, float bat_current_setpoint, int16_t generator_temperature, uint32_t runtime, int32_t time_until_maintenance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, status); + _mav_put_float(buf, 8, battery_current); + _mav_put_float(buf, 12, load_current); + _mav_put_float(buf, 16, power_generated); + _mav_put_float(buf, 20, bus_voltage); + _mav_put_float(buf, 24, bat_current_setpoint); + _mav_put_uint32_t(buf, 28, runtime); + _mav_put_int32_t(buf, 32, time_until_maintenance); + _mav_put_uint16_t(buf, 36, generator_speed); + _mav_put_int16_t(buf, 38, rectifier_temperature); + _mav_put_int16_t(buf, 40, generator_temperature); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GENERATOR_STATUS, buf, MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_CRC); +#else + mavlink_generator_status_t *packet = (mavlink_generator_status_t *)msgbuf; + packet->status = status; + packet->battery_current = battery_current; + packet->load_current = load_current; + packet->power_generated = power_generated; + packet->bus_voltage = bus_voltage; + packet->bat_current_setpoint = bat_current_setpoint; + packet->runtime = runtime; + packet->time_until_maintenance = time_until_maintenance; + packet->generator_speed = generator_speed; + packet->rectifier_temperature = rectifier_temperature; + packet->generator_temperature = generator_temperature; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GENERATOR_STATUS, (const char *)packet, MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GENERATOR_STATUS UNPACKING + + +/** + * @brief Get field status from generator_status message + * + * @return Status flags. + */ +static inline uint64_t mavlink_msg_generator_status_get_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field generator_speed from generator_status message + * + * @return [rpm] Speed of electrical generator or alternator. UINT16_MAX: field not provided. + */ +static inline uint16_t mavlink_msg_generator_status_get_generator_speed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 36); +} + +/** + * @brief Get field battery_current from generator_status message + * + * @return [A] Current into/out of battery. Positive for out. Negative for in. NaN: field not provided. + */ +static inline float mavlink_msg_generator_status_get_battery_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field load_current from generator_status message + * + * @return [A] Current going to the UAV. If battery current not available this is the DC current from the generator. Positive for out. Negative for in. NaN: field not provided + */ +static inline float mavlink_msg_generator_status_get_load_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field power_generated from generator_status message + * + * @return [W] The power being generated. NaN: field not provided + */ +static inline float mavlink_msg_generator_status_get_power_generated(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field bus_voltage from generator_status message + * + * @return [V] Voltage of the bus seen at the generator, or battery bus if battery bus is controlled by generator and at a different voltage to main bus. + */ +static inline float mavlink_msg_generator_status_get_bus_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field rectifier_temperature from generator_status message + * + * @return [degC] The temperature of the rectifier or power converter. INT16_MAX: field not provided. + */ +static inline int16_t mavlink_msg_generator_status_get_rectifier_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 38); +} + +/** + * @brief Get field bat_current_setpoint from generator_status message + * + * @return [A] The target battery current. Positive for out. Negative for in. NaN: field not provided + */ +static inline float mavlink_msg_generator_status_get_bat_current_setpoint(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field generator_temperature from generator_status message + * + * @return [degC] The temperature of the mechanical motor, fuel cell core or generator. INT16_MAX: field not provided. + */ +static inline int16_t mavlink_msg_generator_status_get_generator_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 40); +} + +/** + * @brief Get field runtime from generator_status message + * + * @return [s] Seconds this generator has run since it was rebooted. UINT32_MAX: field not provided. + */ +static inline uint32_t mavlink_msg_generator_status_get_runtime(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 28); +} + +/** + * @brief Get field time_until_maintenance from generator_status message + * + * @return [s] Seconds until this generator requires maintenance. A negative value indicates maintenance is past-due. INT32_MAX: field not provided. + */ +static inline int32_t mavlink_msg_generator_status_get_time_until_maintenance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 32); +} + +/** + * @brief Decode a generator_status message into a struct + * + * @param msg The message to decode + * @param generator_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_generator_status_decode(const mavlink_message_t* msg, mavlink_generator_status_t* generator_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + generator_status->status = mavlink_msg_generator_status_get_status(msg); + generator_status->battery_current = mavlink_msg_generator_status_get_battery_current(msg); + generator_status->load_current = mavlink_msg_generator_status_get_load_current(msg); + generator_status->power_generated = mavlink_msg_generator_status_get_power_generated(msg); + generator_status->bus_voltage = mavlink_msg_generator_status_get_bus_voltage(msg); + generator_status->bat_current_setpoint = mavlink_msg_generator_status_get_bat_current_setpoint(msg); + generator_status->runtime = mavlink_msg_generator_status_get_runtime(msg); + generator_status->time_until_maintenance = mavlink_msg_generator_status_get_time_until_maintenance(msg); + generator_status->generator_speed = mavlink_msg_generator_status_get_generator_speed(msg); + generator_status->rectifier_temperature = mavlink_msg_generator_status_get_rectifier_temperature(msg); + generator_status->generator_temperature = mavlink_msg_generator_status_get_generator_temperature(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GENERATOR_STATUS_LEN? msg->len : MAVLINK_MSG_ID_GENERATOR_STATUS_LEN; + memset(generator_status, 0, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN); + memcpy(generator_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_device_attitude_status.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_attitude_status.h new file mode 100644 index 0000000000..63275480b4 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_attitude_status.h @@ -0,0 +1,405 @@ +#pragma once +// MESSAGE GIMBAL_DEVICE_ATTITUDE_STATUS PACKING + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS 285 + + +typedef struct __mavlink_gimbal_device_attitude_status_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + float q[4]; /*< Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set)*/ + float angular_velocity_x; /*< [rad/s] X component of angular velocity (NaN if unknown)*/ + float angular_velocity_y; /*< [rad/s] Y component of angular velocity (NaN if unknown)*/ + float angular_velocity_z; /*< [rad/s] Z component of angular velocity (NaN if unknown)*/ + uint32_t failure_flags; /*< Failure flags (0 for no failure)*/ + uint16_t flags; /*< Current gimbal flags set.*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ +} mavlink_gimbal_device_attitude_status_t; + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN 40 +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN 40 +#define MAVLINK_MSG_ID_285_LEN 40 +#define MAVLINK_MSG_ID_285_MIN_LEN 40 + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC 137 +#define MAVLINK_MSG_ID_285_CRC 137 + +#define MAVLINK_MSG_GIMBAL_DEVICE_ATTITUDE_STATUS_FIELD_Q_LEN 4 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_ATTITUDE_STATUS { \ + 285, \ + "GIMBAL_DEVICE_ATTITUDE_STATUS", \ + 9, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_gimbal_device_attitude_status_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 39, offsetof(mavlink_gimbal_device_attitude_status_t, target_component) }, \ + { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_device_attitude_status_t, time_boot_ms) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_gimbal_device_attitude_status_t, flags) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_gimbal_device_attitude_status_t, q) }, \ + { "angular_velocity_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_x) }, \ + { "angular_velocity_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_y) }, \ + { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_z) }, \ + { "failure_flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 32, offsetof(mavlink_gimbal_device_attitude_status_t, failure_flags) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_ATTITUDE_STATUS { \ + "GIMBAL_DEVICE_ATTITUDE_STATUS", \ + 9, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_gimbal_device_attitude_status_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 39, offsetof(mavlink_gimbal_device_attitude_status_t, target_component) }, \ + { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_device_attitude_status_t, time_boot_ms) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_gimbal_device_attitude_status_t, flags) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_gimbal_device_attitude_status_t, q) }, \ + { "angular_velocity_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_x) }, \ + { "angular_velocity_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_y) }, \ + { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_z) }, \ + { "failure_flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 32, offsetof(mavlink_gimbal_device_attitude_status_t, failure_flags) }, \ + } \ +} +#endif + +/** + * @brief Pack a gimbal_device_attitude_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param flags Current gimbal flags set. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) + * @param angular_velocity_x [rad/s] X component of angular velocity (NaN if unknown) + * @param angular_velocity_y [rad/s] Y component of angular velocity (NaN if unknown) + * @param angular_velocity_z [rad/s] Z component of angular velocity (NaN if unknown) + * @param failure_flags Failure flags (0 for no failure) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_device_attitude_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t time_boot_ms, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z, uint32_t failure_flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint32_t(buf, 32, failure_flags); + _mav_put_uint16_t(buf, 36, flags); + _mav_put_uint8_t(buf, 38, target_system); + _mav_put_uint8_t(buf, 39, target_component); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); +#else + mavlink_gimbal_device_attitude_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.failure_flags = failure_flags; + packet.flags = flags; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); +} + +/** + * @brief Pack a gimbal_device_attitude_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param flags Current gimbal flags set. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) + * @param angular_velocity_x [rad/s] X component of angular velocity (NaN if unknown) + * @param angular_velocity_y [rad/s] Y component of angular velocity (NaN if unknown) + * @param angular_velocity_z [rad/s] Z component of angular velocity (NaN if unknown) + * @param failure_flags Failure flags (0 for no failure) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_device_attitude_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint32_t time_boot_ms,uint16_t flags,const float *q,float angular_velocity_x,float angular_velocity_y,float angular_velocity_z,uint32_t failure_flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint32_t(buf, 32, failure_flags); + _mav_put_uint16_t(buf, 36, flags); + _mav_put_uint8_t(buf, 38, target_system); + _mav_put_uint8_t(buf, 39, target_component); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); +#else + mavlink_gimbal_device_attitude_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.failure_flags = failure_flags; + packet.flags = flags; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); +} + +/** + * @brief Encode a gimbal_device_attitude_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_device_attitude_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_device_attitude_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_device_attitude_status_t* gimbal_device_attitude_status) +{ + return mavlink_msg_gimbal_device_attitude_status_pack(system_id, component_id, msg, gimbal_device_attitude_status->target_system, gimbal_device_attitude_status->target_component, gimbal_device_attitude_status->time_boot_ms, gimbal_device_attitude_status->flags, gimbal_device_attitude_status->q, gimbal_device_attitude_status->angular_velocity_x, gimbal_device_attitude_status->angular_velocity_y, gimbal_device_attitude_status->angular_velocity_z, gimbal_device_attitude_status->failure_flags); +} + +/** + * @brief Encode a gimbal_device_attitude_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_device_attitude_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_device_attitude_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_device_attitude_status_t* gimbal_device_attitude_status) +{ + return mavlink_msg_gimbal_device_attitude_status_pack_chan(system_id, component_id, chan, msg, gimbal_device_attitude_status->target_system, gimbal_device_attitude_status->target_component, gimbal_device_attitude_status->time_boot_ms, gimbal_device_attitude_status->flags, gimbal_device_attitude_status->q, gimbal_device_attitude_status->angular_velocity_x, gimbal_device_attitude_status->angular_velocity_y, gimbal_device_attitude_status->angular_velocity_z, gimbal_device_attitude_status->failure_flags); +} + +/** + * @brief Send a gimbal_device_attitude_status message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param flags Current gimbal flags set. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) + * @param angular_velocity_x [rad/s] X component of angular velocity (NaN if unknown) + * @param angular_velocity_y [rad/s] Y component of angular velocity (NaN if unknown) + * @param angular_velocity_z [rad/s] Z component of angular velocity (NaN if unknown) + * @param failure_flags Failure flags (0 for no failure) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_device_attitude_status_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t time_boot_ms, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z, uint32_t failure_flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint32_t(buf, 32, failure_flags); + _mav_put_uint16_t(buf, 36, flags); + _mav_put_uint8_t(buf, 38, target_system); + _mav_put_uint8_t(buf, 39, target_component); + _mav_put_float_array(buf, 4, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); +#else + mavlink_gimbal_device_attitude_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.failure_flags = failure_flags; + packet.flags = flags; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); +#endif +} + +/** + * @brief Send a gimbal_device_attitude_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_gimbal_device_attitude_status_send_struct(mavlink_channel_t chan, const mavlink_gimbal_device_attitude_status_t* gimbal_device_attitude_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_device_attitude_status_send(chan, gimbal_device_attitude_status->target_system, gimbal_device_attitude_status->target_component, gimbal_device_attitude_status->time_boot_ms, gimbal_device_attitude_status->flags, gimbal_device_attitude_status->q, gimbal_device_attitude_status->angular_velocity_x, gimbal_device_attitude_status->angular_velocity_y, gimbal_device_attitude_status->angular_velocity_z, gimbal_device_attitude_status->failure_flags); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, (const char *)gimbal_device_attitude_status, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_device_attitude_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t time_boot_ms, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z, uint32_t failure_flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint32_t(buf, 32, failure_flags); + _mav_put_uint16_t(buf, 36, flags); + _mav_put_uint8_t(buf, 38, target_system); + _mav_put_uint8_t(buf, 39, target_component); + _mav_put_float_array(buf, 4, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); +#else + mavlink_gimbal_device_attitude_status_t *packet = (mavlink_gimbal_device_attitude_status_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->angular_velocity_x = angular_velocity_x; + packet->angular_velocity_y = angular_velocity_y; + packet->angular_velocity_z = angular_velocity_z; + packet->failure_flags = failure_flags; + packet->flags = flags; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_DEVICE_ATTITUDE_STATUS UNPACKING + + +/** + * @brief Get field target_system from gimbal_device_attitude_status message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gimbal_device_attitude_status_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 38); +} + +/** + * @brief Get field target_component from gimbal_device_attitude_status message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gimbal_device_attitude_status_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 39); +} + +/** + * @brief Get field time_boot_ms from gimbal_device_attitude_status message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_gimbal_device_attitude_status_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field flags from gimbal_device_attitude_status message + * + * @return Current gimbal flags set. + */ +static inline uint16_t mavlink_msg_gimbal_device_attitude_status_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 36); +} + +/** + * @brief Get field q from gimbal_device_attitude_status message + * + * @return Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) + */ +static inline uint16_t mavlink_msg_gimbal_device_attitude_status_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 4); +} + +/** + * @brief Get field angular_velocity_x from gimbal_device_attitude_status message + * + * @return [rad/s] X component of angular velocity (NaN if unknown) + */ +static inline float mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field angular_velocity_y from gimbal_device_attitude_status message + * + * @return [rad/s] Y component of angular velocity (NaN if unknown) + */ +static inline float mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field angular_velocity_z from gimbal_device_attitude_status message + * + * @return [rad/s] Z component of angular velocity (NaN if unknown) + */ +static inline float mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field failure_flags from gimbal_device_attitude_status message + * + * @return Failure flags (0 for no failure) + */ +static inline uint32_t mavlink_msg_gimbal_device_attitude_status_get_failure_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 32); +} + +/** + * @brief Decode a gimbal_device_attitude_status message into a struct + * + * @param msg The message to decode + * @param gimbal_device_attitude_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_device_attitude_status_decode(const mavlink_message_t* msg, mavlink_gimbal_device_attitude_status_t* gimbal_device_attitude_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + gimbal_device_attitude_status->time_boot_ms = mavlink_msg_gimbal_device_attitude_status_get_time_boot_ms(msg); + mavlink_msg_gimbal_device_attitude_status_get_q(msg, gimbal_device_attitude_status->q); + gimbal_device_attitude_status->angular_velocity_x = mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_x(msg); + gimbal_device_attitude_status->angular_velocity_y = mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_y(msg); + gimbal_device_attitude_status->angular_velocity_z = mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_z(msg); + gimbal_device_attitude_status->failure_flags = mavlink_msg_gimbal_device_attitude_status_get_failure_flags(msg); + gimbal_device_attitude_status->flags = mavlink_msg_gimbal_device_attitude_status_get_flags(msg); + gimbal_device_attitude_status->target_system = mavlink_msg_gimbal_device_attitude_status_get_target_system(msg); + gimbal_device_attitude_status->target_component = mavlink_msg_gimbal_device_attitude_status_get_target_component(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN; + memset(gimbal_device_attitude_status, 0, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); + memcpy(gimbal_device_attitude_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_device_information.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_information.h new file mode 100644 index 0000000000..2f0c86bdc9 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_information.h @@ -0,0 +1,557 @@ +#pragma once +// MESSAGE GIMBAL_DEVICE_INFORMATION PACKING + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION 283 + + +typedef struct __mavlink_gimbal_device_information_t { + uint64_t uid; /*< UID of gimbal hardware (0 if unknown).*/ + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + uint32_t firmware_version; /*< Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff).*/ + uint32_t hardware_version; /*< Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff).*/ + float roll_min; /*< [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left)*/ + float roll_max; /*< [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left)*/ + float pitch_min; /*< [rad] Minimum hardware pitch angle (positive: up, negative: down)*/ + float pitch_max; /*< [rad] Maximum hardware pitch angle (positive: up, negative: down)*/ + float yaw_min; /*< [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left)*/ + float yaw_max; /*< [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left)*/ + uint16_t cap_flags; /*< Bitmap of gimbal capability flags.*/ + uint16_t custom_cap_flags; /*< Bitmap for use for gimbal-specific capability flags.*/ + char vendor_name[32]; /*< Name of the gimbal vendor.*/ + char model_name[32]; /*< Name of the gimbal model.*/ + char custom_name[32]; /*< Custom name of the gimbal given to it by the user.*/ +} mavlink_gimbal_device_information_t; + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN 144 +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN 144 +#define MAVLINK_MSG_ID_283_LEN 144 +#define MAVLINK_MSG_ID_283_MIN_LEN 144 + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC 74 +#define MAVLINK_MSG_ID_283_CRC 74 + +#define MAVLINK_MSG_GIMBAL_DEVICE_INFORMATION_FIELD_VENDOR_NAME_LEN 32 +#define MAVLINK_MSG_GIMBAL_DEVICE_INFORMATION_FIELD_MODEL_NAME_LEN 32 +#define MAVLINK_MSG_GIMBAL_DEVICE_INFORMATION_FIELD_CUSTOM_NAME_LEN 32 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_INFORMATION { \ + 283, \ + "GIMBAL_DEVICE_INFORMATION", \ + 15, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_gimbal_device_information_t, time_boot_ms) }, \ + { "vendor_name", NULL, MAVLINK_TYPE_CHAR, 32, 48, offsetof(mavlink_gimbal_device_information_t, vendor_name) }, \ + { "model_name", NULL, MAVLINK_TYPE_CHAR, 32, 80, offsetof(mavlink_gimbal_device_information_t, model_name) }, \ + { "custom_name", NULL, MAVLINK_TYPE_CHAR, 32, 112, offsetof(mavlink_gimbal_device_information_t, custom_name) }, \ + { "firmware_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_gimbal_device_information_t, firmware_version) }, \ + { "hardware_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 16, offsetof(mavlink_gimbal_device_information_t, hardware_version) }, \ + { "uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gimbal_device_information_t, uid) }, \ + { "cap_flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 44, offsetof(mavlink_gimbal_device_information_t, cap_flags) }, \ + { "custom_cap_flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 46, offsetof(mavlink_gimbal_device_information_t, custom_cap_flags) }, \ + { "roll_min", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_device_information_t, roll_min) }, \ + { "roll_max", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_device_information_t, roll_max) }, \ + { "pitch_min", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_device_information_t, pitch_min) }, \ + { "pitch_max", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_gimbal_device_information_t, pitch_max) }, \ + { "yaw_min", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_gimbal_device_information_t, yaw_min) }, \ + { "yaw_max", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_gimbal_device_information_t, yaw_max) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_INFORMATION { \ + "GIMBAL_DEVICE_INFORMATION", \ + 15, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_gimbal_device_information_t, time_boot_ms) }, \ + { "vendor_name", NULL, MAVLINK_TYPE_CHAR, 32, 48, offsetof(mavlink_gimbal_device_information_t, vendor_name) }, \ + { "model_name", NULL, MAVLINK_TYPE_CHAR, 32, 80, offsetof(mavlink_gimbal_device_information_t, model_name) }, \ + { "custom_name", NULL, MAVLINK_TYPE_CHAR, 32, 112, offsetof(mavlink_gimbal_device_information_t, custom_name) }, \ + { "firmware_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_gimbal_device_information_t, firmware_version) }, \ + { "hardware_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 16, offsetof(mavlink_gimbal_device_information_t, hardware_version) }, \ + { "uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gimbal_device_information_t, uid) }, \ + { "cap_flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 44, offsetof(mavlink_gimbal_device_information_t, cap_flags) }, \ + { "custom_cap_flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 46, offsetof(mavlink_gimbal_device_information_t, custom_cap_flags) }, \ + { "roll_min", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_device_information_t, roll_min) }, \ + { "roll_max", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_device_information_t, roll_max) }, \ + { "pitch_min", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_device_information_t, pitch_min) }, \ + { "pitch_max", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_gimbal_device_information_t, pitch_max) }, \ + { "yaw_min", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_gimbal_device_information_t, yaw_min) }, \ + { "yaw_max", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_gimbal_device_information_t, yaw_max) }, \ + } \ +} +#endif + +/** + * @brief Pack a gimbal_device_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param vendor_name Name of the gimbal vendor. + * @param model_name Name of the gimbal model. + * @param custom_name Custom name of the gimbal given to it by the user. + * @param firmware_version Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + * @param hardware_version Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + * @param uid UID of gimbal hardware (0 if unknown). + * @param cap_flags Bitmap of gimbal capability flags. + * @param custom_cap_flags Bitmap for use for gimbal-specific capability flags. + * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param pitch_min [rad] Minimum hardware pitch angle (positive: up, negative: down) + * @param pitch_max [rad] Maximum hardware pitch angle (positive: up, negative: down) + * @param yaw_min [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left) + * @param yaw_max [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_device_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, const char *vendor_name, const char *model_name, const char *custom_name, uint32_t firmware_version, uint32_t hardware_version, uint64_t uid, uint16_t cap_flags, uint16_t custom_cap_flags, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN]; + _mav_put_uint64_t(buf, 0, uid); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_uint32_t(buf, 12, firmware_version); + _mav_put_uint32_t(buf, 16, hardware_version); + _mav_put_float(buf, 20, roll_min); + _mav_put_float(buf, 24, roll_max); + _mav_put_float(buf, 28, pitch_min); + _mav_put_float(buf, 32, pitch_max); + _mav_put_float(buf, 36, yaw_min); + _mav_put_float(buf, 40, yaw_max); + _mav_put_uint16_t(buf, 44, cap_flags); + _mav_put_uint16_t(buf, 46, custom_cap_flags); + _mav_put_char_array(buf, 48, vendor_name, 32); + _mav_put_char_array(buf, 80, model_name, 32); + _mav_put_char_array(buf, 112, custom_name, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN); +#else + mavlink_gimbal_device_information_t packet; + packet.uid = uid; + packet.time_boot_ms = time_boot_ms; + packet.firmware_version = firmware_version; + packet.hardware_version = hardware_version; + packet.roll_min = roll_min; + packet.roll_max = roll_max; + packet.pitch_min = pitch_min; + packet.pitch_max = pitch_max; + packet.yaw_min = yaw_min; + packet.yaw_max = yaw_max; + packet.cap_flags = cap_flags; + packet.custom_cap_flags = custom_cap_flags; + mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(char)*32); + mav_array_memcpy(packet.model_name, model_name, sizeof(char)*32); + mav_array_memcpy(packet.custom_name, custom_name, sizeof(char)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); +} + +/** + * @brief Pack a gimbal_device_information message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param vendor_name Name of the gimbal vendor. + * @param model_name Name of the gimbal model. + * @param custom_name Custom name of the gimbal given to it by the user. + * @param firmware_version Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + * @param hardware_version Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + * @param uid UID of gimbal hardware (0 if unknown). + * @param cap_flags Bitmap of gimbal capability flags. + * @param custom_cap_flags Bitmap for use for gimbal-specific capability flags. + * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param pitch_min [rad] Minimum hardware pitch angle (positive: up, negative: down) + * @param pitch_max [rad] Maximum hardware pitch angle (positive: up, negative: down) + * @param yaw_min [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left) + * @param yaw_max [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_device_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,const char *vendor_name,const char *model_name,const char *custom_name,uint32_t firmware_version,uint32_t hardware_version,uint64_t uid,uint16_t cap_flags,uint16_t custom_cap_flags,float roll_min,float roll_max,float pitch_min,float pitch_max,float yaw_min,float yaw_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN]; + _mav_put_uint64_t(buf, 0, uid); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_uint32_t(buf, 12, firmware_version); + _mav_put_uint32_t(buf, 16, hardware_version); + _mav_put_float(buf, 20, roll_min); + _mav_put_float(buf, 24, roll_max); + _mav_put_float(buf, 28, pitch_min); + _mav_put_float(buf, 32, pitch_max); + _mav_put_float(buf, 36, yaw_min); + _mav_put_float(buf, 40, yaw_max); + _mav_put_uint16_t(buf, 44, cap_flags); + _mav_put_uint16_t(buf, 46, custom_cap_flags); + _mav_put_char_array(buf, 48, vendor_name, 32); + _mav_put_char_array(buf, 80, model_name, 32); + _mav_put_char_array(buf, 112, custom_name, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN); +#else + mavlink_gimbal_device_information_t packet; + packet.uid = uid; + packet.time_boot_ms = time_boot_ms; + packet.firmware_version = firmware_version; + packet.hardware_version = hardware_version; + packet.roll_min = roll_min; + packet.roll_max = roll_max; + packet.pitch_min = pitch_min; + packet.pitch_max = pitch_max; + packet.yaw_min = yaw_min; + packet.yaw_max = yaw_max; + packet.cap_flags = cap_flags; + packet.custom_cap_flags = custom_cap_flags; + mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(char)*32); + mav_array_memcpy(packet.model_name, model_name, sizeof(char)*32); + mav_array_memcpy(packet.custom_name, custom_name, sizeof(char)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); +} + +/** + * @brief Encode a gimbal_device_information struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_device_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_device_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_device_information_t* gimbal_device_information) +{ + return mavlink_msg_gimbal_device_information_pack(system_id, component_id, msg, gimbal_device_information->time_boot_ms, gimbal_device_information->vendor_name, gimbal_device_information->model_name, gimbal_device_information->custom_name, gimbal_device_information->firmware_version, gimbal_device_information->hardware_version, gimbal_device_information->uid, gimbal_device_information->cap_flags, gimbal_device_information->custom_cap_flags, gimbal_device_information->roll_min, gimbal_device_information->roll_max, gimbal_device_information->pitch_min, gimbal_device_information->pitch_max, gimbal_device_information->yaw_min, gimbal_device_information->yaw_max); +} + +/** + * @brief Encode a gimbal_device_information struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_device_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_device_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_device_information_t* gimbal_device_information) +{ + return mavlink_msg_gimbal_device_information_pack_chan(system_id, component_id, chan, msg, gimbal_device_information->time_boot_ms, gimbal_device_information->vendor_name, gimbal_device_information->model_name, gimbal_device_information->custom_name, gimbal_device_information->firmware_version, gimbal_device_information->hardware_version, gimbal_device_information->uid, gimbal_device_information->cap_flags, gimbal_device_information->custom_cap_flags, gimbal_device_information->roll_min, gimbal_device_information->roll_max, gimbal_device_information->pitch_min, gimbal_device_information->pitch_max, gimbal_device_information->yaw_min, gimbal_device_information->yaw_max); +} + +/** + * @brief Send a gimbal_device_information message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param vendor_name Name of the gimbal vendor. + * @param model_name Name of the gimbal model. + * @param custom_name Custom name of the gimbal given to it by the user. + * @param firmware_version Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + * @param hardware_version Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + * @param uid UID of gimbal hardware (0 if unknown). + * @param cap_flags Bitmap of gimbal capability flags. + * @param custom_cap_flags Bitmap for use for gimbal-specific capability flags. + * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param pitch_min [rad] Minimum hardware pitch angle (positive: up, negative: down) + * @param pitch_max [rad] Maximum hardware pitch angle (positive: up, negative: down) + * @param yaw_min [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left) + * @param yaw_max [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_device_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, const char *vendor_name, const char *model_name, const char *custom_name, uint32_t firmware_version, uint32_t hardware_version, uint64_t uid, uint16_t cap_flags, uint16_t custom_cap_flags, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN]; + _mav_put_uint64_t(buf, 0, uid); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_uint32_t(buf, 12, firmware_version); + _mav_put_uint32_t(buf, 16, hardware_version); + _mav_put_float(buf, 20, roll_min); + _mav_put_float(buf, 24, roll_max); + _mav_put_float(buf, 28, pitch_min); + _mav_put_float(buf, 32, pitch_max); + _mav_put_float(buf, 36, yaw_min); + _mav_put_float(buf, 40, yaw_max); + _mav_put_uint16_t(buf, 44, cap_flags); + _mav_put_uint16_t(buf, 46, custom_cap_flags); + _mav_put_char_array(buf, 48, vendor_name, 32); + _mav_put_char_array(buf, 80, model_name, 32); + _mav_put_char_array(buf, 112, custom_name, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); +#else + mavlink_gimbal_device_information_t packet; + packet.uid = uid; + packet.time_boot_ms = time_boot_ms; + packet.firmware_version = firmware_version; + packet.hardware_version = hardware_version; + packet.roll_min = roll_min; + packet.roll_max = roll_max; + packet.pitch_min = pitch_min; + packet.pitch_max = pitch_max; + packet.yaw_min = yaw_min; + packet.yaw_max = yaw_max; + packet.cap_flags = cap_flags; + packet.custom_cap_flags = custom_cap_flags; + mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(char)*32); + mav_array_memcpy(packet.model_name, model_name, sizeof(char)*32); + mav_array_memcpy(packet.custom_name, custom_name, sizeof(char)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); +#endif +} + +/** + * @brief Send a gimbal_device_information message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_gimbal_device_information_send_struct(mavlink_channel_t chan, const mavlink_gimbal_device_information_t* gimbal_device_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_device_information_send(chan, gimbal_device_information->time_boot_ms, gimbal_device_information->vendor_name, gimbal_device_information->model_name, gimbal_device_information->custom_name, gimbal_device_information->firmware_version, gimbal_device_information->hardware_version, gimbal_device_information->uid, gimbal_device_information->cap_flags, gimbal_device_information->custom_cap_flags, gimbal_device_information->roll_min, gimbal_device_information->roll_max, gimbal_device_information->pitch_min, gimbal_device_information->pitch_max, gimbal_device_information->yaw_min, gimbal_device_information->yaw_max); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, (const char *)gimbal_device_information, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_device_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const char *vendor_name, const char *model_name, const char *custom_name, uint32_t firmware_version, uint32_t hardware_version, uint64_t uid, uint16_t cap_flags, uint16_t custom_cap_flags, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, uid); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_uint32_t(buf, 12, firmware_version); + _mav_put_uint32_t(buf, 16, hardware_version); + _mav_put_float(buf, 20, roll_min); + _mav_put_float(buf, 24, roll_max); + _mav_put_float(buf, 28, pitch_min); + _mav_put_float(buf, 32, pitch_max); + _mav_put_float(buf, 36, yaw_min); + _mav_put_float(buf, 40, yaw_max); + _mav_put_uint16_t(buf, 44, cap_flags); + _mav_put_uint16_t(buf, 46, custom_cap_flags); + _mav_put_char_array(buf, 48, vendor_name, 32); + _mav_put_char_array(buf, 80, model_name, 32); + _mav_put_char_array(buf, 112, custom_name, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); +#else + mavlink_gimbal_device_information_t *packet = (mavlink_gimbal_device_information_t *)msgbuf; + packet->uid = uid; + packet->time_boot_ms = time_boot_ms; + packet->firmware_version = firmware_version; + packet->hardware_version = hardware_version; + packet->roll_min = roll_min; + packet->roll_max = roll_max; + packet->pitch_min = pitch_min; + packet->pitch_max = pitch_max; + packet->yaw_min = yaw_min; + packet->yaw_max = yaw_max; + packet->cap_flags = cap_flags; + packet->custom_cap_flags = custom_cap_flags; + mav_array_memcpy(packet->vendor_name, vendor_name, sizeof(char)*32); + mav_array_memcpy(packet->model_name, model_name, sizeof(char)*32); + mav_array_memcpy(packet->custom_name, custom_name, sizeof(char)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_DEVICE_INFORMATION UNPACKING + + +/** + * @brief Get field time_boot_ms from gimbal_device_information message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_gimbal_device_information_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field vendor_name from gimbal_device_information message + * + * @return Name of the gimbal vendor. + */ +static inline uint16_t mavlink_msg_gimbal_device_information_get_vendor_name(const mavlink_message_t* msg, char *vendor_name) +{ + return _MAV_RETURN_char_array(msg, vendor_name, 32, 48); +} + +/** + * @brief Get field model_name from gimbal_device_information message + * + * @return Name of the gimbal model. + */ +static inline uint16_t mavlink_msg_gimbal_device_information_get_model_name(const mavlink_message_t* msg, char *model_name) +{ + return _MAV_RETURN_char_array(msg, model_name, 32, 80); +} + +/** + * @brief Get field custom_name from gimbal_device_information message + * + * @return Custom name of the gimbal given to it by the user. + */ +static inline uint16_t mavlink_msg_gimbal_device_information_get_custom_name(const mavlink_message_t* msg, char *custom_name) +{ + return _MAV_RETURN_char_array(msg, custom_name, 32, 112); +} + +/** + * @brief Get field firmware_version from gimbal_device_information message + * + * @return Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + */ +static inline uint32_t mavlink_msg_gimbal_device_information_get_firmware_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 12); +} + +/** + * @brief Get field hardware_version from gimbal_device_information message + * + * @return Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + */ +static inline uint32_t mavlink_msg_gimbal_device_information_get_hardware_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 16); +} + +/** + * @brief Get field uid from gimbal_device_information message + * + * @return UID of gimbal hardware (0 if unknown). + */ +static inline uint64_t mavlink_msg_gimbal_device_information_get_uid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field cap_flags from gimbal_device_information message + * + * @return Bitmap of gimbal capability flags. + */ +static inline uint16_t mavlink_msg_gimbal_device_information_get_cap_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 44); +} + +/** + * @brief Get field custom_cap_flags from gimbal_device_information message + * + * @return Bitmap for use for gimbal-specific capability flags. + */ +static inline uint16_t mavlink_msg_gimbal_device_information_get_custom_cap_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 46); +} + +/** + * @brief Get field roll_min from gimbal_device_information message + * + * @return [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + */ +static inline float mavlink_msg_gimbal_device_information_get_roll_min(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field roll_max from gimbal_device_information message + * + * @return [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + */ +static inline float mavlink_msg_gimbal_device_information_get_roll_max(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field pitch_min from gimbal_device_information message + * + * @return [rad] Minimum hardware pitch angle (positive: up, negative: down) + */ +static inline float mavlink_msg_gimbal_device_information_get_pitch_min(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field pitch_max from gimbal_device_information message + * + * @return [rad] Maximum hardware pitch angle (positive: up, negative: down) + */ +static inline float mavlink_msg_gimbal_device_information_get_pitch_max(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field yaw_min from gimbal_device_information message + * + * @return [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left) + */ +static inline float mavlink_msg_gimbal_device_information_get_yaw_min(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field yaw_max from gimbal_device_information message + * + * @return [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left) + */ +static inline float mavlink_msg_gimbal_device_information_get_yaw_max(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Decode a gimbal_device_information message into a struct + * + * @param msg The message to decode + * @param gimbal_device_information C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_device_information_decode(const mavlink_message_t* msg, mavlink_gimbal_device_information_t* gimbal_device_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + gimbal_device_information->uid = mavlink_msg_gimbal_device_information_get_uid(msg); + gimbal_device_information->time_boot_ms = mavlink_msg_gimbal_device_information_get_time_boot_ms(msg); + gimbal_device_information->firmware_version = mavlink_msg_gimbal_device_information_get_firmware_version(msg); + gimbal_device_information->hardware_version = mavlink_msg_gimbal_device_information_get_hardware_version(msg); + gimbal_device_information->roll_min = mavlink_msg_gimbal_device_information_get_roll_min(msg); + gimbal_device_information->roll_max = mavlink_msg_gimbal_device_information_get_roll_max(msg); + gimbal_device_information->pitch_min = mavlink_msg_gimbal_device_information_get_pitch_min(msg); + gimbal_device_information->pitch_max = mavlink_msg_gimbal_device_information_get_pitch_max(msg); + gimbal_device_information->yaw_min = mavlink_msg_gimbal_device_information_get_yaw_min(msg); + gimbal_device_information->yaw_max = mavlink_msg_gimbal_device_information_get_yaw_max(msg); + gimbal_device_information->cap_flags = mavlink_msg_gimbal_device_information_get_cap_flags(msg); + gimbal_device_information->custom_cap_flags = mavlink_msg_gimbal_device_information_get_custom_cap_flags(msg); + mavlink_msg_gimbal_device_information_get_vendor_name(msg, gimbal_device_information->vendor_name); + mavlink_msg_gimbal_device_information_get_model_name(msg, gimbal_device_information->model_name); + mavlink_msg_gimbal_device_information_get_custom_name(msg, gimbal_device_information->custom_name); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN; + memset(gimbal_device_information, 0, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN); + memcpy(gimbal_device_information, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_device_set_attitude.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_set_attitude.h new file mode 100644 index 0000000000..1cfda32666 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_set_attitude.h @@ -0,0 +1,355 @@ +#pragma once +// MESSAGE GIMBAL_DEVICE_SET_ATTITUDE PACKING + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE 284 + + +typedef struct __mavlink_gimbal_device_set_attitude_t { + float q[4]; /*< Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, set all fields to NaN if only angular velocity should be used)*/ + float angular_velocity_x; /*< [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored.*/ + float angular_velocity_y; /*< [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored.*/ + float angular_velocity_z; /*< [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored.*/ + uint16_t flags; /*< Low level gimbal flags.*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ +} mavlink_gimbal_device_set_attitude_t; + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN 32 +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN 32 +#define MAVLINK_MSG_ID_284_LEN 32 +#define MAVLINK_MSG_ID_284_MIN_LEN 32 + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC 99 +#define MAVLINK_MSG_ID_284_CRC 99 + +#define MAVLINK_MSG_GIMBAL_DEVICE_SET_ATTITUDE_FIELD_Q_LEN 4 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_SET_ATTITUDE { \ + 284, \ + "GIMBAL_DEVICE_SET_ATTITUDE", \ + 7, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_gimbal_device_set_attitude_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_gimbal_device_set_attitude_t, target_component) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gimbal_device_set_attitude_t, flags) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 0, offsetof(mavlink_gimbal_device_set_attitude_t, q) }, \ + { "angular_velocity_x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gimbal_device_set_attitude_t, angular_velocity_x) }, \ + { "angular_velocity_y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_device_set_attitude_t, angular_velocity_y) }, \ + { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_device_set_attitude_t, angular_velocity_z) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_SET_ATTITUDE { \ + "GIMBAL_DEVICE_SET_ATTITUDE", \ + 7, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_gimbal_device_set_attitude_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_gimbal_device_set_attitude_t, target_component) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gimbal_device_set_attitude_t, flags) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 0, offsetof(mavlink_gimbal_device_set_attitude_t, q) }, \ + { "angular_velocity_x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gimbal_device_set_attitude_t, angular_velocity_x) }, \ + { "angular_velocity_y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_device_set_attitude_t, angular_velocity_y) }, \ + { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_device_set_attitude_t, angular_velocity_z) }, \ + } \ +} +#endif + +/** + * @brief Pack a gimbal_device_set_attitude message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param flags Low level gimbal flags. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, set all fields to NaN if only angular velocity should be used) + * @param angular_velocity_x [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + * @param angular_velocity_y [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + * @param angular_velocity_z [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_device_set_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN]; + _mav_put_float(buf, 16, angular_velocity_x); + _mav_put_float(buf, 20, angular_velocity_y); + _mav_put_float(buf, 24, angular_velocity_z); + _mav_put_uint16_t(buf, 28, flags); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_float_array(buf, 0, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN); +#else + mavlink_gimbal_device_set_attitude_t packet; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.flags = flags; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); +} + +/** + * @brief Pack a gimbal_device_set_attitude message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param flags Low level gimbal flags. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, set all fields to NaN if only angular velocity should be used) + * @param angular_velocity_x [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + * @param angular_velocity_y [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + * @param angular_velocity_z [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_device_set_attitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t flags,const float *q,float angular_velocity_x,float angular_velocity_y,float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN]; + _mav_put_float(buf, 16, angular_velocity_x); + _mav_put_float(buf, 20, angular_velocity_y); + _mav_put_float(buf, 24, angular_velocity_z); + _mav_put_uint16_t(buf, 28, flags); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_float_array(buf, 0, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN); +#else + mavlink_gimbal_device_set_attitude_t packet; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.flags = flags; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); +} + +/** + * @brief Encode a gimbal_device_set_attitude struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_device_set_attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_device_set_attitude_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_device_set_attitude_t* gimbal_device_set_attitude) +{ + return mavlink_msg_gimbal_device_set_attitude_pack(system_id, component_id, msg, gimbal_device_set_attitude->target_system, gimbal_device_set_attitude->target_component, gimbal_device_set_attitude->flags, gimbal_device_set_attitude->q, gimbal_device_set_attitude->angular_velocity_x, gimbal_device_set_attitude->angular_velocity_y, gimbal_device_set_attitude->angular_velocity_z); +} + +/** + * @brief Encode a gimbal_device_set_attitude struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_device_set_attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_device_set_attitude_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_device_set_attitude_t* gimbal_device_set_attitude) +{ + return mavlink_msg_gimbal_device_set_attitude_pack_chan(system_id, component_id, chan, msg, gimbal_device_set_attitude->target_system, gimbal_device_set_attitude->target_component, gimbal_device_set_attitude->flags, gimbal_device_set_attitude->q, gimbal_device_set_attitude->angular_velocity_x, gimbal_device_set_attitude->angular_velocity_y, gimbal_device_set_attitude->angular_velocity_z); +} + +/** + * @brief Send a gimbal_device_set_attitude message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param flags Low level gimbal flags. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, set all fields to NaN if only angular velocity should be used) + * @param angular_velocity_x [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + * @param angular_velocity_y [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + * @param angular_velocity_z [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_device_set_attitude_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN]; + _mav_put_float(buf, 16, angular_velocity_x); + _mav_put_float(buf, 20, angular_velocity_y); + _mav_put_float(buf, 24, angular_velocity_z); + _mav_put_uint16_t(buf, 28, flags); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_float_array(buf, 0, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); +#else + mavlink_gimbal_device_set_attitude_t packet; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.flags = flags; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); +#endif +} + +/** + * @brief Send a gimbal_device_set_attitude message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_gimbal_device_set_attitude_send_struct(mavlink_channel_t chan, const mavlink_gimbal_device_set_attitude_t* gimbal_device_set_attitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_device_set_attitude_send(chan, gimbal_device_set_attitude->target_system, gimbal_device_set_attitude->target_component, gimbal_device_set_attitude->flags, gimbal_device_set_attitude->q, gimbal_device_set_attitude->angular_velocity_x, gimbal_device_set_attitude->angular_velocity_y, gimbal_device_set_attitude->angular_velocity_z); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char *)gimbal_device_set_attitude, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_device_set_attitude_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 16, angular_velocity_x); + _mav_put_float(buf, 20, angular_velocity_y); + _mav_put_float(buf, 24, angular_velocity_z); + _mav_put_uint16_t(buf, 28, flags); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_float_array(buf, 0, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); +#else + mavlink_gimbal_device_set_attitude_t *packet = (mavlink_gimbal_device_set_attitude_t *)msgbuf; + packet->angular_velocity_x = angular_velocity_x; + packet->angular_velocity_y = angular_velocity_y; + packet->angular_velocity_z = angular_velocity_z; + packet->flags = flags; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_DEVICE_SET_ATTITUDE UNPACKING + + +/** + * @brief Get field target_system from gimbal_device_set_attitude message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gimbal_device_set_attitude_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 30); +} + +/** + * @brief Get field target_component from gimbal_device_set_attitude message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gimbal_device_set_attitude_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 31); +} + +/** + * @brief Get field flags from gimbal_device_set_attitude message + * + * @return Low level gimbal flags. + */ +static inline uint16_t mavlink_msg_gimbal_device_set_attitude_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field q from gimbal_device_set_attitude message + * + * @return Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, set all fields to NaN if only angular velocity should be used) + */ +static inline uint16_t mavlink_msg_gimbal_device_set_attitude_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 0); +} + +/** + * @brief Get field angular_velocity_x from gimbal_device_set_attitude message + * + * @return [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + */ +static inline float mavlink_msg_gimbal_device_set_attitude_get_angular_velocity_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field angular_velocity_y from gimbal_device_set_attitude message + * + * @return [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + */ +static inline float mavlink_msg_gimbal_device_set_attitude_get_angular_velocity_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field angular_velocity_z from gimbal_device_set_attitude message + * + * @return [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + */ +static inline float mavlink_msg_gimbal_device_set_attitude_get_angular_velocity_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a gimbal_device_set_attitude message into a struct + * + * @param msg The message to decode + * @param gimbal_device_set_attitude C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_device_set_attitude_decode(const mavlink_message_t* msg, mavlink_gimbal_device_set_attitude_t* gimbal_device_set_attitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_device_set_attitude_get_q(msg, gimbal_device_set_attitude->q); + gimbal_device_set_attitude->angular_velocity_x = mavlink_msg_gimbal_device_set_attitude_get_angular_velocity_x(msg); + gimbal_device_set_attitude->angular_velocity_y = mavlink_msg_gimbal_device_set_attitude_get_angular_velocity_y(msg); + gimbal_device_set_attitude->angular_velocity_z = mavlink_msg_gimbal_device_set_attitude_get_angular_velocity_z(msg); + gimbal_device_set_attitude->flags = mavlink_msg_gimbal_device_set_attitude_get_flags(msg); + gimbal_device_set_attitude->target_system = mavlink_msg_gimbal_device_set_attitude_get_target_system(msg); + gimbal_device_set_attitude->target_component = mavlink_msg_gimbal_device_set_attitude_get_target_component(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN; + memset(gimbal_device_set_attitude, 0, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN); + memcpy(gimbal_device_set_attitude, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_information.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_information.h new file mode 100644 index 0000000000..58c8c59d97 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_information.h @@ -0,0 +1,413 @@ +#pragma once +// MESSAGE GIMBAL_MANAGER_INFORMATION PACKING + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION 280 + + +typedef struct __mavlink_gimbal_manager_information_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + uint32_t cap_flags; /*< Bitmap of gimbal capability flags.*/ + float roll_min; /*< [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left)*/ + float roll_max; /*< [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left)*/ + float pitch_min; /*< [rad] Minimum pitch angle (positive: up, negative: down)*/ + float pitch_max; /*< [rad] Maximum pitch angle (positive: up, negative: down)*/ + float yaw_min; /*< [rad] Minimum yaw angle (positive: to the right, negative: to the left)*/ + float yaw_max; /*< [rad] Maximum yaw angle (positive: to the right, negative: to the left)*/ + uint8_t gimbal_device_id; /*< Gimbal device ID that this gimbal manager is responsible for.*/ +} mavlink_gimbal_manager_information_t; + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN 33 +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN 33 +#define MAVLINK_MSG_ID_280_LEN 33 +#define MAVLINK_MSG_ID_280_MIN_LEN 33 + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC 70 +#define MAVLINK_MSG_ID_280_CRC 70 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_INFORMATION { \ + 280, \ + "GIMBAL_MANAGER_INFORMATION", \ + 9, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_information_t, time_boot_ms) }, \ + { "cap_flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gimbal_manager_information_t, cap_flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gimbal_manager_information_t, gimbal_device_id) }, \ + { "roll_min", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gimbal_manager_information_t, roll_min) }, \ + { "roll_max", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gimbal_manager_information_t, roll_max) }, \ + { "pitch_min", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gimbal_manager_information_t, pitch_min) }, \ + { "pitch_max", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_manager_information_t, pitch_max) }, \ + { "yaw_min", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_manager_information_t, yaw_min) }, \ + { "yaw_max", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_manager_information_t, yaw_max) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_INFORMATION { \ + "GIMBAL_MANAGER_INFORMATION", \ + 9, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_information_t, time_boot_ms) }, \ + { "cap_flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gimbal_manager_information_t, cap_flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gimbal_manager_information_t, gimbal_device_id) }, \ + { "roll_min", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gimbal_manager_information_t, roll_min) }, \ + { "roll_max", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gimbal_manager_information_t, roll_max) }, \ + { "pitch_min", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gimbal_manager_information_t, pitch_min) }, \ + { "pitch_max", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_manager_information_t, pitch_max) }, \ + { "yaw_min", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_manager_information_t, yaw_min) }, \ + { "yaw_max", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_manager_information_t, yaw_max) }, \ + } \ +} +#endif + +/** + * @brief Pack a gimbal_manager_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param cap_flags Bitmap of gimbal capability flags. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param pitch_min [rad] Minimum pitch angle (positive: up, negative: down) + * @param pitch_max [rad] Maximum pitch angle (positive: up, negative: down) + * @param yaw_min [rad] Minimum yaw angle (positive: to the right, negative: to the left) + * @param yaw_max [rad] Maximum yaw angle (positive: to the right, negative: to the left) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint32_t cap_flags, uint8_t gimbal_device_id, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, cap_flags); + _mav_put_float(buf, 8, roll_min); + _mav_put_float(buf, 12, roll_max); + _mav_put_float(buf, 16, pitch_min); + _mav_put_float(buf, 20, pitch_max); + _mav_put_float(buf, 24, yaw_min); + _mav_put_float(buf, 28, yaw_max); + _mav_put_uint8_t(buf, 32, gimbal_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN); +#else + mavlink_gimbal_manager_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.cap_flags = cap_flags; + packet.roll_min = roll_min; + packet.roll_max = roll_max; + packet.pitch_min = pitch_min; + packet.pitch_max = pitch_max; + packet.yaw_min = yaw_min; + packet.yaw_max = yaw_max; + packet.gimbal_device_id = gimbal_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC); +} + +/** + * @brief Pack a gimbal_manager_information message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param cap_flags Bitmap of gimbal capability flags. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param pitch_min [rad] Minimum pitch angle (positive: up, negative: down) + * @param pitch_max [rad] Maximum pitch angle (positive: up, negative: down) + * @param yaw_min [rad] Minimum yaw angle (positive: to the right, negative: to the left) + * @param yaw_max [rad] Maximum yaw angle (positive: to the right, negative: to the left) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint32_t cap_flags,uint8_t gimbal_device_id,float roll_min,float roll_max,float pitch_min,float pitch_max,float yaw_min,float yaw_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, cap_flags); + _mav_put_float(buf, 8, roll_min); + _mav_put_float(buf, 12, roll_max); + _mav_put_float(buf, 16, pitch_min); + _mav_put_float(buf, 20, pitch_max); + _mav_put_float(buf, 24, yaw_min); + _mav_put_float(buf, 28, yaw_max); + _mav_put_uint8_t(buf, 32, gimbal_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN); +#else + mavlink_gimbal_manager_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.cap_flags = cap_flags; + packet.roll_min = roll_min; + packet.roll_max = roll_max; + packet.pitch_min = pitch_min; + packet.pitch_max = pitch_max; + packet.yaw_min = yaw_min; + packet.yaw_max = yaw_max; + packet.gimbal_device_id = gimbal_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC); +} + +/** + * @brief Encode a gimbal_manager_information struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_manager_information_t* gimbal_manager_information) +{ + return mavlink_msg_gimbal_manager_information_pack(system_id, component_id, msg, gimbal_manager_information->time_boot_ms, gimbal_manager_information->cap_flags, gimbal_manager_information->gimbal_device_id, gimbal_manager_information->roll_min, gimbal_manager_information->roll_max, gimbal_manager_information->pitch_min, gimbal_manager_information->pitch_max, gimbal_manager_information->yaw_min, gimbal_manager_information->yaw_max); +} + +/** + * @brief Encode a gimbal_manager_information struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_manager_information_t* gimbal_manager_information) +{ + return mavlink_msg_gimbal_manager_information_pack_chan(system_id, component_id, chan, msg, gimbal_manager_information->time_boot_ms, gimbal_manager_information->cap_flags, gimbal_manager_information->gimbal_device_id, gimbal_manager_information->roll_min, gimbal_manager_information->roll_max, gimbal_manager_information->pitch_min, gimbal_manager_information->pitch_max, gimbal_manager_information->yaw_min, gimbal_manager_information->yaw_max); +} + +/** + * @brief Send a gimbal_manager_information message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param cap_flags Bitmap of gimbal capability flags. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param pitch_min [rad] Minimum pitch angle (positive: up, negative: down) + * @param pitch_max [rad] Maximum pitch angle (positive: up, negative: down) + * @param yaw_min [rad] Minimum yaw angle (positive: to the right, negative: to the left) + * @param yaw_max [rad] Maximum yaw angle (positive: to the right, negative: to the left) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_manager_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t cap_flags, uint8_t gimbal_device_id, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, cap_flags); + _mav_put_float(buf, 8, roll_min); + _mav_put_float(buf, 12, roll_max); + _mav_put_float(buf, 16, pitch_min); + _mav_put_float(buf, 20, pitch_max); + _mav_put_float(buf, 24, yaw_min); + _mav_put_float(buf, 28, yaw_max); + _mav_put_uint8_t(buf, 32, gimbal_device_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC); +#else + mavlink_gimbal_manager_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.cap_flags = cap_flags; + packet.roll_min = roll_min; + packet.roll_max = roll_max; + packet.pitch_min = pitch_min; + packet.pitch_max = pitch_max; + packet.yaw_min = yaw_min; + packet.yaw_max = yaw_max; + packet.gimbal_device_id = gimbal_device_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC); +#endif +} + +/** + * @brief Send a gimbal_manager_information message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_gimbal_manager_information_send_struct(mavlink_channel_t chan, const mavlink_gimbal_manager_information_t* gimbal_manager_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_manager_information_send(chan, gimbal_manager_information->time_boot_ms, gimbal_manager_information->cap_flags, gimbal_manager_information->gimbal_device_id, gimbal_manager_information->roll_min, gimbal_manager_information->roll_max, gimbal_manager_information->pitch_min, gimbal_manager_information->pitch_max, gimbal_manager_information->yaw_min, gimbal_manager_information->yaw_max); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, (const char *)gimbal_manager_information, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_manager_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t cap_flags, uint8_t gimbal_device_id, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, cap_flags); + _mav_put_float(buf, 8, roll_min); + _mav_put_float(buf, 12, roll_max); + _mav_put_float(buf, 16, pitch_min); + _mav_put_float(buf, 20, pitch_max); + _mav_put_float(buf, 24, yaw_min); + _mav_put_float(buf, 28, yaw_max); + _mav_put_uint8_t(buf, 32, gimbal_device_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC); +#else + mavlink_gimbal_manager_information_t *packet = (mavlink_gimbal_manager_information_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->cap_flags = cap_flags; + packet->roll_min = roll_min; + packet->roll_max = roll_max; + packet->pitch_min = pitch_min; + packet->pitch_max = pitch_max; + packet->yaw_min = yaw_min; + packet->yaw_max = yaw_max; + packet->gimbal_device_id = gimbal_device_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_MANAGER_INFORMATION UNPACKING + + +/** + * @brief Get field time_boot_ms from gimbal_manager_information message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_gimbal_manager_information_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field cap_flags from gimbal_manager_information message + * + * @return Bitmap of gimbal capability flags. + */ +static inline uint32_t mavlink_msg_gimbal_manager_information_get_cap_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field gimbal_device_id from gimbal_manager_information message + * + * @return Gimbal device ID that this gimbal manager is responsible for. + */ +static inline uint8_t mavlink_msg_gimbal_manager_information_get_gimbal_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field roll_min from gimbal_manager_information message + * + * @return [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + */ +static inline float mavlink_msg_gimbal_manager_information_get_roll_min(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field roll_max from gimbal_manager_information message + * + * @return [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + */ +static inline float mavlink_msg_gimbal_manager_information_get_roll_max(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field pitch_min from gimbal_manager_information message + * + * @return [rad] Minimum pitch angle (positive: up, negative: down) + */ +static inline float mavlink_msg_gimbal_manager_information_get_pitch_min(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field pitch_max from gimbal_manager_information message + * + * @return [rad] Maximum pitch angle (positive: up, negative: down) + */ +static inline float mavlink_msg_gimbal_manager_information_get_pitch_max(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field yaw_min from gimbal_manager_information message + * + * @return [rad] Minimum yaw angle (positive: to the right, negative: to the left) + */ +static inline float mavlink_msg_gimbal_manager_information_get_yaw_min(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field yaw_max from gimbal_manager_information message + * + * @return [rad] Maximum yaw angle (positive: to the right, negative: to the left) + */ +static inline float mavlink_msg_gimbal_manager_information_get_yaw_max(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Decode a gimbal_manager_information message into a struct + * + * @param msg The message to decode + * @param gimbal_manager_information C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_manager_information_decode(const mavlink_message_t* msg, mavlink_gimbal_manager_information_t* gimbal_manager_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + gimbal_manager_information->time_boot_ms = mavlink_msg_gimbal_manager_information_get_time_boot_ms(msg); + gimbal_manager_information->cap_flags = mavlink_msg_gimbal_manager_information_get_cap_flags(msg); + gimbal_manager_information->roll_min = mavlink_msg_gimbal_manager_information_get_roll_min(msg); + gimbal_manager_information->roll_max = mavlink_msg_gimbal_manager_information_get_roll_max(msg); + gimbal_manager_information->pitch_min = mavlink_msg_gimbal_manager_information_get_pitch_min(msg); + gimbal_manager_information->pitch_max = mavlink_msg_gimbal_manager_information_get_pitch_max(msg); + gimbal_manager_information->yaw_min = mavlink_msg_gimbal_manager_information_get_yaw_min(msg); + gimbal_manager_information->yaw_max = mavlink_msg_gimbal_manager_information_get_yaw_max(msg); + gimbal_manager_information->gimbal_device_id = mavlink_msg_gimbal_manager_information_get_gimbal_device_id(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN; + memset(gimbal_manager_information, 0, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN); + memcpy(gimbal_manager_information, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_attitude.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_attitude.h new file mode 100644 index 0000000000..828f1e6a3f --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_attitude.h @@ -0,0 +1,380 @@ +#pragma once +// MESSAGE GIMBAL_MANAGER_SET_ATTITUDE PACKING + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE 282 + + +typedef struct __mavlink_gimbal_manager_set_attitude_t { + uint32_t flags; /*< High level gimbal manager flags to use.*/ + float q[4]; /*< Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_MANAGER_FLAGS_YAW_LOCK is set)*/ + float angular_velocity_x; /*< [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored.*/ + float angular_velocity_y; /*< [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored.*/ + float angular_velocity_z; /*< [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored.*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + uint8_t gimbal_device_id; /*< Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).*/ +} mavlink_gimbal_manager_set_attitude_t; + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN 35 +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN 35 +#define MAVLINK_MSG_ID_282_LEN 35 +#define MAVLINK_MSG_ID_282_MIN_LEN 35 + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC 123 +#define MAVLINK_MSG_ID_282_CRC 123 + +#define MAVLINK_MSG_GIMBAL_MANAGER_SET_ATTITUDE_FIELD_Q_LEN 4 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_ATTITUDE { \ + 282, \ + "GIMBAL_MANAGER_SET_ATTITUDE", \ + 8, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gimbal_manager_set_attitude_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gimbal_manager_set_attitude_t, target_component) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_set_attitude_t, flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gimbal_manager_set_attitude_t, gimbal_device_id) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_gimbal_manager_set_attitude_t, q) }, \ + { "angular_velocity_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_manager_set_attitude_t, angular_velocity_x) }, \ + { "angular_velocity_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_manager_set_attitude_t, angular_velocity_y) }, \ + { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_manager_set_attitude_t, angular_velocity_z) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_ATTITUDE { \ + "GIMBAL_MANAGER_SET_ATTITUDE", \ + 8, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gimbal_manager_set_attitude_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gimbal_manager_set_attitude_t, target_component) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_set_attitude_t, flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gimbal_manager_set_attitude_t, gimbal_device_id) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_gimbal_manager_set_attitude_t, q) }, \ + { "angular_velocity_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_manager_set_attitude_t, angular_velocity_x) }, \ + { "angular_velocity_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_manager_set_attitude_t, angular_velocity_y) }, \ + { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_manager_set_attitude_t, angular_velocity_z) }, \ + } \ +} +#endif + +/** + * @brief Pack a gimbal_manager_set_attitude message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags to use. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_MANAGER_FLAGS_YAW_LOCK is set) + * @param angular_velocity_x [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + * @param angular_velocity_y [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + * @param angular_velocity_z [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, gimbal_device_id); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN); +#else + mavlink_gimbal_manager_set_attitude_t packet; + packet.flags = flags; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); +} + +/** + * @brief Pack a gimbal_manager_set_attitude message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags to use. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_MANAGER_FLAGS_YAW_LOCK is set) + * @param angular_velocity_x [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + * @param angular_velocity_y [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + * @param angular_velocity_z [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_attitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint32_t flags,uint8_t gimbal_device_id,const float *q,float angular_velocity_x,float angular_velocity_y,float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, gimbal_device_id); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN); +#else + mavlink_gimbal_manager_set_attitude_t packet; + packet.flags = flags; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); +} + +/** + * @brief Encode a gimbal_manager_set_attitude struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_set_attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_attitude_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_manager_set_attitude_t* gimbal_manager_set_attitude) +{ + return mavlink_msg_gimbal_manager_set_attitude_pack(system_id, component_id, msg, gimbal_manager_set_attitude->target_system, gimbal_manager_set_attitude->target_component, gimbal_manager_set_attitude->flags, gimbal_manager_set_attitude->gimbal_device_id, gimbal_manager_set_attitude->q, gimbal_manager_set_attitude->angular_velocity_x, gimbal_manager_set_attitude->angular_velocity_y, gimbal_manager_set_attitude->angular_velocity_z); +} + +/** + * @brief Encode a gimbal_manager_set_attitude struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_set_attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_attitude_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_manager_set_attitude_t* gimbal_manager_set_attitude) +{ + return mavlink_msg_gimbal_manager_set_attitude_pack_chan(system_id, component_id, chan, msg, gimbal_manager_set_attitude->target_system, gimbal_manager_set_attitude->target_component, gimbal_manager_set_attitude->flags, gimbal_manager_set_attitude->gimbal_device_id, gimbal_manager_set_attitude->q, gimbal_manager_set_attitude->angular_velocity_x, gimbal_manager_set_attitude->angular_velocity_y, gimbal_manager_set_attitude->angular_velocity_z); +} + +/** + * @brief Send a gimbal_manager_set_attitude message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags to use. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_MANAGER_FLAGS_YAW_LOCK is set) + * @param angular_velocity_x [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + * @param angular_velocity_y [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + * @param angular_velocity_z [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_manager_set_attitude_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, gimbal_device_id); + _mav_put_float_array(buf, 4, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); +#else + mavlink_gimbal_manager_set_attitude_t packet; + packet.flags = flags; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); +#endif +} + +/** + * @brief Send a gimbal_manager_set_attitude message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_gimbal_manager_set_attitude_send_struct(mavlink_channel_t chan, const mavlink_gimbal_manager_set_attitude_t* gimbal_manager_set_attitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_manager_set_attitude_send(chan, gimbal_manager_set_attitude->target_system, gimbal_manager_set_attitude->target_component, gimbal_manager_set_attitude->flags, gimbal_manager_set_attitude->gimbal_device_id, gimbal_manager_set_attitude->q, gimbal_manager_set_attitude->angular_velocity_x, gimbal_manager_set_attitude->angular_velocity_y, gimbal_manager_set_attitude->angular_velocity_z); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE, (const char *)gimbal_manager_set_attitude, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_manager_set_attitude_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, gimbal_device_id); + _mav_put_float_array(buf, 4, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); +#else + mavlink_gimbal_manager_set_attitude_t *packet = (mavlink_gimbal_manager_set_attitude_t *)msgbuf; + packet->flags = flags; + packet->angular_velocity_x = angular_velocity_x; + packet->angular_velocity_y = angular_velocity_y; + packet->angular_velocity_z = angular_velocity_z; + packet->target_system = target_system; + packet->target_component = target_component; + packet->gimbal_device_id = gimbal_device_id; + mav_array_memcpy(packet->q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_MANAGER_SET_ATTITUDE UNPACKING + + +/** + * @brief Get field target_system from gimbal_manager_set_attitude message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_attitude_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field target_component from gimbal_manager_set_attitude message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_attitude_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field flags from gimbal_manager_set_attitude message + * + * @return High level gimbal manager flags to use. + */ +static inline uint32_t mavlink_msg_gimbal_manager_set_attitude_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field gimbal_device_id from gimbal_manager_set_attitude message + * + * @return Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_attitude_get_gimbal_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field q from gimbal_manager_set_attitude message + * + * @return Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_MANAGER_FLAGS_YAW_LOCK is set) + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_attitude_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 4); +} + +/** + * @brief Get field angular_velocity_x from gimbal_manager_set_attitude message + * + * @return [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + */ +static inline float mavlink_msg_gimbal_manager_set_attitude_get_angular_velocity_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field angular_velocity_y from gimbal_manager_set_attitude message + * + * @return [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + */ +static inline float mavlink_msg_gimbal_manager_set_attitude_get_angular_velocity_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field angular_velocity_z from gimbal_manager_set_attitude message + * + * @return [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + */ +static inline float mavlink_msg_gimbal_manager_set_attitude_get_angular_velocity_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Decode a gimbal_manager_set_attitude message into a struct + * + * @param msg The message to decode + * @param gimbal_manager_set_attitude C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_manager_set_attitude_decode(const mavlink_message_t* msg, mavlink_gimbal_manager_set_attitude_t* gimbal_manager_set_attitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + gimbal_manager_set_attitude->flags = mavlink_msg_gimbal_manager_set_attitude_get_flags(msg); + mavlink_msg_gimbal_manager_set_attitude_get_q(msg, gimbal_manager_set_attitude->q); + gimbal_manager_set_attitude->angular_velocity_x = mavlink_msg_gimbal_manager_set_attitude_get_angular_velocity_x(msg); + gimbal_manager_set_attitude->angular_velocity_y = mavlink_msg_gimbal_manager_set_attitude_get_angular_velocity_y(msg); + gimbal_manager_set_attitude->angular_velocity_z = mavlink_msg_gimbal_manager_set_attitude_get_angular_velocity_z(msg); + gimbal_manager_set_attitude->target_system = mavlink_msg_gimbal_manager_set_attitude_get_target_system(msg); + gimbal_manager_set_attitude->target_component = mavlink_msg_gimbal_manager_set_attitude_get_target_component(msg); + gimbal_manager_set_attitude->gimbal_device_id = mavlink_msg_gimbal_manager_set_attitude_get_gimbal_device_id(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN; + memset(gimbal_manager_set_attitude, 0, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN); + memcpy(gimbal_manager_set_attitude, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_manual_control.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_manual_control.h new file mode 100644 index 0000000000..d3096c49bb --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_manual_control.h @@ -0,0 +1,388 @@ +#pragma once +// MESSAGE GIMBAL_MANAGER_SET_MANUAL_CONTROL PACKING + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL 288 + + +typedef struct __mavlink_gimbal_manager_set_manual_control_t { + uint32_t flags; /*< High level gimbal manager flags.*/ + float pitch; /*< Pitch angle unitless (-1..1, positive: up, negative: down, NaN to be ignored).*/ + float yaw; /*< Yaw angle unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored).*/ + float pitch_rate; /*< Pitch angular rate unitless (-1..1, positive: up, negative: down, NaN to be ignored).*/ + float yaw_rate; /*< Yaw angular rate unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored).*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + uint8_t gimbal_device_id; /*< Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).*/ +} mavlink_gimbal_manager_set_manual_control_t; + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN 23 +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN 23 +#define MAVLINK_MSG_ID_288_LEN 23 +#define MAVLINK_MSG_ID_288_MIN_LEN 23 + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC 20 +#define MAVLINK_MSG_ID_288_CRC 20 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_MANUAL_CONTROL { \ + 288, \ + "GIMBAL_MANAGER_SET_MANUAL_CONTROL", \ + 8, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_gimbal_manager_set_manual_control_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_gimbal_manager_set_manual_control_t, target_component) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_set_manual_control_t, flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_gimbal_manager_set_manual_control_t, gimbal_device_id) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_gimbal_manager_set_manual_control_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gimbal_manager_set_manual_control_t, yaw) }, \ + { "pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gimbal_manager_set_manual_control_t, pitch_rate) }, \ + { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gimbal_manager_set_manual_control_t, yaw_rate) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_MANUAL_CONTROL { \ + "GIMBAL_MANAGER_SET_MANUAL_CONTROL", \ + 8, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_gimbal_manager_set_manual_control_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_gimbal_manager_set_manual_control_t, target_component) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_set_manual_control_t, flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_gimbal_manager_set_manual_control_t, gimbal_device_id) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_gimbal_manager_set_manual_control_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gimbal_manager_set_manual_control_t, yaw) }, \ + { "pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gimbal_manager_set_manual_control_t, pitch_rate) }, \ + { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gimbal_manager_set_manual_control_t, yaw_rate) }, \ + } \ +} +#endif + +/** + * @brief Pack a gimbal_manager_set_manual_control message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param pitch Pitch angle unitless (-1..1, positive: up, negative: down, NaN to be ignored). + * @param yaw Yaw angle unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + * @param pitch_rate Pitch angular rate unitless (-1..1, positive: up, negative: down, NaN to be ignored). + * @param yaw_rate Yaw angular rate unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_manual_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, float pitch, float yaw, float pitch_rate, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN); +#else + mavlink_gimbal_manager_set_manual_control_t packet; + packet.flags = flags; + packet.pitch = pitch; + packet.yaw = yaw; + packet.pitch_rate = pitch_rate; + packet.yaw_rate = yaw_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC); +} + +/** + * @brief Pack a gimbal_manager_set_manual_control message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param pitch Pitch angle unitless (-1..1, positive: up, negative: down, NaN to be ignored). + * @param yaw Yaw angle unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + * @param pitch_rate Pitch angular rate unitless (-1..1, positive: up, negative: down, NaN to be ignored). + * @param yaw_rate Yaw angular rate unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_manual_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint32_t flags,uint8_t gimbal_device_id,float pitch,float yaw,float pitch_rate,float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN); +#else + mavlink_gimbal_manager_set_manual_control_t packet; + packet.flags = flags; + packet.pitch = pitch; + packet.yaw = yaw; + packet.pitch_rate = pitch_rate; + packet.yaw_rate = yaw_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC); +} + +/** + * @brief Encode a gimbal_manager_set_manual_control struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_set_manual_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_manual_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_manager_set_manual_control_t* gimbal_manager_set_manual_control) +{ + return mavlink_msg_gimbal_manager_set_manual_control_pack(system_id, component_id, msg, gimbal_manager_set_manual_control->target_system, gimbal_manager_set_manual_control->target_component, gimbal_manager_set_manual_control->flags, gimbal_manager_set_manual_control->gimbal_device_id, gimbal_manager_set_manual_control->pitch, gimbal_manager_set_manual_control->yaw, gimbal_manager_set_manual_control->pitch_rate, gimbal_manager_set_manual_control->yaw_rate); +} + +/** + * @brief Encode a gimbal_manager_set_manual_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_set_manual_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_manual_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_manager_set_manual_control_t* gimbal_manager_set_manual_control) +{ + return mavlink_msg_gimbal_manager_set_manual_control_pack_chan(system_id, component_id, chan, msg, gimbal_manager_set_manual_control->target_system, gimbal_manager_set_manual_control->target_component, gimbal_manager_set_manual_control->flags, gimbal_manager_set_manual_control->gimbal_device_id, gimbal_manager_set_manual_control->pitch, gimbal_manager_set_manual_control->yaw, gimbal_manager_set_manual_control->pitch_rate, gimbal_manager_set_manual_control->yaw_rate); +} + +/** + * @brief Send a gimbal_manager_set_manual_control message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param pitch Pitch angle unitless (-1..1, positive: up, negative: down, NaN to be ignored). + * @param yaw Yaw angle unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + * @param pitch_rate Pitch angular rate unitless (-1..1, positive: up, negative: down, NaN to be ignored). + * @param yaw_rate Yaw angular rate unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_manager_set_manual_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, float pitch, float yaw, float pitch_rate, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC); +#else + mavlink_gimbal_manager_set_manual_control_t packet; + packet.flags = flags; + packet.pitch = pitch; + packet.yaw = yaw; + packet.pitch_rate = pitch_rate; + packet.yaw_rate = yaw_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC); +#endif +} + +/** + * @brief Send a gimbal_manager_set_manual_control message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_gimbal_manager_set_manual_control_send_struct(mavlink_channel_t chan, const mavlink_gimbal_manager_set_manual_control_t* gimbal_manager_set_manual_control) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_manager_set_manual_control_send(chan, gimbal_manager_set_manual_control->target_system, gimbal_manager_set_manual_control->target_component, gimbal_manager_set_manual_control->flags, gimbal_manager_set_manual_control->gimbal_device_id, gimbal_manager_set_manual_control->pitch, gimbal_manager_set_manual_control->yaw, gimbal_manager_set_manual_control->pitch_rate, gimbal_manager_set_manual_control->yaw_rate); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL, (const char *)gimbal_manager_set_manual_control, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_manager_set_manual_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, float pitch, float yaw, float pitch_rate, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC); +#else + mavlink_gimbal_manager_set_manual_control_t *packet = (mavlink_gimbal_manager_set_manual_control_t *)msgbuf; + packet->flags = flags; + packet->pitch = pitch; + packet->yaw = yaw; + packet->pitch_rate = pitch_rate; + packet->yaw_rate = yaw_rate; + packet->target_system = target_system; + packet->target_component = target_component; + packet->gimbal_device_id = gimbal_device_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_MANAGER_SET_MANUAL_CONTROL UNPACKING + + +/** + * @brief Get field target_system from gimbal_manager_set_manual_control message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_manual_control_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field target_component from gimbal_manager_set_manual_control message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_manual_control_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 21); +} + +/** + * @brief Get field flags from gimbal_manager_set_manual_control message + * + * @return High level gimbal manager flags. + */ +static inline uint32_t mavlink_msg_gimbal_manager_set_manual_control_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field gimbal_device_id from gimbal_manager_set_manual_control message + * + * @return Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_manual_control_get_gimbal_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 22); +} + +/** + * @brief Get field pitch from gimbal_manager_set_manual_control message + * + * @return Pitch angle unitless (-1..1, positive: up, negative: down, NaN to be ignored). + */ +static inline float mavlink_msg_gimbal_manager_set_manual_control_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field yaw from gimbal_manager_set_manual_control message + * + * @return Yaw angle unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + */ +static inline float mavlink_msg_gimbal_manager_set_manual_control_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field pitch_rate from gimbal_manager_set_manual_control message + * + * @return Pitch angular rate unitless (-1..1, positive: up, negative: down, NaN to be ignored). + */ +static inline float mavlink_msg_gimbal_manager_set_manual_control_get_pitch_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field yaw_rate from gimbal_manager_set_manual_control message + * + * @return Yaw angular rate unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + */ +static inline float mavlink_msg_gimbal_manager_set_manual_control_get_yaw_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Decode a gimbal_manager_set_manual_control message into a struct + * + * @param msg The message to decode + * @param gimbal_manager_set_manual_control C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_manager_set_manual_control_decode(const mavlink_message_t* msg, mavlink_gimbal_manager_set_manual_control_t* gimbal_manager_set_manual_control) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + gimbal_manager_set_manual_control->flags = mavlink_msg_gimbal_manager_set_manual_control_get_flags(msg); + gimbal_manager_set_manual_control->pitch = mavlink_msg_gimbal_manager_set_manual_control_get_pitch(msg); + gimbal_manager_set_manual_control->yaw = mavlink_msg_gimbal_manager_set_manual_control_get_yaw(msg); + gimbal_manager_set_manual_control->pitch_rate = mavlink_msg_gimbal_manager_set_manual_control_get_pitch_rate(msg); + gimbal_manager_set_manual_control->yaw_rate = mavlink_msg_gimbal_manager_set_manual_control_get_yaw_rate(msg); + gimbal_manager_set_manual_control->target_system = mavlink_msg_gimbal_manager_set_manual_control_get_target_system(msg); + gimbal_manager_set_manual_control->target_component = mavlink_msg_gimbal_manager_set_manual_control_get_target_component(msg); + gimbal_manager_set_manual_control->gimbal_device_id = mavlink_msg_gimbal_manager_set_manual_control_get_gimbal_device_id(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN; + memset(gimbal_manager_set_manual_control, 0, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN); + memcpy(gimbal_manager_set_manual_control, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_pitchyaw.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_pitchyaw.h new file mode 100644 index 0000000000..de6a43b1d3 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_pitchyaw.h @@ -0,0 +1,388 @@ +#pragma once +// MESSAGE GIMBAL_MANAGER_SET_PITCHYAW PACKING + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW 287 + + +typedef struct __mavlink_gimbal_manager_set_pitchyaw_t { + uint32_t flags; /*< High level gimbal manager flags to use.*/ + float pitch; /*< [rad] Pitch angle (positive: up, negative: down, NaN to be ignored).*/ + float yaw; /*< [rad] Yaw angle (positive: to the right, negative: to the left, NaN to be ignored).*/ + float pitch_rate; /*< [rad/s] Pitch angular rate (positive: up, negative: down, NaN to be ignored).*/ + float yaw_rate; /*< [rad/s] Yaw angular rate (positive: to the right, negative: to the left, NaN to be ignored).*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + uint8_t gimbal_device_id; /*< Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).*/ +} mavlink_gimbal_manager_set_pitchyaw_t; + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN 23 +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN 23 +#define MAVLINK_MSG_ID_287_LEN 23 +#define MAVLINK_MSG_ID_287_MIN_LEN 23 + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC 1 +#define MAVLINK_MSG_ID_287_CRC 1 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_PITCHYAW { \ + 287, \ + "GIMBAL_MANAGER_SET_PITCHYAW", \ + 8, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, target_component) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, gimbal_device_id) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, yaw) }, \ + { "pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, pitch_rate) }, \ + { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, yaw_rate) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_PITCHYAW { \ + "GIMBAL_MANAGER_SET_PITCHYAW", \ + 8, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, target_component) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, gimbal_device_id) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, yaw) }, \ + { "pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, pitch_rate) }, \ + { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, yaw_rate) }, \ + } \ +} +#endif + +/** + * @brief Pack a gimbal_manager_set_pitchyaw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags to use. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param pitch [rad] Pitch angle (positive: up, negative: down, NaN to be ignored). + * @param yaw [rad] Yaw angle (positive: to the right, negative: to the left, NaN to be ignored). + * @param pitch_rate [rad/s] Pitch angular rate (positive: up, negative: down, NaN to be ignored). + * @param yaw_rate [rad/s] Yaw angular rate (positive: to the right, negative: to the left, NaN to be ignored). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_pitchyaw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, float pitch, float yaw, float pitch_rate, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN); +#else + mavlink_gimbal_manager_set_pitchyaw_t packet; + packet.flags = flags; + packet.pitch = pitch; + packet.yaw = yaw; + packet.pitch_rate = pitch_rate; + packet.yaw_rate = yaw_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC); +} + +/** + * @brief Pack a gimbal_manager_set_pitchyaw message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags to use. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param pitch [rad] Pitch angle (positive: up, negative: down, NaN to be ignored). + * @param yaw [rad] Yaw angle (positive: to the right, negative: to the left, NaN to be ignored). + * @param pitch_rate [rad/s] Pitch angular rate (positive: up, negative: down, NaN to be ignored). + * @param yaw_rate [rad/s] Yaw angular rate (positive: to the right, negative: to the left, NaN to be ignored). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_pitchyaw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint32_t flags,uint8_t gimbal_device_id,float pitch,float yaw,float pitch_rate,float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN); +#else + mavlink_gimbal_manager_set_pitchyaw_t packet; + packet.flags = flags; + packet.pitch = pitch; + packet.yaw = yaw; + packet.pitch_rate = pitch_rate; + packet.yaw_rate = yaw_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC); +} + +/** + * @brief Encode a gimbal_manager_set_pitchyaw struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_set_pitchyaw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_pitchyaw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_manager_set_pitchyaw_t* gimbal_manager_set_pitchyaw) +{ + return mavlink_msg_gimbal_manager_set_pitchyaw_pack(system_id, component_id, msg, gimbal_manager_set_pitchyaw->target_system, gimbal_manager_set_pitchyaw->target_component, gimbal_manager_set_pitchyaw->flags, gimbal_manager_set_pitchyaw->gimbal_device_id, gimbal_manager_set_pitchyaw->pitch, gimbal_manager_set_pitchyaw->yaw, gimbal_manager_set_pitchyaw->pitch_rate, gimbal_manager_set_pitchyaw->yaw_rate); +} + +/** + * @brief Encode a gimbal_manager_set_pitchyaw struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_set_pitchyaw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_pitchyaw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_manager_set_pitchyaw_t* gimbal_manager_set_pitchyaw) +{ + return mavlink_msg_gimbal_manager_set_pitchyaw_pack_chan(system_id, component_id, chan, msg, gimbal_manager_set_pitchyaw->target_system, gimbal_manager_set_pitchyaw->target_component, gimbal_manager_set_pitchyaw->flags, gimbal_manager_set_pitchyaw->gimbal_device_id, gimbal_manager_set_pitchyaw->pitch, gimbal_manager_set_pitchyaw->yaw, gimbal_manager_set_pitchyaw->pitch_rate, gimbal_manager_set_pitchyaw->yaw_rate); +} + +/** + * @brief Send a gimbal_manager_set_pitchyaw message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags to use. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param pitch [rad] Pitch angle (positive: up, negative: down, NaN to be ignored). + * @param yaw [rad] Yaw angle (positive: to the right, negative: to the left, NaN to be ignored). + * @param pitch_rate [rad/s] Pitch angular rate (positive: up, negative: down, NaN to be ignored). + * @param yaw_rate [rad/s] Yaw angular rate (positive: to the right, negative: to the left, NaN to be ignored). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_manager_set_pitchyaw_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, float pitch, float yaw, float pitch_rate, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC); +#else + mavlink_gimbal_manager_set_pitchyaw_t packet; + packet.flags = flags; + packet.pitch = pitch; + packet.yaw = yaw; + packet.pitch_rate = pitch_rate; + packet.yaw_rate = yaw_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC); +#endif +} + +/** + * @brief Send a gimbal_manager_set_pitchyaw message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_gimbal_manager_set_pitchyaw_send_struct(mavlink_channel_t chan, const mavlink_gimbal_manager_set_pitchyaw_t* gimbal_manager_set_pitchyaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_manager_set_pitchyaw_send(chan, gimbal_manager_set_pitchyaw->target_system, gimbal_manager_set_pitchyaw->target_component, gimbal_manager_set_pitchyaw->flags, gimbal_manager_set_pitchyaw->gimbal_device_id, gimbal_manager_set_pitchyaw->pitch, gimbal_manager_set_pitchyaw->yaw, gimbal_manager_set_pitchyaw->pitch_rate, gimbal_manager_set_pitchyaw->yaw_rate); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW, (const char *)gimbal_manager_set_pitchyaw, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_manager_set_pitchyaw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, float pitch, float yaw, float pitch_rate, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC); +#else + mavlink_gimbal_manager_set_pitchyaw_t *packet = (mavlink_gimbal_manager_set_pitchyaw_t *)msgbuf; + packet->flags = flags; + packet->pitch = pitch; + packet->yaw = yaw; + packet->pitch_rate = pitch_rate; + packet->yaw_rate = yaw_rate; + packet->target_system = target_system; + packet->target_component = target_component; + packet->gimbal_device_id = gimbal_device_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_MANAGER_SET_PITCHYAW UNPACKING + + +/** + * @brief Get field target_system from gimbal_manager_set_pitchyaw message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_pitchyaw_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field target_component from gimbal_manager_set_pitchyaw message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_pitchyaw_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 21); +} + +/** + * @brief Get field flags from gimbal_manager_set_pitchyaw message + * + * @return High level gimbal manager flags to use. + */ +static inline uint32_t mavlink_msg_gimbal_manager_set_pitchyaw_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field gimbal_device_id from gimbal_manager_set_pitchyaw message + * + * @return Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_pitchyaw_get_gimbal_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 22); +} + +/** + * @brief Get field pitch from gimbal_manager_set_pitchyaw message + * + * @return [rad] Pitch angle (positive: up, negative: down, NaN to be ignored). + */ +static inline float mavlink_msg_gimbal_manager_set_pitchyaw_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field yaw from gimbal_manager_set_pitchyaw message + * + * @return [rad] Yaw angle (positive: to the right, negative: to the left, NaN to be ignored). + */ +static inline float mavlink_msg_gimbal_manager_set_pitchyaw_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field pitch_rate from gimbal_manager_set_pitchyaw message + * + * @return [rad/s] Pitch angular rate (positive: up, negative: down, NaN to be ignored). + */ +static inline float mavlink_msg_gimbal_manager_set_pitchyaw_get_pitch_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field yaw_rate from gimbal_manager_set_pitchyaw message + * + * @return [rad/s] Yaw angular rate (positive: to the right, negative: to the left, NaN to be ignored). + */ +static inline float mavlink_msg_gimbal_manager_set_pitchyaw_get_yaw_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Decode a gimbal_manager_set_pitchyaw message into a struct + * + * @param msg The message to decode + * @param gimbal_manager_set_pitchyaw C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_manager_set_pitchyaw_decode(const mavlink_message_t* msg, mavlink_gimbal_manager_set_pitchyaw_t* gimbal_manager_set_pitchyaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + gimbal_manager_set_pitchyaw->flags = mavlink_msg_gimbal_manager_set_pitchyaw_get_flags(msg); + gimbal_manager_set_pitchyaw->pitch = mavlink_msg_gimbal_manager_set_pitchyaw_get_pitch(msg); + gimbal_manager_set_pitchyaw->yaw = mavlink_msg_gimbal_manager_set_pitchyaw_get_yaw(msg); + gimbal_manager_set_pitchyaw->pitch_rate = mavlink_msg_gimbal_manager_set_pitchyaw_get_pitch_rate(msg); + gimbal_manager_set_pitchyaw->yaw_rate = mavlink_msg_gimbal_manager_set_pitchyaw_get_yaw_rate(msg); + gimbal_manager_set_pitchyaw->target_system = mavlink_msg_gimbal_manager_set_pitchyaw_get_target_system(msg); + gimbal_manager_set_pitchyaw->target_component = mavlink_msg_gimbal_manager_set_pitchyaw_get_target_component(msg); + gimbal_manager_set_pitchyaw->gimbal_device_id = mavlink_msg_gimbal_manager_set_pitchyaw_get_gimbal_device_id(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN; + memset(gimbal_manager_set_pitchyaw, 0, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN); + memcpy(gimbal_manager_set_pitchyaw, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_status.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_status.h new file mode 100644 index 0000000000..4eace068bb --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_status.h @@ -0,0 +1,363 @@ +#pragma once +// MESSAGE GIMBAL_MANAGER_STATUS PACKING + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS 281 + + +typedef struct __mavlink_gimbal_manager_status_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + uint32_t flags; /*< High level gimbal manager flags currently applied.*/ + uint8_t gimbal_device_id; /*< Gimbal device ID that this gimbal manager is responsible for.*/ + uint8_t primary_control_sysid; /*< System ID of MAVLink component with primary control, 0 for none.*/ + uint8_t primary_control_compid; /*< Component ID of MAVLink component with primary control, 0 for none.*/ + uint8_t secondary_control_sysid; /*< System ID of MAVLink component with secondary control, 0 for none.*/ + uint8_t secondary_control_compid; /*< Component ID of MAVLink component with secondary control, 0 for none.*/ +} mavlink_gimbal_manager_status_t; + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN 13 +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN 13 +#define MAVLINK_MSG_ID_281_LEN 13 +#define MAVLINK_MSG_ID_281_MIN_LEN 13 + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC 48 +#define MAVLINK_MSG_ID_281_CRC 48 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_STATUS { \ + 281, \ + "GIMBAL_MANAGER_STATUS", \ + 7, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_status_t, time_boot_ms) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gimbal_manager_status_t, flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_gimbal_manager_status_t, gimbal_device_id) }, \ + { "primary_control_sysid", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_gimbal_manager_status_t, primary_control_sysid) }, \ + { "primary_control_compid", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_gimbal_manager_status_t, primary_control_compid) }, \ + { "secondary_control_sysid", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_gimbal_manager_status_t, secondary_control_sysid) }, \ + { "secondary_control_compid", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_gimbal_manager_status_t, secondary_control_compid) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_STATUS { \ + "GIMBAL_MANAGER_STATUS", \ + 7, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_status_t, time_boot_ms) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gimbal_manager_status_t, flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_gimbal_manager_status_t, gimbal_device_id) }, \ + { "primary_control_sysid", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_gimbal_manager_status_t, primary_control_sysid) }, \ + { "primary_control_compid", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_gimbal_manager_status_t, primary_control_compid) }, \ + { "secondary_control_sysid", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_gimbal_manager_status_t, secondary_control_sysid) }, \ + { "secondary_control_compid", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_gimbal_manager_status_t, secondary_control_compid) }, \ + } \ +} +#endif + +/** + * @brief Pack a gimbal_manager_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param flags High level gimbal manager flags currently applied. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param primary_control_sysid System ID of MAVLink component with primary control, 0 for none. + * @param primary_control_compid Component ID of MAVLink component with primary control, 0 for none. + * @param secondary_control_sysid System ID of MAVLink component with secondary control, 0 for none. + * @param secondary_control_compid Component ID of MAVLink component with secondary control, 0 for none. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint32_t flags, uint8_t gimbal_device_id, uint8_t primary_control_sysid, uint8_t primary_control_compid, uint8_t secondary_control_sysid, uint8_t secondary_control_compid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, flags); + _mav_put_uint8_t(buf, 8, gimbal_device_id); + _mav_put_uint8_t(buf, 9, primary_control_sysid); + _mav_put_uint8_t(buf, 10, primary_control_compid); + _mav_put_uint8_t(buf, 11, secondary_control_sysid); + _mav_put_uint8_t(buf, 12, secondary_control_compid); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN); +#else + mavlink_gimbal_manager_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.flags = flags; + packet.gimbal_device_id = gimbal_device_id; + packet.primary_control_sysid = primary_control_sysid; + packet.primary_control_compid = primary_control_compid; + packet.secondary_control_sysid = secondary_control_sysid; + packet.secondary_control_compid = secondary_control_compid; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC); +} + +/** + * @brief Pack a gimbal_manager_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param flags High level gimbal manager flags currently applied. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param primary_control_sysid System ID of MAVLink component with primary control, 0 for none. + * @param primary_control_compid Component ID of MAVLink component with primary control, 0 for none. + * @param secondary_control_sysid System ID of MAVLink component with secondary control, 0 for none. + * @param secondary_control_compid Component ID of MAVLink component with secondary control, 0 for none. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint32_t flags,uint8_t gimbal_device_id,uint8_t primary_control_sysid,uint8_t primary_control_compid,uint8_t secondary_control_sysid,uint8_t secondary_control_compid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, flags); + _mav_put_uint8_t(buf, 8, gimbal_device_id); + _mav_put_uint8_t(buf, 9, primary_control_sysid); + _mav_put_uint8_t(buf, 10, primary_control_compid); + _mav_put_uint8_t(buf, 11, secondary_control_sysid); + _mav_put_uint8_t(buf, 12, secondary_control_compid); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN); +#else + mavlink_gimbal_manager_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.flags = flags; + packet.gimbal_device_id = gimbal_device_id; + packet.primary_control_sysid = primary_control_sysid; + packet.primary_control_compid = primary_control_compid; + packet.secondary_control_sysid = secondary_control_sysid; + packet.secondary_control_compid = secondary_control_compid; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC); +} + +/** + * @brief Encode a gimbal_manager_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_manager_status_t* gimbal_manager_status) +{ + return mavlink_msg_gimbal_manager_status_pack(system_id, component_id, msg, gimbal_manager_status->time_boot_ms, gimbal_manager_status->flags, gimbal_manager_status->gimbal_device_id, gimbal_manager_status->primary_control_sysid, gimbal_manager_status->primary_control_compid, gimbal_manager_status->secondary_control_sysid, gimbal_manager_status->secondary_control_compid); +} + +/** + * @brief Encode a gimbal_manager_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_manager_status_t* gimbal_manager_status) +{ + return mavlink_msg_gimbal_manager_status_pack_chan(system_id, component_id, chan, msg, gimbal_manager_status->time_boot_ms, gimbal_manager_status->flags, gimbal_manager_status->gimbal_device_id, gimbal_manager_status->primary_control_sysid, gimbal_manager_status->primary_control_compid, gimbal_manager_status->secondary_control_sysid, gimbal_manager_status->secondary_control_compid); +} + +/** + * @brief Send a gimbal_manager_status message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param flags High level gimbal manager flags currently applied. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param primary_control_sysid System ID of MAVLink component with primary control, 0 for none. + * @param primary_control_compid Component ID of MAVLink component with primary control, 0 for none. + * @param secondary_control_sysid System ID of MAVLink component with secondary control, 0 for none. + * @param secondary_control_compid Component ID of MAVLink component with secondary control, 0 for none. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_manager_status_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t flags, uint8_t gimbal_device_id, uint8_t primary_control_sysid, uint8_t primary_control_compid, uint8_t secondary_control_sysid, uint8_t secondary_control_compid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, flags); + _mav_put_uint8_t(buf, 8, gimbal_device_id); + _mav_put_uint8_t(buf, 9, primary_control_sysid); + _mav_put_uint8_t(buf, 10, primary_control_compid); + _mav_put_uint8_t(buf, 11, secondary_control_sysid); + _mav_put_uint8_t(buf, 12, secondary_control_compid); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC); +#else + mavlink_gimbal_manager_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.flags = flags; + packet.gimbal_device_id = gimbal_device_id; + packet.primary_control_sysid = primary_control_sysid; + packet.primary_control_compid = primary_control_compid; + packet.secondary_control_sysid = secondary_control_sysid; + packet.secondary_control_compid = secondary_control_compid; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC); +#endif +} + +/** + * @brief Send a gimbal_manager_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_gimbal_manager_status_send_struct(mavlink_channel_t chan, const mavlink_gimbal_manager_status_t* gimbal_manager_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_manager_status_send(chan, gimbal_manager_status->time_boot_ms, gimbal_manager_status->flags, gimbal_manager_status->gimbal_device_id, gimbal_manager_status->primary_control_sysid, gimbal_manager_status->primary_control_compid, gimbal_manager_status->secondary_control_sysid, gimbal_manager_status->secondary_control_compid); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, (const char *)gimbal_manager_status, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_manager_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t flags, uint8_t gimbal_device_id, uint8_t primary_control_sysid, uint8_t primary_control_compid, uint8_t secondary_control_sysid, uint8_t secondary_control_compid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, flags); + _mav_put_uint8_t(buf, 8, gimbal_device_id); + _mav_put_uint8_t(buf, 9, primary_control_sysid); + _mav_put_uint8_t(buf, 10, primary_control_compid); + _mav_put_uint8_t(buf, 11, secondary_control_sysid); + _mav_put_uint8_t(buf, 12, secondary_control_compid); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC); +#else + mavlink_gimbal_manager_status_t *packet = (mavlink_gimbal_manager_status_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->flags = flags; + packet->gimbal_device_id = gimbal_device_id; + packet->primary_control_sysid = primary_control_sysid; + packet->primary_control_compid = primary_control_compid; + packet->secondary_control_sysid = secondary_control_sysid; + packet->secondary_control_compid = secondary_control_compid; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_MANAGER_STATUS UNPACKING + + +/** + * @brief Get field time_boot_ms from gimbal_manager_status message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_gimbal_manager_status_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field flags from gimbal_manager_status message + * + * @return High level gimbal manager flags currently applied. + */ +static inline uint32_t mavlink_msg_gimbal_manager_status_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field gimbal_device_id from gimbal_manager_status message + * + * @return Gimbal device ID that this gimbal manager is responsible for. + */ +static inline uint8_t mavlink_msg_gimbal_manager_status_get_gimbal_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field primary_control_sysid from gimbal_manager_status message + * + * @return System ID of MAVLink component with primary control, 0 for none. + */ +static inline uint8_t mavlink_msg_gimbal_manager_status_get_primary_control_sysid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 9); +} + +/** + * @brief Get field primary_control_compid from gimbal_manager_status message + * + * @return Component ID of MAVLink component with primary control, 0 for none. + */ +static inline uint8_t mavlink_msg_gimbal_manager_status_get_primary_control_compid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 10); +} + +/** + * @brief Get field secondary_control_sysid from gimbal_manager_status message + * + * @return System ID of MAVLink component with secondary control, 0 for none. + */ +static inline uint8_t mavlink_msg_gimbal_manager_status_get_secondary_control_sysid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 11); +} + +/** + * @brief Get field secondary_control_compid from gimbal_manager_status message + * + * @return Component ID of MAVLink component with secondary control, 0 for none. + */ +static inline uint8_t mavlink_msg_gimbal_manager_status_get_secondary_control_compid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Decode a gimbal_manager_status message into a struct + * + * @param msg The message to decode + * @param gimbal_manager_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_manager_status_decode(const mavlink_message_t* msg, mavlink_gimbal_manager_status_t* gimbal_manager_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + gimbal_manager_status->time_boot_ms = mavlink_msg_gimbal_manager_status_get_time_boot_ms(msg); + gimbal_manager_status->flags = mavlink_msg_gimbal_manager_status_get_flags(msg); + gimbal_manager_status->gimbal_device_id = mavlink_msg_gimbal_manager_status_get_gimbal_device_id(msg); + gimbal_manager_status->primary_control_sysid = mavlink_msg_gimbal_manager_status_get_primary_control_sysid(msg); + gimbal_manager_status->primary_control_compid = mavlink_msg_gimbal_manager_status_get_primary_control_compid(msg); + gimbal_manager_status->secondary_control_sysid = mavlink_msg_gimbal_manager_status_get_secondary_control_sysid(msg); + gimbal_manager_status->secondary_control_compid = mavlink_msg_gimbal_manager_status_get_secondary_control_compid(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN; + memset(gimbal_manager_status, 0, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN); + memcpy(gimbal_manager_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_global_vision_position_estimate.h b/lib/main/MAVLink/common/mavlink_msg_global_vision_position_estimate.h index 5f191fe3ad..94a3d05dd4 100755 --- a/lib/main/MAVLink/common/mavlink_msg_global_vision_position_estimate.h +++ b/lib/main/MAVLink/common/mavlink_msg_global_vision_position_estimate.h @@ -12,23 +12,25 @@ typedef struct __mavlink_global_vision_position_estimate_t { float roll; /*< [rad] Roll angle*/ float pitch; /*< [rad] Pitch angle*/ float yaw; /*< [rad] Yaw angle*/ + float covariance[21]; /*< Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x_global, y_global, z_global, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.*/ + uint8_t reset_counter; /*< Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps.*/ } mavlink_global_vision_position_estimate_t; -#define MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN 32 +#define MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN 117 #define MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_MIN_LEN 32 -#define MAVLINK_MSG_ID_101_LEN 32 +#define MAVLINK_MSG_ID_101_LEN 117 #define MAVLINK_MSG_ID_101_MIN_LEN 32 #define MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC 102 #define MAVLINK_MSG_ID_101_CRC 102 - +#define MAVLINK_MSG_GLOBAL_VISION_POSITION_ESTIMATE_FIELD_COVARIANCE_LEN 21 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE { \ 101, \ "GLOBAL_VISION_POSITION_ESTIMATE", \ - 7, \ + 9, \ { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_global_vision_position_estimate_t, usec) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_global_vision_position_estimate_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_global_vision_position_estimate_t, y) }, \ @@ -36,12 +38,14 @@ typedef struct __mavlink_global_vision_position_estimate_t { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_global_vision_position_estimate_t, roll) }, \ { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_global_vision_position_estimate_t, pitch) }, \ { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_global_vision_position_estimate_t, yaw) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 32, offsetof(mavlink_global_vision_position_estimate_t, covariance) }, \ + { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 116, offsetof(mavlink_global_vision_position_estimate_t, reset_counter) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE { \ "GLOBAL_VISION_POSITION_ESTIMATE", \ - 7, \ + 9, \ { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_global_vision_position_estimate_t, usec) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_global_vision_position_estimate_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_global_vision_position_estimate_t, y) }, \ @@ -49,6 +53,8 @@ typedef struct __mavlink_global_vision_position_estimate_t { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_global_vision_position_estimate_t, roll) }, \ { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_global_vision_position_estimate_t, pitch) }, \ { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_global_vision_position_estimate_t, yaw) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 32, offsetof(mavlink_global_vision_position_estimate_t, covariance) }, \ + { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 116, offsetof(mavlink_global_vision_position_estimate_t, reset_counter) }, \ } \ } #endif @@ -66,10 +72,12 @@ typedef struct __mavlink_global_vision_position_estimate_t { * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x_global, y_global, z_global, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) + uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN]; @@ -80,7 +88,8 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); #else mavlink_global_vision_position_estimate_t packet; @@ -91,7 +100,8 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); #endif @@ -112,11 +122,13 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x_global, y_global, z_global, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) + uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw,const float *covariance,uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN]; @@ -127,7 +139,8 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uin _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); #else mavlink_global_vision_position_estimate_t packet; @@ -138,7 +151,8 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uin packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); #endif @@ -156,7 +170,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uin */ static inline uint16_t mavlink_msg_global_vision_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_vision_position_estimate_t* global_vision_position_estimate) { - return mavlink_msg_global_vision_position_estimate_pack(system_id, component_id, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw); + return mavlink_msg_global_vision_position_estimate_pack(system_id, component_id, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw, global_vision_position_estimate->covariance, global_vision_position_estimate->reset_counter); } /** @@ -170,7 +184,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_encode(uint8_ */ static inline uint16_t mavlink_msg_global_vision_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_vision_position_estimate_t* global_vision_position_estimate) { - return mavlink_msg_global_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw); + return mavlink_msg_global_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw, global_vision_position_estimate->covariance, global_vision_position_estimate->reset_counter); } /** @@ -184,10 +198,12 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_encode_chan(u * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x_global, y_global, z_global, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN]; @@ -198,7 +214,8 @@ static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_chan _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); #else mavlink_global_vision_position_estimate_t packet; @@ -209,7 +226,8 @@ static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_chan packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); #endif } @@ -222,7 +240,7 @@ static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_chan static inline void mavlink_msg_global_vision_position_estimate_send_struct(mavlink_channel_t chan, const mavlink_global_vision_position_estimate_t* global_vision_position_estimate) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_global_vision_position_estimate_send(chan, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw); + mavlink_msg_global_vision_position_estimate_send(chan, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw, global_vision_position_estimate->covariance, global_vision_position_estimate->reset_counter); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)global_vision_position_estimate, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); #endif @@ -236,7 +254,7 @@ static inline void mavlink_msg_global_vision_position_estimate_send_struct(mavli is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_global_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +static inline void mavlink_msg_global_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -247,7 +265,8 @@ static inline void mavlink_msg_global_vision_position_estimate_send_buf(mavlink_ _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); #else mavlink_global_vision_position_estimate_t *packet = (mavlink_global_vision_position_estimate_t *)msgbuf; @@ -258,7 +277,8 @@ static inline void mavlink_msg_global_vision_position_estimate_send_buf(mavlink_ packet->roll = roll; packet->pitch = pitch; packet->yaw = yaw; - + packet->reset_counter = reset_counter; + mav_array_memcpy(packet->covariance, covariance, sizeof(float)*21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); #endif } @@ -339,6 +359,26 @@ static inline float mavlink_msg_global_vision_position_estimate_get_yaw(const ma return _MAV_RETURN_float(msg, 28); } +/** + * @brief Get field covariance from global_vision_position_estimate message + * + * @return Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x_global, y_global, z_global, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + */ +static inline uint16_t mavlink_msg_global_vision_position_estimate_get_covariance(const mavlink_message_t* msg, float *covariance) +{ + return _MAV_RETURN_float_array(msg, covariance, 21, 32); +} + +/** + * @brief Get field reset_counter from global_vision_position_estimate message + * + * @return Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + */ +static inline uint8_t mavlink_msg_global_vision_position_estimate_get_reset_counter(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 116); +} + /** * @brief Decode a global_vision_position_estimate message into a struct * @@ -355,6 +395,8 @@ static inline void mavlink_msg_global_vision_position_estimate_decode(const mavl global_vision_position_estimate->roll = mavlink_msg_global_vision_position_estimate_get_roll(msg); global_vision_position_estimate->pitch = mavlink_msg_global_vision_position_estimate_get_pitch(msg); global_vision_position_estimate->yaw = mavlink_msg_global_vision_position_estimate_get_yaw(msg); + mavlink_msg_global_vision_position_estimate_get_covariance(msg, global_vision_position_estimate->covariance); + global_vision_position_estimate->reset_counter = mavlink_msg_global_vision_position_estimate_get_reset_counter(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN? msg->len : MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN; memset(global_vision_position_estimate, 0, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_gps2_raw.h b/lib/main/MAVLink/common/mavlink_msg_gps2_raw.h index 330b7e96dd..5e741ef493 100755 --- a/lib/main/MAVLink/common/mavlink_msg_gps2_raw.h +++ b/lib/main/MAVLink/common/mavlink_msg_gps2_raw.h @@ -3,25 +3,26 @@ #define MAVLINK_MSG_ID_GPS2_RAW 124 - +MAVPACKED( typedef struct __mavlink_gps2_raw_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ int32_t lat; /*< [degE7] Latitude (WGS84)*/ int32_t lon; /*< [degE7] Longitude (WGS84)*/ int32_t alt; /*< [mm] Altitude (MSL). Positive for up.*/ uint32_t dgps_age; /*< [ms] Age of DGPS info*/ - uint16_t eph; /*< [cm] GPS HDOP horizontal dilution of position. If unknown, set to: UINT16_MAX*/ - uint16_t epv; /*< [cm] GPS VDOP vertical dilution of position. If unknown, set to: UINT16_MAX*/ + uint16_t eph; /*< GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX*/ + uint16_t epv; /*< GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX*/ uint16_t vel; /*< [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX*/ uint16_t cog; /*< [cdeg] Course over ground (NOT heading, but direction of movement): 0.0..359.99 degrees. If unknown, set to: UINT16_MAX*/ uint8_t fix_type; /*< GPS fix type.*/ uint8_t satellites_visible; /*< Number of satellites visible. If unknown, set to 255*/ uint8_t dgps_numch; /*< Number of DGPS satellites*/ -} mavlink_gps2_raw_t; + uint16_t yaw; /*< [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north.*/ +}) mavlink_gps2_raw_t; -#define MAVLINK_MSG_ID_GPS2_RAW_LEN 35 +#define MAVLINK_MSG_ID_GPS2_RAW_LEN 37 #define MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN 35 -#define MAVLINK_MSG_ID_124_LEN 35 +#define MAVLINK_MSG_ID_124_LEN 37 #define MAVLINK_MSG_ID_124_MIN_LEN 35 #define MAVLINK_MSG_ID_GPS2_RAW_CRC 87 @@ -33,7 +34,7 @@ typedef struct __mavlink_gps2_raw_t { #define MAVLINK_MESSAGE_INFO_GPS2_RAW { \ 124, \ "GPS2_RAW", \ - 12, \ + 13, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps2_raw_t, time_usec) }, \ { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_raw_t, fix_type) }, \ { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_raw_t, lat) }, \ @@ -46,12 +47,13 @@ typedef struct __mavlink_gps2_raw_t { { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps2_raw_t, satellites_visible) }, \ { "dgps_numch", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_raw_t, dgps_numch) }, \ { "dgps_age", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_raw_t, dgps_age) }, \ + { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 35, offsetof(mavlink_gps2_raw_t, yaw) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_GPS2_RAW { \ "GPS2_RAW", \ - 12, \ + 13, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps2_raw_t, time_usec) }, \ { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_raw_t, fix_type) }, \ { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_raw_t, lat) }, \ @@ -64,6 +66,7 @@ typedef struct __mavlink_gps2_raw_t { { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps2_raw_t, satellites_visible) }, \ { "dgps_numch", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_raw_t, dgps_numch) }, \ { "dgps_age", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_raw_t, dgps_age) }, \ + { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 35, offsetof(mavlink_gps2_raw_t, yaw) }, \ } \ } #endif @@ -79,17 +82,18 @@ typedef struct __mavlink_gps2_raw_t { * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph [cm] GPS HDOP horizontal dilution of position. If unknown, set to: UINT16_MAX - * @param epv [cm] GPS VDOP vertical dilution of position. If unknown, set to: UINT16_MAX + * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement): 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 * @param dgps_numch Number of DGPS satellites * @param dgps_age [ms] Age of DGPS info + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) + uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; @@ -105,6 +109,7 @@ static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t comp _mav_put_uint8_t(buf, 32, fix_type); _mav_put_uint8_t(buf, 33, satellites_visible); _mav_put_uint8_t(buf, 34, dgps_numch); + _mav_put_uint16_t(buf, 35, yaw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); #else @@ -121,6 +126,7 @@ static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t comp packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; packet.dgps_numch = dgps_numch; + packet.yaw = yaw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); #endif @@ -140,18 +146,19 @@ static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t comp * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph [cm] GPS HDOP horizontal dilution of position. If unknown, set to: UINT16_MAX - * @param epv [cm] GPS VDOP vertical dilution of position. If unknown, set to: UINT16_MAX + * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement): 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 * @param dgps_numch Number of DGPS satellites * @param dgps_age [ms] Age of DGPS info + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible,uint8_t dgps_numch,uint32_t dgps_age) + uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible,uint8_t dgps_numch,uint32_t dgps_age,uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; @@ -167,6 +174,7 @@ static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t _mav_put_uint8_t(buf, 32, fix_type); _mav_put_uint8_t(buf, 33, satellites_visible); _mav_put_uint8_t(buf, 34, dgps_numch); + _mav_put_uint16_t(buf, 35, yaw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); #else @@ -183,6 +191,7 @@ static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; packet.dgps_numch = dgps_numch; + packet.yaw = yaw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); #endif @@ -201,7 +210,7 @@ static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_gps2_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw) { - return mavlink_msg_gps2_raw_pack(system_id, component_id, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age); + return mavlink_msg_gps2_raw_pack(system_id, component_id, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age, gps2_raw->yaw); } /** @@ -215,7 +224,7 @@ static inline uint16_t mavlink_msg_gps2_raw_encode(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_gps2_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw) { - return mavlink_msg_gps2_raw_pack_chan(system_id, component_id, chan, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age); + return mavlink_msg_gps2_raw_pack_chan(system_id, component_id, chan, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age, gps2_raw->yaw); } /** @@ -227,17 +236,18 @@ static inline uint16_t mavlink_msg_gps2_raw_encode_chan(uint8_t system_id, uint8 * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph [cm] GPS HDOP horizontal dilution of position. If unknown, set to: UINT16_MAX - * @param epv [cm] GPS VDOP vertical dilution of position. If unknown, set to: UINT16_MAX + * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement): 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 * @param dgps_numch Number of DGPS satellites * @param dgps_age [ms] Age of DGPS info + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) +static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; @@ -253,6 +263,7 @@ static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t ti _mav_put_uint8_t(buf, 32, fix_type); _mav_put_uint8_t(buf, 33, satellites_visible); _mav_put_uint8_t(buf, 34, dgps_numch); + _mav_put_uint16_t(buf, 35, yaw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); #else @@ -269,6 +280,7 @@ static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t ti packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; packet.dgps_numch = dgps_numch; + packet.yaw = yaw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); #endif @@ -282,7 +294,7 @@ static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t ti static inline void mavlink_msg_gps2_raw_send_struct(mavlink_channel_t chan, const mavlink_gps2_raw_t* gps2_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_gps2_raw_send(chan, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age); + mavlink_msg_gps2_raw_send(chan, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age, gps2_raw->yaw); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)gps2_raw, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); #endif @@ -296,7 +308,7 @@ static inline void mavlink_msg_gps2_raw_send_struct(mavlink_channel_t chan, cons is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) +static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -312,6 +324,7 @@ static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavl _mav_put_uint8_t(buf, 32, fix_type); _mav_put_uint8_t(buf, 33, satellites_visible); _mav_put_uint8_t(buf, 34, dgps_numch); + _mav_put_uint16_t(buf, 35, yaw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); #else @@ -328,6 +341,7 @@ static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavl packet->fix_type = fix_type; packet->satellites_visible = satellites_visible; packet->dgps_numch = dgps_numch; + packet->yaw = yaw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)packet, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); #endif @@ -392,7 +406,7 @@ static inline int32_t mavlink_msg_gps2_raw_get_alt(const mavlink_message_t* msg) /** * @brief Get field eph from gps2_raw message * - * @return [cm] GPS HDOP horizontal dilution of position. If unknown, set to: UINT16_MAX + * @return GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps2_raw_get_eph(const mavlink_message_t* msg) { @@ -402,7 +416,7 @@ static inline uint16_t mavlink_msg_gps2_raw_get_eph(const mavlink_message_t* msg /** * @brief Get field epv from gps2_raw message * - * @return [cm] GPS VDOP vertical dilution of position. If unknown, set to: UINT16_MAX + * @return GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps2_raw_get_epv(const mavlink_message_t* msg) { @@ -459,6 +473,16 @@ static inline uint32_t mavlink_msg_gps2_raw_get_dgps_age(const mavlink_message_t return _MAV_RETURN_uint32_t(msg, 20); } +/** + * @brief Get field yaw from gps2_raw message + * + * @return [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + */ +static inline uint16_t mavlink_msg_gps2_raw_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 35); +} + /** * @brief Decode a gps2_raw message into a struct * @@ -480,6 +504,7 @@ static inline void mavlink_msg_gps2_raw_decode(const mavlink_message_t* msg, mav gps2_raw->fix_type = mavlink_msg_gps2_raw_get_fix_type(msg); gps2_raw->satellites_visible = mavlink_msg_gps2_raw_get_satellites_visible(msg); gps2_raw->dgps_numch = mavlink_msg_gps2_raw_get_dgps_numch(msg); + gps2_raw->yaw = mavlink_msg_gps2_raw_get_yaw(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_GPS2_RAW_LEN? msg->len : MAVLINK_MSG_ID_GPS2_RAW_LEN; memset(gps2_raw, 0, MAVLINK_MSG_ID_GPS2_RAW_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_gps_global_origin.h b/lib/main/MAVLink/common/mavlink_msg_gps_global_origin.h index 7719543ee2..ccbde8fd14 100755 --- a/lib/main/MAVLink/common/mavlink_msg_gps_global_origin.h +++ b/lib/main/MAVLink/common/mavlink_msg_gps_global_origin.h @@ -3,16 +3,17 @@ #define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN 49 - +MAVPACKED( typedef struct __mavlink_gps_global_origin_t { int32_t latitude; /*< [degE7] Latitude (WGS84)*/ int32_t longitude; /*< [degE7] Longitude (WGS84)*/ int32_t altitude; /*< [mm] Altitude (MSL). Positive for up.*/ -} mavlink_gps_global_origin_t; + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ +}) mavlink_gps_global_origin_t; -#define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN 12 +#define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN 20 #define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_MIN_LEN 12 -#define MAVLINK_MSG_ID_49_LEN 12 +#define MAVLINK_MSG_ID_49_LEN 20 #define MAVLINK_MSG_ID_49_MIN_LEN 12 #define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC 39 @@ -24,19 +25,21 @@ typedef struct __mavlink_gps_global_origin_t { #define MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN { \ 49, \ "GPS_GLOBAL_ORIGIN", \ - 3, \ + 4, \ { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_gps_global_origin_t, latitude) }, \ { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_gps_global_origin_t, longitude) }, \ { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_global_origin_t, altitude) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 12, offsetof(mavlink_gps_global_origin_t, time_usec) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN { \ "GPS_GLOBAL_ORIGIN", \ - 3, \ + 4, \ { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_gps_global_origin_t, latitude) }, \ { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_gps_global_origin_t, longitude) }, \ { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_global_origin_t, altitude) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 12, offsetof(mavlink_gps_global_origin_t, time_usec) }, \ } \ } #endif @@ -50,16 +53,18 @@ typedef struct __mavlink_gps_global_origin_t { * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - int32_t latitude, int32_t longitude, int32_t altitude) + int32_t latitude, int32_t longitude, int32_t altitude, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN]; _mav_put_int32_t(buf, 0, latitude); _mav_put_int32_t(buf, 4, longitude); _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint64_t(buf, 12, time_usec); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); #else @@ -67,6 +72,7 @@ static inline uint16_t mavlink_msg_gps_global_origin_pack(uint8_t system_id, uin packet.latitude = latitude; packet.longitude = longitude; packet.altitude = altitude; + packet.time_usec = time_usec; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); #endif @@ -84,17 +90,19 @@ static inline uint16_t mavlink_msg_gps_global_origin_pack(uint8_t system_id, uin * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_global_origin_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - int32_t latitude,int32_t longitude,int32_t altitude) + int32_t latitude,int32_t longitude,int32_t altitude,uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN]; _mav_put_int32_t(buf, 0, latitude); _mav_put_int32_t(buf, 4, longitude); _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint64_t(buf, 12, time_usec); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); #else @@ -102,6 +110,7 @@ static inline uint16_t mavlink_msg_gps_global_origin_pack_chan(uint8_t system_id packet.latitude = latitude; packet.longitude = longitude; packet.altitude = altitude; + packet.time_usec = time_usec; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); #endif @@ -120,7 +129,7 @@ static inline uint16_t mavlink_msg_gps_global_origin_pack_chan(uint8_t system_id */ static inline uint16_t mavlink_msg_gps_global_origin_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_global_origin_t* gps_global_origin) { - return mavlink_msg_gps_global_origin_pack(system_id, component_id, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude); + return mavlink_msg_gps_global_origin_pack(system_id, component_id, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude, gps_global_origin->time_usec); } /** @@ -134,7 +143,7 @@ static inline uint16_t mavlink_msg_gps_global_origin_encode(uint8_t system_id, u */ static inline uint16_t mavlink_msg_gps_global_origin_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_global_origin_t* gps_global_origin) { - return mavlink_msg_gps_global_origin_pack_chan(system_id, component_id, chan, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude); + return mavlink_msg_gps_global_origin_pack_chan(system_id, component_id, chan, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude, gps_global_origin->time_usec); } /** @@ -144,16 +153,18 @@ static inline uint16_t mavlink_msg_gps_global_origin_encode_chan(uint8_t system_ * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) +static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN]; _mav_put_int32_t(buf, 0, latitude); _mav_put_int32_t(buf, 4, longitude); _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint64_t(buf, 12, time_usec); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); #else @@ -161,6 +172,7 @@ static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, in packet.latitude = latitude; packet.longitude = longitude; packet.altitude = altitude; + packet.time_usec = time_usec; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)&packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); #endif @@ -174,7 +186,7 @@ static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, in static inline void mavlink_msg_gps_global_origin_send_struct(mavlink_channel_t chan, const mavlink_gps_global_origin_t* gps_global_origin) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_gps_global_origin_send(chan, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude); + mavlink_msg_gps_global_origin_send(chan, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude, gps_global_origin->time_usec); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)gps_global_origin, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); #endif @@ -188,13 +200,14 @@ static inline void mavlink_msg_gps_global_origin_send_struct(mavlink_channel_t c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) +static inline void mavlink_msg_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_int32_t(buf, 0, latitude); _mav_put_int32_t(buf, 4, longitude); _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint64_t(buf, 12, time_usec); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); #else @@ -202,6 +215,7 @@ static inline void mavlink_msg_gps_global_origin_send_buf(mavlink_message_t *msg packet->latitude = latitude; packet->longitude = longitude; packet->altitude = altitude; + packet->time_usec = time_usec; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); #endif @@ -243,6 +257,16 @@ static inline int32_t mavlink_msg_gps_global_origin_get_altitude(const mavlink_m return _MAV_RETURN_int32_t(msg, 8); } +/** + * @brief Get field time_usec from gps_global_origin message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_gps_global_origin_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 12); +} + /** * @brief Decode a gps_global_origin message into a struct * @@ -255,6 +279,7 @@ static inline void mavlink_msg_gps_global_origin_decode(const mavlink_message_t* gps_global_origin->latitude = mavlink_msg_gps_global_origin_get_latitude(msg); gps_global_origin->longitude = mavlink_msg_gps_global_origin_get_longitude(msg); gps_global_origin->altitude = mavlink_msg_gps_global_origin_get_altitude(msg); + gps_global_origin->time_usec = mavlink_msg_gps_global_origin_get_time_usec(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN? msg->len : MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN; memset(gps_global_origin, 0, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_gps_input.h b/lib/main/MAVLink/common/mavlink_msg_gps_input.h index 6268b11af3..de76ddbf1d 100755 --- a/lib/main/MAVLink/common/mavlink_msg_gps_input.h +++ b/lib/main/MAVLink/common/mavlink_msg_gps_input.h @@ -3,15 +3,15 @@ #define MAVLINK_MSG_ID_GPS_INPUT 232 - +MAVPACKED( typedef struct __mavlink_gps_input_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ uint32_t time_week_ms; /*< [ms] GPS time (from start of GPS week)*/ int32_t lat; /*< [degE7] Latitude (WGS84)*/ int32_t lon; /*< [degE7] Longitude (WGS84)*/ float alt; /*< [m] Altitude (MSL). Positive for up.*/ - float hdop; /*< [m] GPS HDOP horizontal dilution of position*/ - float vdop; /*< [m] GPS VDOP vertical dilution of position*/ + float hdop; /*< GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX*/ + float vdop; /*< GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX*/ float vn; /*< [m/s] GPS velocity in north direction in earth-fixed NED frame*/ float ve; /*< [m/s] GPS velocity in east direction in earth-fixed NED frame*/ float vd; /*< [m/s] GPS velocity in down direction in earth-fixed NED frame*/ @@ -23,11 +23,12 @@ typedef struct __mavlink_gps_input_t { uint8_t gps_id; /*< ID of the GPS for multiple GPS inputs*/ uint8_t fix_type; /*< 0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK*/ uint8_t satellites_visible; /*< Number of satellites visible.*/ -} mavlink_gps_input_t; + uint16_t yaw; /*< [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north*/ +}) mavlink_gps_input_t; -#define MAVLINK_MSG_ID_GPS_INPUT_LEN 63 +#define MAVLINK_MSG_ID_GPS_INPUT_LEN 65 #define MAVLINK_MSG_ID_GPS_INPUT_MIN_LEN 63 -#define MAVLINK_MSG_ID_232_LEN 63 +#define MAVLINK_MSG_ID_232_LEN 65 #define MAVLINK_MSG_ID_232_MIN_LEN 63 #define MAVLINK_MSG_ID_GPS_INPUT_CRC 151 @@ -39,7 +40,7 @@ typedef struct __mavlink_gps_input_t { #define MAVLINK_MESSAGE_INFO_GPS_INPUT { \ 232, \ "GPS_INPUT", \ - 18, \ + 19, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_input_t, time_usec) }, \ { "gps_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 60, offsetof(mavlink_gps_input_t, gps_id) }, \ { "ignore_flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 56, offsetof(mavlink_gps_input_t, ignore_flags) }, \ @@ -58,12 +59,13 @@ typedef struct __mavlink_gps_input_t { { "horiz_accuracy", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_gps_input_t, horiz_accuracy) }, \ { "vert_accuracy", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_gps_input_t, vert_accuracy) }, \ { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 62, offsetof(mavlink_gps_input_t, satellites_visible) }, \ + { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 63, offsetof(mavlink_gps_input_t, yaw) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_GPS_INPUT { \ "GPS_INPUT", \ - 18, \ + 19, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_input_t, time_usec) }, \ { "gps_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 60, offsetof(mavlink_gps_input_t, gps_id) }, \ { "ignore_flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 56, offsetof(mavlink_gps_input_t, ignore_flags) }, \ @@ -82,6 +84,7 @@ typedef struct __mavlink_gps_input_t { { "horiz_accuracy", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_gps_input_t, horiz_accuracy) }, \ { "vert_accuracy", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_gps_input_t, vert_accuracy) }, \ { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 62, offsetof(mavlink_gps_input_t, satellites_visible) }, \ + { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 63, offsetof(mavlink_gps_input_t, yaw) }, \ } \ } #endif @@ -101,8 +104,8 @@ typedef struct __mavlink_gps_input_t { * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [m] Altitude (MSL). Positive for up. - * @param hdop [m] GPS HDOP horizontal dilution of position - * @param vdop [m] GPS VDOP vertical dilution of position + * @param hdop GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param vdop GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vn [m/s] GPS velocity in north direction in earth-fixed NED frame * @param ve [m/s] GPS velocity in east direction in earth-fixed NED frame * @param vd [m/s] GPS velocity in down direction in earth-fixed NED frame @@ -110,10 +113,11 @@ typedef struct __mavlink_gps_input_t { * @param horiz_accuracy [m] GPS horizontal accuracy * @param vert_accuracy [m] GPS vertical accuracy * @param satellites_visible Number of satellites visible. + * @param yaw [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_input_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t gps_id, uint16_t ignore_flags, uint32_t time_week_ms, uint16_t time_week, uint8_t fix_type, int32_t lat, int32_t lon, float alt, float hdop, float vdop, float vn, float ve, float vd, float speed_accuracy, float horiz_accuracy, float vert_accuracy, uint8_t satellites_visible) + uint64_t time_usec, uint8_t gps_id, uint16_t ignore_flags, uint32_t time_week_ms, uint16_t time_week, uint8_t fix_type, int32_t lat, int32_t lon, float alt, float hdop, float vdop, float vn, float ve, float vd, float speed_accuracy, float horiz_accuracy, float vert_accuracy, uint8_t satellites_visible, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_INPUT_LEN]; @@ -135,6 +139,7 @@ static inline uint16_t mavlink_msg_gps_input_pack(uint8_t system_id, uint8_t com _mav_put_uint8_t(buf, 60, gps_id); _mav_put_uint8_t(buf, 61, fix_type); _mav_put_uint8_t(buf, 62, satellites_visible); + _mav_put_uint16_t(buf, 63, yaw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INPUT_LEN); #else @@ -157,6 +162,7 @@ static inline uint16_t mavlink_msg_gps_input_pack(uint8_t system_id, uint8_t com packet.gps_id = gps_id; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.yaw = yaw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INPUT_LEN); #endif @@ -180,8 +186,8 @@ static inline uint16_t mavlink_msg_gps_input_pack(uint8_t system_id, uint8_t com * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [m] Altitude (MSL). Positive for up. - * @param hdop [m] GPS HDOP horizontal dilution of position - * @param vdop [m] GPS VDOP vertical dilution of position + * @param hdop GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param vdop GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vn [m/s] GPS velocity in north direction in earth-fixed NED frame * @param ve [m/s] GPS velocity in east direction in earth-fixed NED frame * @param vd [m/s] GPS velocity in down direction in earth-fixed NED frame @@ -189,11 +195,12 @@ static inline uint16_t mavlink_msg_gps_input_pack(uint8_t system_id, uint8_t com * @param horiz_accuracy [m] GPS horizontal accuracy * @param vert_accuracy [m] GPS vertical accuracy * @param satellites_visible Number of satellites visible. + * @param yaw [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_input_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,uint8_t gps_id,uint16_t ignore_flags,uint32_t time_week_ms,uint16_t time_week,uint8_t fix_type,int32_t lat,int32_t lon,float alt,float hdop,float vdop,float vn,float ve,float vd,float speed_accuracy,float horiz_accuracy,float vert_accuracy,uint8_t satellites_visible) + uint64_t time_usec,uint8_t gps_id,uint16_t ignore_flags,uint32_t time_week_ms,uint16_t time_week,uint8_t fix_type,int32_t lat,int32_t lon,float alt,float hdop,float vdop,float vn,float ve,float vd,float speed_accuracy,float horiz_accuracy,float vert_accuracy,uint8_t satellites_visible,uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_INPUT_LEN]; @@ -215,6 +222,7 @@ static inline uint16_t mavlink_msg_gps_input_pack_chan(uint8_t system_id, uint8_ _mav_put_uint8_t(buf, 60, gps_id); _mav_put_uint8_t(buf, 61, fix_type); _mav_put_uint8_t(buf, 62, satellites_visible); + _mav_put_uint16_t(buf, 63, yaw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INPUT_LEN); #else @@ -237,6 +245,7 @@ static inline uint16_t mavlink_msg_gps_input_pack_chan(uint8_t system_id, uint8_ packet.gps_id = gps_id; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.yaw = yaw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INPUT_LEN); #endif @@ -255,7 +264,7 @@ static inline uint16_t mavlink_msg_gps_input_pack_chan(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_gps_input_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_input_t* gps_input) { - return mavlink_msg_gps_input_pack(system_id, component_id, msg, gps_input->time_usec, gps_input->gps_id, gps_input->ignore_flags, gps_input->time_week_ms, gps_input->time_week, gps_input->fix_type, gps_input->lat, gps_input->lon, gps_input->alt, gps_input->hdop, gps_input->vdop, gps_input->vn, gps_input->ve, gps_input->vd, gps_input->speed_accuracy, gps_input->horiz_accuracy, gps_input->vert_accuracy, gps_input->satellites_visible); + return mavlink_msg_gps_input_pack(system_id, component_id, msg, gps_input->time_usec, gps_input->gps_id, gps_input->ignore_flags, gps_input->time_week_ms, gps_input->time_week, gps_input->fix_type, gps_input->lat, gps_input->lon, gps_input->alt, gps_input->hdop, gps_input->vdop, gps_input->vn, gps_input->ve, gps_input->vd, gps_input->speed_accuracy, gps_input->horiz_accuracy, gps_input->vert_accuracy, gps_input->satellites_visible, gps_input->yaw); } /** @@ -269,7 +278,7 @@ static inline uint16_t mavlink_msg_gps_input_encode(uint8_t system_id, uint8_t c */ static inline uint16_t mavlink_msg_gps_input_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_input_t* gps_input) { - return mavlink_msg_gps_input_pack_chan(system_id, component_id, chan, msg, gps_input->time_usec, gps_input->gps_id, gps_input->ignore_flags, gps_input->time_week_ms, gps_input->time_week, gps_input->fix_type, gps_input->lat, gps_input->lon, gps_input->alt, gps_input->hdop, gps_input->vdop, gps_input->vn, gps_input->ve, gps_input->vd, gps_input->speed_accuracy, gps_input->horiz_accuracy, gps_input->vert_accuracy, gps_input->satellites_visible); + return mavlink_msg_gps_input_pack_chan(system_id, component_id, chan, msg, gps_input->time_usec, gps_input->gps_id, gps_input->ignore_flags, gps_input->time_week_ms, gps_input->time_week, gps_input->fix_type, gps_input->lat, gps_input->lon, gps_input->alt, gps_input->hdop, gps_input->vdop, gps_input->vn, gps_input->ve, gps_input->vd, gps_input->speed_accuracy, gps_input->horiz_accuracy, gps_input->vert_accuracy, gps_input->satellites_visible, gps_input->yaw); } /** @@ -285,8 +294,8 @@ static inline uint16_t mavlink_msg_gps_input_encode_chan(uint8_t system_id, uint * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [m] Altitude (MSL). Positive for up. - * @param hdop [m] GPS HDOP horizontal dilution of position - * @param vdop [m] GPS VDOP vertical dilution of position + * @param hdop GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param vdop GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vn [m/s] GPS velocity in north direction in earth-fixed NED frame * @param ve [m/s] GPS velocity in east direction in earth-fixed NED frame * @param vd [m/s] GPS velocity in down direction in earth-fixed NED frame @@ -294,10 +303,11 @@ static inline uint16_t mavlink_msg_gps_input_encode_chan(uint8_t system_id, uint * @param horiz_accuracy [m] GPS horizontal accuracy * @param vert_accuracy [m] GPS vertical accuracy * @param satellites_visible Number of satellites visible. + * @param yaw [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_input_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t gps_id, uint16_t ignore_flags, uint32_t time_week_ms, uint16_t time_week, uint8_t fix_type, int32_t lat, int32_t lon, float alt, float hdop, float vdop, float vn, float ve, float vd, float speed_accuracy, float horiz_accuracy, float vert_accuracy, uint8_t satellites_visible) +static inline void mavlink_msg_gps_input_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t gps_id, uint16_t ignore_flags, uint32_t time_week_ms, uint16_t time_week, uint8_t fix_type, int32_t lat, int32_t lon, float alt, float hdop, float vdop, float vn, float ve, float vd, float speed_accuracy, float horiz_accuracy, float vert_accuracy, uint8_t satellites_visible, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_INPUT_LEN]; @@ -319,6 +329,7 @@ static inline void mavlink_msg_gps_input_send(mavlink_channel_t chan, uint64_t t _mav_put_uint8_t(buf, 60, gps_id); _mav_put_uint8_t(buf, 61, fix_type); _mav_put_uint8_t(buf, 62, satellites_visible); + _mav_put_uint16_t(buf, 63, yaw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INPUT, buf, MAVLINK_MSG_ID_GPS_INPUT_MIN_LEN, MAVLINK_MSG_ID_GPS_INPUT_LEN, MAVLINK_MSG_ID_GPS_INPUT_CRC); #else @@ -341,6 +352,7 @@ static inline void mavlink_msg_gps_input_send(mavlink_channel_t chan, uint64_t t packet.gps_id = gps_id; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.yaw = yaw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INPUT, (const char *)&packet, MAVLINK_MSG_ID_GPS_INPUT_MIN_LEN, MAVLINK_MSG_ID_GPS_INPUT_LEN, MAVLINK_MSG_ID_GPS_INPUT_CRC); #endif @@ -354,7 +366,7 @@ static inline void mavlink_msg_gps_input_send(mavlink_channel_t chan, uint64_t t static inline void mavlink_msg_gps_input_send_struct(mavlink_channel_t chan, const mavlink_gps_input_t* gps_input) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_gps_input_send(chan, gps_input->time_usec, gps_input->gps_id, gps_input->ignore_flags, gps_input->time_week_ms, gps_input->time_week, gps_input->fix_type, gps_input->lat, gps_input->lon, gps_input->alt, gps_input->hdop, gps_input->vdop, gps_input->vn, gps_input->ve, gps_input->vd, gps_input->speed_accuracy, gps_input->horiz_accuracy, gps_input->vert_accuracy, gps_input->satellites_visible); + mavlink_msg_gps_input_send(chan, gps_input->time_usec, gps_input->gps_id, gps_input->ignore_flags, gps_input->time_week_ms, gps_input->time_week, gps_input->fix_type, gps_input->lat, gps_input->lon, gps_input->alt, gps_input->hdop, gps_input->vdop, gps_input->vn, gps_input->ve, gps_input->vd, gps_input->speed_accuracy, gps_input->horiz_accuracy, gps_input->vert_accuracy, gps_input->satellites_visible, gps_input->yaw); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INPUT, (const char *)gps_input, MAVLINK_MSG_ID_GPS_INPUT_MIN_LEN, MAVLINK_MSG_ID_GPS_INPUT_LEN, MAVLINK_MSG_ID_GPS_INPUT_CRC); #endif @@ -368,7 +380,7 @@ static inline void mavlink_msg_gps_input_send_struct(mavlink_channel_t chan, con is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_gps_input_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t gps_id, uint16_t ignore_flags, uint32_t time_week_ms, uint16_t time_week, uint8_t fix_type, int32_t lat, int32_t lon, float alt, float hdop, float vdop, float vn, float ve, float vd, float speed_accuracy, float horiz_accuracy, float vert_accuracy, uint8_t satellites_visible) +static inline void mavlink_msg_gps_input_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t gps_id, uint16_t ignore_flags, uint32_t time_week_ms, uint16_t time_week, uint8_t fix_type, int32_t lat, int32_t lon, float alt, float hdop, float vdop, float vn, float ve, float vd, float speed_accuracy, float horiz_accuracy, float vert_accuracy, uint8_t satellites_visible, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -390,6 +402,7 @@ static inline void mavlink_msg_gps_input_send_buf(mavlink_message_t *msgbuf, mav _mav_put_uint8_t(buf, 60, gps_id); _mav_put_uint8_t(buf, 61, fix_type); _mav_put_uint8_t(buf, 62, satellites_visible); + _mav_put_uint16_t(buf, 63, yaw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INPUT, buf, MAVLINK_MSG_ID_GPS_INPUT_MIN_LEN, MAVLINK_MSG_ID_GPS_INPUT_LEN, MAVLINK_MSG_ID_GPS_INPUT_CRC); #else @@ -412,6 +425,7 @@ static inline void mavlink_msg_gps_input_send_buf(mavlink_message_t *msgbuf, mav packet->gps_id = gps_id; packet->fix_type = fix_type; packet->satellites_visible = satellites_visible; + packet->yaw = yaw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INPUT, (const char *)packet, MAVLINK_MSG_ID_GPS_INPUT_MIN_LEN, MAVLINK_MSG_ID_GPS_INPUT_LEN, MAVLINK_MSG_ID_GPS_INPUT_CRC); #endif @@ -516,7 +530,7 @@ static inline float mavlink_msg_gps_input_get_alt(const mavlink_message_t* msg) /** * @brief Get field hdop from gps_input message * - * @return [m] GPS HDOP horizontal dilution of position + * @return GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX */ static inline float mavlink_msg_gps_input_get_hdop(const mavlink_message_t* msg) { @@ -526,7 +540,7 @@ static inline float mavlink_msg_gps_input_get_hdop(const mavlink_message_t* msg) /** * @brief Get field vdop from gps_input message * - * @return [m] GPS VDOP vertical dilution of position + * @return GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX */ static inline float mavlink_msg_gps_input_get_vdop(const mavlink_message_t* msg) { @@ -603,6 +617,16 @@ static inline uint8_t mavlink_msg_gps_input_get_satellites_visible(const mavlink return _MAV_RETURN_uint8_t(msg, 62); } +/** + * @brief Get field yaw from gps_input message + * + * @return [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north + */ +static inline uint16_t mavlink_msg_gps_input_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 63); +} + /** * @brief Decode a gps_input message into a struct * @@ -630,6 +654,7 @@ static inline void mavlink_msg_gps_input_decode(const mavlink_message_t* msg, ma gps_input->gps_id = mavlink_msg_gps_input_get_gps_id(msg); gps_input->fix_type = mavlink_msg_gps_input_get_fix_type(msg); gps_input->satellites_visible = mavlink_msg_gps_input_get_satellites_visible(msg); + gps_input->yaw = mavlink_msg_gps_input_get_yaw(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_GPS_INPUT_LEN? msg->len : MAVLINK_MSG_ID_GPS_INPUT_LEN; memset(gps_input, 0, MAVLINK_MSG_ID_GPS_INPUT_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_gps_raw_int.h b/lib/main/MAVLink/common/mavlink_msg_gps_raw_int.h index 6e22537c93..b87d82e489 100755 --- a/lib/main/MAVLink/common/mavlink_msg_gps_raw_int.h +++ b/lib/main/MAVLink/common/mavlink_msg_gps_raw_int.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_GPS_RAW_INT 24 - +MAVPACKED( typedef struct __mavlink_gps_raw_int_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ int32_t lat; /*< [degE7] Latitude (WGS84, EGM96 ellipsoid)*/ @@ -15,11 +15,17 @@ typedef struct __mavlink_gps_raw_int_t { uint16_t cog; /*< [cdeg] Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX*/ uint8_t fix_type; /*< GPS fix type.*/ uint8_t satellites_visible; /*< Number of satellites visible. If unknown, set to 255*/ -} mavlink_gps_raw_int_t; + int32_t alt_ellipsoid; /*< [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up.*/ + uint32_t h_acc; /*< [mm] Position uncertainty.*/ + uint32_t v_acc; /*< [mm] Altitude uncertainty.*/ + uint32_t vel_acc; /*< [mm] Speed uncertainty.*/ + uint32_t hdg_acc; /*< [degE5] Heading / track uncertainty*/ + uint16_t yaw; /*< [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north.*/ +}) mavlink_gps_raw_int_t; -#define MAVLINK_MSG_ID_GPS_RAW_INT_LEN 30 +#define MAVLINK_MSG_ID_GPS_RAW_INT_LEN 52 #define MAVLINK_MSG_ID_GPS_RAW_INT_MIN_LEN 30 -#define MAVLINK_MSG_ID_24_LEN 30 +#define MAVLINK_MSG_ID_24_LEN 52 #define MAVLINK_MSG_ID_24_MIN_LEN 30 #define MAVLINK_MSG_ID_GPS_RAW_INT_CRC 24 @@ -31,7 +37,7 @@ typedef struct __mavlink_gps_raw_int_t { #define MAVLINK_MESSAGE_INFO_GPS_RAW_INT { \ 24, \ "GPS_RAW_INT", \ - 10, \ + 16, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_raw_int_t, time_usec) }, \ { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_gps_raw_int_t, fix_type) }, \ { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_raw_int_t, lat) }, \ @@ -42,12 +48,18 @@ typedef struct __mavlink_gps_raw_int_t { { "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_gps_raw_int_t, vel) }, \ { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_gps_raw_int_t, cog) }, \ { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_gps_raw_int_t, satellites_visible) }, \ + { "alt_ellipsoid", NULL, MAVLINK_TYPE_INT32_T, 0, 30, offsetof(mavlink_gps_raw_int_t, alt_ellipsoid) }, \ + { "h_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 34, offsetof(mavlink_gps_raw_int_t, h_acc) }, \ + { "v_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 38, offsetof(mavlink_gps_raw_int_t, v_acc) }, \ + { "vel_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 42, offsetof(mavlink_gps_raw_int_t, vel_acc) }, \ + { "hdg_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 46, offsetof(mavlink_gps_raw_int_t, hdg_acc) }, \ + { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 50, offsetof(mavlink_gps_raw_int_t, yaw) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_GPS_RAW_INT { \ "GPS_RAW_INT", \ - 10, \ + 16, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_raw_int_t, time_usec) }, \ { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_gps_raw_int_t, fix_type) }, \ { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_raw_int_t, lat) }, \ @@ -58,6 +70,12 @@ typedef struct __mavlink_gps_raw_int_t { { "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_gps_raw_int_t, vel) }, \ { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_gps_raw_int_t, cog) }, \ { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_gps_raw_int_t, satellites_visible) }, \ + { "alt_ellipsoid", NULL, MAVLINK_TYPE_INT32_T, 0, 30, offsetof(mavlink_gps_raw_int_t, alt_ellipsoid) }, \ + { "h_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 34, offsetof(mavlink_gps_raw_int_t, h_acc) }, \ + { "v_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 38, offsetof(mavlink_gps_raw_int_t, v_acc) }, \ + { "vel_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 42, offsetof(mavlink_gps_raw_int_t, vel_acc) }, \ + { "hdg_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 46, offsetof(mavlink_gps_raw_int_t, hdg_acc) }, \ + { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 50, offsetof(mavlink_gps_raw_int_t, yaw) }, \ } \ } #endif @@ -78,10 +96,16 @@ typedef struct __mavlink_gps_raw_int_t { * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param alt_ellipsoid [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + * @param h_acc [mm] Position uncertainty. + * @param v_acc [mm] Altitude uncertainty. + * @param vel_acc [mm] Speed uncertainty. + * @param hdg_acc [degE5] Heading / track uncertainty + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible) + uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, int32_t alt_ellipsoid, uint32_t h_acc, uint32_t v_acc, uint32_t vel_acc, uint32_t hdg_acc, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_RAW_INT_LEN]; @@ -95,6 +119,12 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c _mav_put_uint16_t(buf, 26, cog); _mav_put_uint8_t(buf, 28, fix_type); _mav_put_uint8_t(buf, 29, satellites_visible); + _mav_put_int32_t(buf, 30, alt_ellipsoid); + _mav_put_uint32_t(buf, 34, h_acc); + _mav_put_uint32_t(buf, 38, v_acc); + _mav_put_uint32_t(buf, 42, vel_acc); + _mav_put_uint32_t(buf, 46, hdg_acc); + _mav_put_uint16_t(buf, 50, yaw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); #else @@ -109,6 +139,12 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c packet.cog = cog; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.alt_ellipsoid = alt_ellipsoid; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.hdg_acc = hdg_acc; + packet.yaw = yaw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); #endif @@ -133,11 +169,17 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param alt_ellipsoid [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + * @param h_acc [mm] Position uncertainty. + * @param v_acc [mm] Altitude uncertainty. + * @param vel_acc [mm] Speed uncertainty. + * @param hdg_acc [degE5] Heading / track uncertainty + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible) + uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible,int32_t alt_ellipsoid,uint32_t h_acc,uint32_t v_acc,uint32_t vel_acc,uint32_t hdg_acc,uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_RAW_INT_LEN]; @@ -151,6 +193,12 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint _mav_put_uint16_t(buf, 26, cog); _mav_put_uint8_t(buf, 28, fix_type); _mav_put_uint8_t(buf, 29, satellites_visible); + _mav_put_int32_t(buf, 30, alt_ellipsoid); + _mav_put_uint32_t(buf, 34, h_acc); + _mav_put_uint32_t(buf, 38, v_acc); + _mav_put_uint32_t(buf, 42, vel_acc); + _mav_put_uint32_t(buf, 46, hdg_acc); + _mav_put_uint16_t(buf, 50, yaw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); #else @@ -165,6 +213,12 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint packet.cog = cog; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.alt_ellipsoid = alt_ellipsoid; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.hdg_acc = hdg_acc; + packet.yaw = yaw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); #endif @@ -183,7 +237,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_raw_int_t* gps_raw_int) { - return mavlink_msg_gps_raw_int_pack(system_id, component_id, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible); + return mavlink_msg_gps_raw_int_pack(system_id, component_id, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible, gps_raw_int->alt_ellipsoid, gps_raw_int->h_acc, gps_raw_int->v_acc, gps_raw_int->vel_acc, gps_raw_int->hdg_acc, gps_raw_int->yaw); } /** @@ -197,7 +251,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_gps_raw_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_raw_int_t* gps_raw_int) { - return mavlink_msg_gps_raw_int_pack_chan(system_id, component_id, chan, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible); + return mavlink_msg_gps_raw_int_pack_chan(system_id, component_id, chan, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible, gps_raw_int->alt_ellipsoid, gps_raw_int->h_acc, gps_raw_int->v_acc, gps_raw_int->vel_acc, gps_raw_int->hdg_acc, gps_raw_int->yaw); } /** @@ -214,10 +268,16 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode_chan(uint8_t system_id, ui * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param alt_ellipsoid [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + * @param h_acc [mm] Position uncertainty. + * @param v_acc [mm] Altitude uncertainty. + * @param vel_acc [mm] Speed uncertainty. + * @param hdg_acc [degE5] Heading / track uncertainty + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible) +static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, int32_t alt_ellipsoid, uint32_t h_acc, uint32_t v_acc, uint32_t vel_acc, uint32_t hdg_acc, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_RAW_INT_LEN]; @@ -231,6 +291,12 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t _mav_put_uint16_t(buf, 26, cog); _mav_put_uint8_t(buf, 28, fix_type); _mav_put_uint8_t(buf, 29, satellites_visible); + _mav_put_int32_t(buf, 30, alt_ellipsoid); + _mav_put_uint32_t(buf, 34, h_acc); + _mav_put_uint32_t(buf, 38, v_acc); + _mav_put_uint32_t(buf, 42, vel_acc); + _mav_put_uint32_t(buf, 46, hdg_acc); + _mav_put_uint16_t(buf, 50, yaw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_MIN_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); #else @@ -245,6 +311,12 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t packet.cog = cog; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.alt_ellipsoid = alt_ellipsoid; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.hdg_acc = hdg_acc; + packet.yaw = yaw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)&packet, MAVLINK_MSG_ID_GPS_RAW_INT_MIN_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); #endif @@ -258,7 +330,7 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t static inline void mavlink_msg_gps_raw_int_send_struct(mavlink_channel_t chan, const mavlink_gps_raw_int_t* gps_raw_int) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_gps_raw_int_send(chan, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible); + mavlink_msg_gps_raw_int_send(chan, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible, gps_raw_int->alt_ellipsoid, gps_raw_int->h_acc, gps_raw_int->v_acc, gps_raw_int->vel_acc, gps_raw_int->hdg_acc, gps_raw_int->yaw); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)gps_raw_int, MAVLINK_MSG_ID_GPS_RAW_INT_MIN_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); #endif @@ -272,7 +344,7 @@ static inline void mavlink_msg_gps_raw_int_send_struct(mavlink_channel_t chan, c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_gps_raw_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible) +static inline void mavlink_msg_gps_raw_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, int32_t alt_ellipsoid, uint32_t h_acc, uint32_t v_acc, uint32_t vel_acc, uint32_t hdg_acc, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -286,6 +358,12 @@ static inline void mavlink_msg_gps_raw_int_send_buf(mavlink_message_t *msgbuf, m _mav_put_uint16_t(buf, 26, cog); _mav_put_uint8_t(buf, 28, fix_type); _mav_put_uint8_t(buf, 29, satellites_visible); + _mav_put_int32_t(buf, 30, alt_ellipsoid); + _mav_put_uint32_t(buf, 34, h_acc); + _mav_put_uint32_t(buf, 38, v_acc); + _mav_put_uint32_t(buf, 42, vel_acc); + _mav_put_uint32_t(buf, 46, hdg_acc); + _mav_put_uint16_t(buf, 50, yaw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_MIN_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); #else @@ -300,6 +378,12 @@ static inline void mavlink_msg_gps_raw_int_send_buf(mavlink_message_t *msgbuf, m packet->cog = cog; packet->fix_type = fix_type; packet->satellites_visible = satellites_visible; + packet->alt_ellipsoid = alt_ellipsoid; + packet->h_acc = h_acc; + packet->v_acc = v_acc; + packet->vel_acc = vel_acc; + packet->hdg_acc = hdg_acc; + packet->yaw = yaw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)packet, MAVLINK_MSG_ID_GPS_RAW_INT_MIN_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); #endif @@ -411,6 +495,66 @@ static inline uint8_t mavlink_msg_gps_raw_int_get_satellites_visible(const mavli return _MAV_RETURN_uint8_t(msg, 29); } +/** + * @brief Get field alt_ellipsoid from gps_raw_int message + * + * @return [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + */ +static inline int32_t mavlink_msg_gps_raw_int_get_alt_ellipsoid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 30); +} + +/** + * @brief Get field h_acc from gps_raw_int message + * + * @return [mm] Position uncertainty. + */ +static inline uint32_t mavlink_msg_gps_raw_int_get_h_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 34); +} + +/** + * @brief Get field v_acc from gps_raw_int message + * + * @return [mm] Altitude uncertainty. + */ +static inline uint32_t mavlink_msg_gps_raw_int_get_v_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 38); +} + +/** + * @brief Get field vel_acc from gps_raw_int message + * + * @return [mm] Speed uncertainty. + */ +static inline uint32_t mavlink_msg_gps_raw_int_get_vel_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 42); +} + +/** + * @brief Get field hdg_acc from gps_raw_int message + * + * @return [degE5] Heading / track uncertainty + */ +static inline uint32_t mavlink_msg_gps_raw_int_get_hdg_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 46); +} + +/** + * @brief Get field yaw from gps_raw_int message + * + * @return [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + */ +static inline uint16_t mavlink_msg_gps_raw_int_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 50); +} + /** * @brief Decode a gps_raw_int message into a struct * @@ -430,6 +574,12 @@ static inline void mavlink_msg_gps_raw_int_decode(const mavlink_message_t* msg, gps_raw_int->cog = mavlink_msg_gps_raw_int_get_cog(msg); gps_raw_int->fix_type = mavlink_msg_gps_raw_int_get_fix_type(msg); gps_raw_int->satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(msg); + gps_raw_int->alt_ellipsoid = mavlink_msg_gps_raw_int_get_alt_ellipsoid(msg); + gps_raw_int->h_acc = mavlink_msg_gps_raw_int_get_h_acc(msg); + gps_raw_int->v_acc = mavlink_msg_gps_raw_int_get_v_acc(msg); + gps_raw_int->vel_acc = mavlink_msg_gps_raw_int_get_vel_acc(msg); + gps_raw_int->hdg_acc = mavlink_msg_gps_raw_int_get_hdg_acc(msg); + gps_raw_int->yaw = mavlink_msg_gps_raw_int_get_yaw(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_GPS_RAW_INT_LEN? msg->len : MAVLINK_MSG_ID_GPS_RAW_INT_LEN; memset(gps_raw_int, 0, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_highres_imu.h b/lib/main/MAVLink/common/mavlink_msg_highres_imu.h index 59718e30cd..a8e0899923 100755 --- a/lib/main/MAVLink/common/mavlink_msg_highres_imu.h +++ b/lib/main/MAVLink/common/mavlink_msg_highres_imu.h @@ -15,16 +15,17 @@ typedef struct __mavlink_highres_imu_t { float xmag; /*< [gauss] X Magnetic field*/ float ymag; /*< [gauss] Y Magnetic field*/ float zmag; /*< [gauss] Z Magnetic field*/ - float abs_pressure; /*< [mbar] Absolute pressure*/ - float diff_pressure; /*< [mbar] Differential pressure*/ + float abs_pressure; /*< [hPa] Absolute pressure*/ + float diff_pressure; /*< [hPa] Differential pressure*/ float pressure_alt; /*< Altitude calculated from pressure*/ float temperature; /*< [degC] Temperature*/ uint16_t fields_updated; /*< Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature*/ + uint8_t id; /*< Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0)*/ } mavlink_highres_imu_t; -#define MAVLINK_MSG_ID_HIGHRES_IMU_LEN 62 +#define MAVLINK_MSG_ID_HIGHRES_IMU_LEN 63 #define MAVLINK_MSG_ID_HIGHRES_IMU_MIN_LEN 62 -#define MAVLINK_MSG_ID_105_LEN 62 +#define MAVLINK_MSG_ID_105_LEN 63 #define MAVLINK_MSG_ID_105_MIN_LEN 62 #define MAVLINK_MSG_ID_HIGHRES_IMU_CRC 93 @@ -36,7 +37,7 @@ typedef struct __mavlink_highres_imu_t { #define MAVLINK_MESSAGE_INFO_HIGHRES_IMU { \ 105, \ "HIGHRES_IMU", \ - 15, \ + 16, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_highres_imu_t, time_usec) }, \ { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_highres_imu_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_highres_imu_t, yacc) }, \ @@ -52,12 +53,13 @@ typedef struct __mavlink_highres_imu_t { { "pressure_alt", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_highres_imu_t, pressure_alt) }, \ { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_highres_imu_t, temperature) }, \ { "fields_updated", NULL, MAVLINK_TYPE_UINT16_T, 0, 60, offsetof(mavlink_highres_imu_t, fields_updated) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 62, offsetof(mavlink_highres_imu_t, id) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_HIGHRES_IMU { \ "HIGHRES_IMU", \ - 15, \ + 16, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_highres_imu_t, time_usec) }, \ { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_highres_imu_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_highres_imu_t, yacc) }, \ @@ -73,6 +75,7 @@ typedef struct __mavlink_highres_imu_t { { "pressure_alt", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_highres_imu_t, pressure_alt) }, \ { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_highres_imu_t, temperature) }, \ { "fields_updated", NULL, MAVLINK_TYPE_UINT16_T, 0, 60, offsetof(mavlink_highres_imu_t, fields_updated) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 62, offsetof(mavlink_highres_imu_t, id) }, \ } \ } #endif @@ -93,15 +96,16 @@ typedef struct __mavlink_highres_imu_t { * @param xmag [gauss] X Magnetic field * @param ymag [gauss] Y Magnetic field * @param zmag [gauss] Z Magnetic field - * @param abs_pressure [mbar] Absolute pressure - * @param diff_pressure [mbar] Differential pressure + * @param abs_pressure [hPa] Absolute pressure + * @param diff_pressure [hPa] Differential pressure * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + * @param id Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated) + uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated, uint8_t id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIGHRES_IMU_LEN]; @@ -120,6 +124,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c _mav_put_float(buf, 52, pressure_alt); _mav_put_float(buf, 56, temperature); _mav_put_uint16_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 62, id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); #else @@ -139,6 +144,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c packet.pressure_alt = pressure_alt; packet.temperature = temperature; packet.fields_updated = fields_updated; + packet.id = id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); #endif @@ -163,16 +169,17 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c * @param xmag [gauss] X Magnetic field * @param ymag [gauss] Y Magnetic field * @param zmag [gauss] Z Magnetic field - * @param abs_pressure [mbar] Absolute pressure - * @param diff_pressure [mbar] Differential pressure + * @param abs_pressure [hPa] Absolute pressure + * @param diff_pressure [hPa] Differential pressure * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + * @param id Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float xmag,float ymag,float zmag,float abs_pressure,float diff_pressure,float pressure_alt,float temperature,uint16_t fields_updated) + uint64_t time_usec,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float xmag,float ymag,float zmag,float abs_pressure,float diff_pressure,float pressure_alt,float temperature,uint16_t fields_updated,uint8_t id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIGHRES_IMU_LEN]; @@ -191,6 +198,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint _mav_put_float(buf, 52, pressure_alt); _mav_put_float(buf, 56, temperature); _mav_put_uint16_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 62, id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); #else @@ -210,6 +218,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint packet.pressure_alt = pressure_alt; packet.temperature = temperature; packet.fields_updated = fields_updated; + packet.id = id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); #endif @@ -228,7 +237,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_highres_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_highres_imu_t* highres_imu) { - return mavlink_msg_highres_imu_pack(system_id, component_id, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated); + return mavlink_msg_highres_imu_pack(system_id, component_id, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated, highres_imu->id); } /** @@ -242,7 +251,7 @@ static inline uint16_t mavlink_msg_highres_imu_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_highres_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_highres_imu_t* highres_imu) { - return mavlink_msg_highres_imu_pack_chan(system_id, component_id, chan, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated); + return mavlink_msg_highres_imu_pack_chan(system_id, component_id, chan, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated, highres_imu->id); } /** @@ -259,15 +268,16 @@ static inline uint16_t mavlink_msg_highres_imu_encode_chan(uint8_t system_id, ui * @param xmag [gauss] X Magnetic field * @param ymag [gauss] Y Magnetic field * @param zmag [gauss] Z Magnetic field - * @param abs_pressure [mbar] Absolute pressure - * @param diff_pressure [mbar] Differential pressure + * @param abs_pressure [hPa] Absolute pressure + * @param diff_pressure [hPa] Differential pressure * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + * @param id Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated) +static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated, uint8_t id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIGHRES_IMU_LEN]; @@ -286,6 +296,7 @@ static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t _mav_put_float(buf, 52, pressure_alt); _mav_put_float(buf, 56, temperature); _mav_put_uint16_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 62, id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_MIN_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); #else @@ -305,6 +316,7 @@ static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t packet.pressure_alt = pressure_alt; packet.temperature = temperature; packet.fields_updated = fields_updated; + packet.id = id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)&packet, MAVLINK_MSG_ID_HIGHRES_IMU_MIN_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); #endif @@ -318,7 +330,7 @@ static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t static inline void mavlink_msg_highres_imu_send_struct(mavlink_channel_t chan, const mavlink_highres_imu_t* highres_imu) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_highres_imu_send(chan, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated); + mavlink_msg_highres_imu_send(chan, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated, highres_imu->id); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)highres_imu, MAVLINK_MSG_ID_HIGHRES_IMU_MIN_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); #endif @@ -332,7 +344,7 @@ static inline void mavlink_msg_highres_imu_send_struct(mavlink_channel_t chan, c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_highres_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated) +static inline void mavlink_msg_highres_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated, uint8_t id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -351,6 +363,7 @@ static inline void mavlink_msg_highres_imu_send_buf(mavlink_message_t *msgbuf, m _mav_put_float(buf, 52, pressure_alt); _mav_put_float(buf, 56, temperature); _mav_put_uint16_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 62, id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_MIN_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); #else @@ -370,6 +383,7 @@ static inline void mavlink_msg_highres_imu_send_buf(mavlink_message_t *msgbuf, m packet->pressure_alt = pressure_alt; packet->temperature = temperature; packet->fields_updated = fields_updated; + packet->id = id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)packet, MAVLINK_MSG_ID_HIGHRES_IMU_MIN_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); #endif @@ -484,7 +498,7 @@ static inline float mavlink_msg_highres_imu_get_zmag(const mavlink_message_t* ms /** * @brief Get field abs_pressure from highres_imu message * - * @return [mbar] Absolute pressure + * @return [hPa] Absolute pressure */ static inline float mavlink_msg_highres_imu_get_abs_pressure(const mavlink_message_t* msg) { @@ -494,7 +508,7 @@ static inline float mavlink_msg_highres_imu_get_abs_pressure(const mavlink_messa /** * @brief Get field diff_pressure from highres_imu message * - * @return [mbar] Differential pressure + * @return [hPa] Differential pressure */ static inline float mavlink_msg_highres_imu_get_diff_pressure(const mavlink_message_t* msg) { @@ -531,6 +545,16 @@ static inline uint16_t mavlink_msg_highres_imu_get_fields_updated(const mavlink_ return _MAV_RETURN_uint16_t(msg, 60); } +/** + * @brief Get field id from highres_imu message + * + * @return Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) + */ +static inline uint8_t mavlink_msg_highres_imu_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 62); +} + /** * @brief Decode a highres_imu message into a struct * @@ -555,6 +579,7 @@ static inline void mavlink_msg_highres_imu_decode(const mavlink_message_t* msg, highres_imu->pressure_alt = mavlink_msg_highres_imu_get_pressure_alt(msg); highres_imu->temperature = mavlink_msg_highres_imu_get_temperature(msg); highres_imu->fields_updated = mavlink_msg_highres_imu_get_fields_updated(msg); + highres_imu->id = mavlink_msg_highres_imu_get_id(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_HIGHRES_IMU_LEN? msg->len : MAVLINK_MSG_ID_HIGHRES_IMU_LEN; memset(highres_imu, 0, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_hil_gps.h b/lib/main/MAVLink/common/mavlink_msg_hil_gps.h index 1d1c2575c8..4b8ff2b85e 100755 --- a/lib/main/MAVLink/common/mavlink_msg_hil_gps.h +++ b/lib/main/MAVLink/common/mavlink_msg_hil_gps.h @@ -3,14 +3,14 @@ #define MAVLINK_MSG_ID_HIL_GPS 113 - +MAVPACKED( typedef struct __mavlink_hil_gps_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ int32_t lat; /*< [degE7] Latitude (WGS84)*/ int32_t lon; /*< [degE7] Longitude (WGS84)*/ int32_t alt; /*< [mm] Altitude (MSL). Positive for up.*/ - uint16_t eph; /*< [cm] GPS HDOP horizontal dilution of position. If unknown, set to: 65535*/ - uint16_t epv; /*< [cm] GPS VDOP vertical dilution of position. If unknown, set to: 65535*/ + uint16_t eph; /*< GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX*/ + uint16_t epv; /*< GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX*/ uint16_t vel; /*< [cm/s] GPS ground speed. If unknown, set to: 65535*/ int16_t vn; /*< [cm/s] GPS velocity in north direction in earth-fixed NED frame*/ int16_t ve; /*< [cm/s] GPS velocity in east direction in earth-fixed NED frame*/ @@ -18,11 +18,13 @@ typedef struct __mavlink_hil_gps_t { uint16_t cog; /*< [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: 65535*/ uint8_t fix_type; /*< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.*/ uint8_t satellites_visible; /*< Number of satellites visible. If unknown, set to 255*/ -} mavlink_hil_gps_t; + uint8_t id; /*< GPS ID (zero indexed). Used for multiple GPS inputs*/ + uint16_t yaw; /*< [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north*/ +}) mavlink_hil_gps_t; -#define MAVLINK_MSG_ID_HIL_GPS_LEN 36 +#define MAVLINK_MSG_ID_HIL_GPS_LEN 39 #define MAVLINK_MSG_ID_HIL_GPS_MIN_LEN 36 -#define MAVLINK_MSG_ID_113_LEN 36 +#define MAVLINK_MSG_ID_113_LEN 39 #define MAVLINK_MSG_ID_113_MIN_LEN 36 #define MAVLINK_MSG_ID_HIL_GPS_CRC 124 @@ -34,7 +36,7 @@ typedef struct __mavlink_hil_gps_t { #define MAVLINK_MESSAGE_INFO_HIL_GPS { \ 113, \ "HIL_GPS", \ - 13, \ + 15, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_gps_t, time_usec) }, \ { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_hil_gps_t, fix_type) }, \ { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_hil_gps_t, lat) }, \ @@ -48,12 +50,14 @@ typedef struct __mavlink_hil_gps_t { { "vd", NULL, MAVLINK_TYPE_INT16_T, 0, 30, offsetof(mavlink_hil_gps_t, vd) }, \ { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_hil_gps_t, cog) }, \ { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_hil_gps_t, satellites_visible) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_hil_gps_t, id) }, \ + { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 37, offsetof(mavlink_hil_gps_t, yaw) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_HIL_GPS { \ "HIL_GPS", \ - 13, \ + 15, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_gps_t, time_usec) }, \ { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_hil_gps_t, fix_type) }, \ { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_hil_gps_t, lat) }, \ @@ -67,6 +71,8 @@ typedef struct __mavlink_hil_gps_t { { "vd", NULL, MAVLINK_TYPE_INT16_T, 0, 30, offsetof(mavlink_hil_gps_t, vd) }, \ { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_hil_gps_t, cog) }, \ { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_hil_gps_t, satellites_visible) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_hil_gps_t, id) }, \ + { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 37, offsetof(mavlink_hil_gps_t, yaw) }, \ } \ } #endif @@ -82,18 +88,20 @@ typedef struct __mavlink_hil_gps_t { * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph [cm] GPS HDOP horizontal dilution of position. If unknown, set to: 65535 - * @param epv [cm] GPS VDOP vertical dilution of position. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: 65535 * @param vn [cm/s] GPS velocity in north direction in earth-fixed NED frame * @param ve [cm/s] GPS velocity in east direction in earth-fixed NED frame * @param vd [cm/s] GPS velocity in down direction in earth-fixed NED frame * @param cog [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: 65535 * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param id GPS ID (zero indexed). Used for multiple GPS inputs + * @param yaw [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible) + uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible, uint8_t id, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIL_GPS_LEN]; @@ -110,6 +118,8 @@ static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t compo _mav_put_uint16_t(buf, 32, cog); _mav_put_uint8_t(buf, 34, fix_type); _mav_put_uint8_t(buf, 35, satellites_visible); + _mav_put_uint8_t(buf, 36, id); + _mav_put_uint16_t(buf, 37, yaw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_GPS_LEN); #else @@ -127,6 +137,8 @@ static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t compo packet.cog = cog; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.id = id; + packet.yaw = yaw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_GPS_LEN); #endif @@ -146,19 +158,21 @@ static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t compo * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph [cm] GPS HDOP horizontal dilution of position. If unknown, set to: 65535 - * @param epv [cm] GPS VDOP vertical dilution of position. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: 65535 * @param vn [cm/s] GPS velocity in north direction in earth-fixed NED frame * @param ve [cm/s] GPS velocity in east direction in earth-fixed NED frame * @param vd [cm/s] GPS velocity in down direction in earth-fixed NED frame * @param cog [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: 65535 * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param id GPS ID (zero indexed). Used for multiple GPS inputs + * @param yaw [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_hil_gps_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,int16_t vn,int16_t ve,int16_t vd,uint16_t cog,uint8_t satellites_visible) + uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,int16_t vn,int16_t ve,int16_t vd,uint16_t cog,uint8_t satellites_visible,uint8_t id,uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIL_GPS_LEN]; @@ -175,6 +189,8 @@ static inline uint16_t mavlink_msg_hil_gps_pack_chan(uint8_t system_id, uint8_t _mav_put_uint16_t(buf, 32, cog); _mav_put_uint8_t(buf, 34, fix_type); _mav_put_uint8_t(buf, 35, satellites_visible); + _mav_put_uint8_t(buf, 36, id); + _mav_put_uint16_t(buf, 37, yaw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_GPS_LEN); #else @@ -192,6 +208,8 @@ static inline uint16_t mavlink_msg_hil_gps_pack_chan(uint8_t system_id, uint8_t packet.cog = cog; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.id = id; + packet.yaw = yaw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_GPS_LEN); #endif @@ -210,7 +228,7 @@ static inline uint16_t mavlink_msg_hil_gps_pack_chan(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_hil_gps_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_gps_t* hil_gps) { - return mavlink_msg_hil_gps_pack(system_id, component_id, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible); + return mavlink_msg_hil_gps_pack(system_id, component_id, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible, hil_gps->id, hil_gps->yaw); } /** @@ -224,7 +242,7 @@ static inline uint16_t mavlink_msg_hil_gps_encode(uint8_t system_id, uint8_t com */ static inline uint16_t mavlink_msg_hil_gps_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_gps_t* hil_gps) { - return mavlink_msg_hil_gps_pack_chan(system_id, component_id, chan, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible); + return mavlink_msg_hil_gps_pack_chan(system_id, component_id, chan, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible, hil_gps->id, hil_gps->yaw); } /** @@ -236,18 +254,20 @@ static inline uint16_t mavlink_msg_hil_gps_encode_chan(uint8_t system_id, uint8_ * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph [cm] GPS HDOP horizontal dilution of position. If unknown, set to: 65535 - * @param epv [cm] GPS VDOP vertical dilution of position. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: 65535 * @param vn [cm/s] GPS velocity in north direction in earth-fixed NED frame * @param ve [cm/s] GPS velocity in east direction in earth-fixed NED frame * @param vd [cm/s] GPS velocity in down direction in earth-fixed NED frame * @param cog [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: 65535 * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param id GPS ID (zero indexed). Used for multiple GPS inputs + * @param yaw [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_hil_gps_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible) +static inline void mavlink_msg_hil_gps_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible, uint8_t id, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIL_GPS_LEN]; @@ -264,6 +284,8 @@ static inline void mavlink_msg_hil_gps_send(mavlink_channel_t chan, uint64_t tim _mav_put_uint16_t(buf, 32, cog); _mav_put_uint8_t(buf, 34, fix_type); _mav_put_uint8_t(buf, 35, satellites_visible); + _mav_put_uint8_t(buf, 36, id); + _mav_put_uint16_t(buf, 37, yaw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_MIN_LEN, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); #else @@ -281,6 +303,8 @@ static inline void mavlink_msg_hil_gps_send(mavlink_channel_t chan, uint64_t tim packet.cog = cog; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.id = id; + packet.yaw = yaw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)&packet, MAVLINK_MSG_ID_HIL_GPS_MIN_LEN, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); #endif @@ -294,7 +318,7 @@ static inline void mavlink_msg_hil_gps_send(mavlink_channel_t chan, uint64_t tim static inline void mavlink_msg_hil_gps_send_struct(mavlink_channel_t chan, const mavlink_hil_gps_t* hil_gps) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_hil_gps_send(chan, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible); + mavlink_msg_hil_gps_send(chan, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible, hil_gps->id, hil_gps->yaw); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)hil_gps, MAVLINK_MSG_ID_HIL_GPS_MIN_LEN, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); #endif @@ -308,7 +332,7 @@ static inline void mavlink_msg_hil_gps_send_struct(mavlink_channel_t chan, const is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_hil_gps_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible) +static inline void mavlink_msg_hil_gps_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible, uint8_t id, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -325,6 +349,8 @@ static inline void mavlink_msg_hil_gps_send_buf(mavlink_message_t *msgbuf, mavli _mav_put_uint16_t(buf, 32, cog); _mav_put_uint8_t(buf, 34, fix_type); _mav_put_uint8_t(buf, 35, satellites_visible); + _mav_put_uint8_t(buf, 36, id); + _mav_put_uint16_t(buf, 37, yaw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_MIN_LEN, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); #else @@ -342,6 +368,8 @@ static inline void mavlink_msg_hil_gps_send_buf(mavlink_message_t *msgbuf, mavli packet->cog = cog; packet->fix_type = fix_type; packet->satellites_visible = satellites_visible; + packet->id = id; + packet->yaw = yaw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)packet, MAVLINK_MSG_ID_HIL_GPS_MIN_LEN, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); #endif @@ -406,7 +434,7 @@ static inline int32_t mavlink_msg_hil_gps_get_alt(const mavlink_message_t* msg) /** * @brief Get field eph from hil_gps message * - * @return [cm] GPS HDOP horizontal dilution of position. If unknown, set to: 65535 + * @return GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_hil_gps_get_eph(const mavlink_message_t* msg) { @@ -416,7 +444,7 @@ static inline uint16_t mavlink_msg_hil_gps_get_eph(const mavlink_message_t* msg) /** * @brief Get field epv from hil_gps message * - * @return [cm] GPS VDOP vertical dilution of position. If unknown, set to: 65535 + * @return GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_hil_gps_get_epv(const mavlink_message_t* msg) { @@ -483,6 +511,26 @@ static inline uint8_t mavlink_msg_hil_gps_get_satellites_visible(const mavlink_m return _MAV_RETURN_uint8_t(msg, 35); } +/** + * @brief Get field id from hil_gps message + * + * @return GPS ID (zero indexed). Used for multiple GPS inputs + */ +static inline uint8_t mavlink_msg_hil_gps_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 36); +} + +/** + * @brief Get field yaw from hil_gps message + * + * @return [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north + */ +static inline uint16_t mavlink_msg_hil_gps_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 37); +} + /** * @brief Decode a hil_gps message into a struct * @@ -505,6 +553,8 @@ static inline void mavlink_msg_hil_gps_decode(const mavlink_message_t* msg, mavl hil_gps->cog = mavlink_msg_hil_gps_get_cog(msg); hil_gps->fix_type = mavlink_msg_hil_gps_get_fix_type(msg); hil_gps->satellites_visible = mavlink_msg_hil_gps_get_satellites_visible(msg); + hil_gps->id = mavlink_msg_hil_gps_get_id(msg); + hil_gps->yaw = mavlink_msg_hil_gps_get_yaw(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_HIL_GPS_LEN? msg->len : MAVLINK_MSG_ID_HIL_GPS_LEN; memset(hil_gps, 0, MAVLINK_MSG_ID_HIL_GPS_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_hil_sensor.h b/lib/main/MAVLink/common/mavlink_msg_hil_sensor.h index e9d03cc70e..007c5a6989 100755 --- a/lib/main/MAVLink/common/mavlink_msg_hil_sensor.h +++ b/lib/main/MAVLink/common/mavlink_msg_hil_sensor.h @@ -15,16 +15,17 @@ typedef struct __mavlink_hil_sensor_t { float xmag; /*< [gauss] X Magnetic field*/ float ymag; /*< [gauss] Y Magnetic field*/ float zmag; /*< [gauss] Z Magnetic field*/ - float abs_pressure; /*< [mbar] Absolute pressure*/ - float diff_pressure; /*< [mbar] Differential pressure (airspeed)*/ + float abs_pressure; /*< [hPa] Absolute pressure*/ + float diff_pressure; /*< [hPa] Differential pressure (airspeed)*/ float pressure_alt; /*< Altitude calculated from pressure*/ float temperature; /*< [degC] Temperature*/ uint32_t fields_updated; /*< Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature, bit 31: full reset of attitude/position/velocities/etc was performed in sim.*/ + uint8_t id; /*< Sensor ID (zero indexed). Used for multiple sensor inputs*/ } mavlink_hil_sensor_t; -#define MAVLINK_MSG_ID_HIL_SENSOR_LEN 64 +#define MAVLINK_MSG_ID_HIL_SENSOR_LEN 65 #define MAVLINK_MSG_ID_HIL_SENSOR_MIN_LEN 64 -#define MAVLINK_MSG_ID_107_LEN 64 +#define MAVLINK_MSG_ID_107_LEN 65 #define MAVLINK_MSG_ID_107_MIN_LEN 64 #define MAVLINK_MSG_ID_HIL_SENSOR_CRC 108 @@ -36,7 +37,7 @@ typedef struct __mavlink_hil_sensor_t { #define MAVLINK_MESSAGE_INFO_HIL_SENSOR { \ 107, \ "HIL_SENSOR", \ - 15, \ + 16, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_sensor_t, time_usec) }, \ { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_sensor_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_sensor_t, yacc) }, \ @@ -52,12 +53,13 @@ typedef struct __mavlink_hil_sensor_t { { "pressure_alt", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_hil_sensor_t, pressure_alt) }, \ { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_hil_sensor_t, temperature) }, \ { "fields_updated", NULL, MAVLINK_TYPE_UINT32_T, 0, 60, offsetof(mavlink_hil_sensor_t, fields_updated) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 64, offsetof(mavlink_hil_sensor_t, id) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_HIL_SENSOR { \ "HIL_SENSOR", \ - 15, \ + 16, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_sensor_t, time_usec) }, \ { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_sensor_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_sensor_t, yacc) }, \ @@ -73,6 +75,7 @@ typedef struct __mavlink_hil_sensor_t { { "pressure_alt", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_hil_sensor_t, pressure_alt) }, \ { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_hil_sensor_t, temperature) }, \ { "fields_updated", NULL, MAVLINK_TYPE_UINT32_T, 0, 60, offsetof(mavlink_hil_sensor_t, fields_updated) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 64, offsetof(mavlink_hil_sensor_t, id) }, \ } \ } #endif @@ -93,15 +96,16 @@ typedef struct __mavlink_hil_sensor_t { * @param xmag [gauss] X Magnetic field * @param ymag [gauss] Y Magnetic field * @param zmag [gauss] Z Magnetic field - * @param abs_pressure [mbar] Absolute pressure - * @param diff_pressure [mbar] Differential pressure (airspeed) + * @param abs_pressure [hPa] Absolute pressure + * @param diff_pressure [hPa] Differential pressure (airspeed) * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature, bit 31: full reset of attitude/position/velocities/etc was performed in sim. + * @param id Sensor ID (zero indexed). Used for multiple sensor inputs * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_hil_sensor_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated) + uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated, uint8_t id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIL_SENSOR_LEN]; @@ -120,6 +124,7 @@ static inline uint16_t mavlink_msg_hil_sensor_pack(uint8_t system_id, uint8_t co _mav_put_float(buf, 52, pressure_alt); _mav_put_float(buf, 56, temperature); _mav_put_uint32_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 64, id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); #else @@ -139,6 +144,7 @@ static inline uint16_t mavlink_msg_hil_sensor_pack(uint8_t system_id, uint8_t co packet.pressure_alt = pressure_alt; packet.temperature = temperature; packet.fields_updated = fields_updated; + packet.id = id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); #endif @@ -163,16 +169,17 @@ static inline uint16_t mavlink_msg_hil_sensor_pack(uint8_t system_id, uint8_t co * @param xmag [gauss] X Magnetic field * @param ymag [gauss] Y Magnetic field * @param zmag [gauss] Z Magnetic field - * @param abs_pressure [mbar] Absolute pressure - * @param diff_pressure [mbar] Differential pressure (airspeed) + * @param abs_pressure [hPa] Absolute pressure + * @param diff_pressure [hPa] Differential pressure (airspeed) * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature, bit 31: full reset of attitude/position/velocities/etc was performed in sim. + * @param id Sensor ID (zero indexed). Used for multiple sensor inputs * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_hil_sensor_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float xmag,float ymag,float zmag,float abs_pressure,float diff_pressure,float pressure_alt,float temperature,uint32_t fields_updated) + uint64_t time_usec,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float xmag,float ymag,float zmag,float abs_pressure,float diff_pressure,float pressure_alt,float temperature,uint32_t fields_updated,uint8_t id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIL_SENSOR_LEN]; @@ -191,6 +198,7 @@ static inline uint16_t mavlink_msg_hil_sensor_pack_chan(uint8_t system_id, uint8 _mav_put_float(buf, 52, pressure_alt); _mav_put_float(buf, 56, temperature); _mav_put_uint32_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 64, id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); #else @@ -210,6 +218,7 @@ static inline uint16_t mavlink_msg_hil_sensor_pack_chan(uint8_t system_id, uint8 packet.pressure_alt = pressure_alt; packet.temperature = temperature; packet.fields_updated = fields_updated; + packet.id = id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); #endif @@ -228,7 +237,7 @@ static inline uint16_t mavlink_msg_hil_sensor_pack_chan(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_hil_sensor_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_sensor_t* hil_sensor) { - return mavlink_msg_hil_sensor_pack(system_id, component_id, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated); + return mavlink_msg_hil_sensor_pack(system_id, component_id, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated, hil_sensor->id); } /** @@ -242,7 +251,7 @@ static inline uint16_t mavlink_msg_hil_sensor_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_hil_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_sensor_t* hil_sensor) { - return mavlink_msg_hil_sensor_pack_chan(system_id, component_id, chan, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated); + return mavlink_msg_hil_sensor_pack_chan(system_id, component_id, chan, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated, hil_sensor->id); } /** @@ -259,15 +268,16 @@ static inline uint16_t mavlink_msg_hil_sensor_encode_chan(uint8_t system_id, uin * @param xmag [gauss] X Magnetic field * @param ymag [gauss] Y Magnetic field * @param zmag [gauss] Z Magnetic field - * @param abs_pressure [mbar] Absolute pressure - * @param diff_pressure [mbar] Differential pressure (airspeed) + * @param abs_pressure [hPa] Absolute pressure + * @param diff_pressure [hPa] Differential pressure (airspeed) * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature, bit 31: full reset of attitude/position/velocities/etc was performed in sim. + * @param id Sensor ID (zero indexed). Used for multiple sensor inputs */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_hil_sensor_send(mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated) +static inline void mavlink_msg_hil_sensor_send(mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated, uint8_t id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIL_SENSOR_LEN]; @@ -286,6 +296,7 @@ static inline void mavlink_msg_hil_sensor_send(mavlink_channel_t chan, uint64_t _mav_put_float(buf, 52, pressure_alt); _mav_put_float(buf, 56, temperature); _mav_put_uint32_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 64, id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); #else @@ -305,6 +316,7 @@ static inline void mavlink_msg_hil_sensor_send(mavlink_channel_t chan, uint64_t packet.pressure_alt = pressure_alt; packet.temperature = temperature; packet.fields_updated = fields_updated; + packet.id = id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_HIL_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); #endif @@ -318,7 +330,7 @@ static inline void mavlink_msg_hil_sensor_send(mavlink_channel_t chan, uint64_t static inline void mavlink_msg_hil_sensor_send_struct(mavlink_channel_t chan, const mavlink_hil_sensor_t* hil_sensor) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_hil_sensor_send(chan, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated); + mavlink_msg_hil_sensor_send(chan, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated, hil_sensor->id); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)hil_sensor, MAVLINK_MSG_ID_HIL_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); #endif @@ -332,7 +344,7 @@ static inline void mavlink_msg_hil_sensor_send_struct(mavlink_channel_t chan, co is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_hil_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated) +static inline void mavlink_msg_hil_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated, uint8_t id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -351,6 +363,7 @@ static inline void mavlink_msg_hil_sensor_send_buf(mavlink_message_t *msgbuf, ma _mav_put_float(buf, 52, pressure_alt); _mav_put_float(buf, 56, temperature); _mav_put_uint32_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 64, id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); #else @@ -370,6 +383,7 @@ static inline void mavlink_msg_hil_sensor_send_buf(mavlink_message_t *msgbuf, ma packet->pressure_alt = pressure_alt; packet->temperature = temperature; packet->fields_updated = fields_updated; + packet->id = id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)packet, MAVLINK_MSG_ID_HIL_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); #endif @@ -484,7 +498,7 @@ static inline float mavlink_msg_hil_sensor_get_zmag(const mavlink_message_t* msg /** * @brief Get field abs_pressure from hil_sensor message * - * @return [mbar] Absolute pressure + * @return [hPa] Absolute pressure */ static inline float mavlink_msg_hil_sensor_get_abs_pressure(const mavlink_message_t* msg) { @@ -494,7 +508,7 @@ static inline float mavlink_msg_hil_sensor_get_abs_pressure(const mavlink_messag /** * @brief Get field diff_pressure from hil_sensor message * - * @return [mbar] Differential pressure (airspeed) + * @return [hPa] Differential pressure (airspeed) */ static inline float mavlink_msg_hil_sensor_get_diff_pressure(const mavlink_message_t* msg) { @@ -531,6 +545,16 @@ static inline uint32_t mavlink_msg_hil_sensor_get_fields_updated(const mavlink_m return _MAV_RETURN_uint32_t(msg, 60); } +/** + * @brief Get field id from hil_sensor message + * + * @return Sensor ID (zero indexed). Used for multiple sensor inputs + */ +static inline uint8_t mavlink_msg_hil_sensor_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 64); +} + /** * @brief Decode a hil_sensor message into a struct * @@ -555,6 +579,7 @@ static inline void mavlink_msg_hil_sensor_decode(const mavlink_message_t* msg, m hil_sensor->pressure_alt = mavlink_msg_hil_sensor_get_pressure_alt(msg); hil_sensor->temperature = mavlink_msg_hil_sensor_get_temperature(msg); hil_sensor->fields_updated = mavlink_msg_hil_sensor_get_fields_updated(msg); + hil_sensor->id = mavlink_msg_hil_sensor_get_id(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_HIL_SENSOR_LEN? msg->len : MAVLINK_MSG_ID_HIL_SENSOR_LEN; memset(hil_sensor, 0, MAVLINK_MSG_ID_HIL_SENSOR_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_home_position.h b/lib/main/MAVLink/common/mavlink_msg_home_position.h index d275d1d630..ab5130b810 100755 --- a/lib/main/MAVLink/common/mavlink_msg_home_position.h +++ b/lib/main/MAVLink/common/mavlink_msg_home_position.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_HOME_POSITION 242 - +MAVPACKED( typedef struct __mavlink_home_position_t { int32_t latitude; /*< [degE7] Latitude (WGS84)*/ int32_t longitude; /*< [degE7] Longitude (WGS84)*/ @@ -15,11 +15,12 @@ typedef struct __mavlink_home_position_t { float approach_x; /*< [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone.*/ float approach_y; /*< [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone.*/ float approach_z; /*< [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone.*/ -} mavlink_home_position_t; + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ +}) mavlink_home_position_t; -#define MAVLINK_MSG_ID_HOME_POSITION_LEN 52 +#define MAVLINK_MSG_ID_HOME_POSITION_LEN 60 #define MAVLINK_MSG_ID_HOME_POSITION_MIN_LEN 52 -#define MAVLINK_MSG_ID_242_LEN 52 +#define MAVLINK_MSG_ID_242_LEN 60 #define MAVLINK_MSG_ID_242_MIN_LEN 52 #define MAVLINK_MSG_ID_HOME_POSITION_CRC 104 @@ -31,7 +32,7 @@ typedef struct __mavlink_home_position_t { #define MAVLINK_MESSAGE_INFO_HOME_POSITION { \ 242, \ "HOME_POSITION", \ - 10, \ + 11, \ { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_home_position_t, latitude) }, \ { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_home_position_t, longitude) }, \ { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_home_position_t, altitude) }, \ @@ -42,12 +43,13 @@ typedef struct __mavlink_home_position_t { { "approach_x", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_home_position_t, approach_x) }, \ { "approach_y", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_home_position_t, approach_y) }, \ { "approach_z", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_home_position_t, approach_z) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 52, offsetof(mavlink_home_position_t, time_usec) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_HOME_POSITION { \ "HOME_POSITION", \ - 10, \ + 11, \ { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_home_position_t, latitude) }, \ { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_home_position_t, longitude) }, \ { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_home_position_t, altitude) }, \ @@ -58,6 +60,7 @@ typedef struct __mavlink_home_position_t { { "approach_x", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_home_position_t, approach_x) }, \ { "approach_y", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_home_position_t, approach_y) }, \ { "approach_z", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_home_position_t, approach_z) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 52, offsetof(mavlink_home_position_t, time_usec) }, \ } \ } #endif @@ -78,10 +81,11 @@ typedef struct __mavlink_home_position_t { * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_z [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_home_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z) + int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HOME_POSITION_LEN]; @@ -94,6 +98,7 @@ static inline uint16_t mavlink_msg_home_position_pack(uint8_t system_id, uint8_t _mav_put_float(buf, 40, approach_x); _mav_put_float(buf, 44, approach_y); _mav_put_float(buf, 48, approach_z); + _mav_put_uint64_t(buf, 52, time_usec); _mav_put_float_array(buf, 24, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HOME_POSITION_LEN); #else @@ -107,6 +112,7 @@ static inline uint16_t mavlink_msg_home_position_pack(uint8_t system_id, uint8_t packet.approach_x = approach_x; packet.approach_y = approach_y; packet.approach_z = approach_z; + packet.time_usec = time_usec; mav_array_memcpy(packet.q, q, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HOME_POSITION_LEN); #endif @@ -131,11 +137,12 @@ static inline uint16_t mavlink_msg_home_position_pack(uint8_t system_id, uint8_t * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_z [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_home_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - int32_t latitude,int32_t longitude,int32_t altitude,float x,float y,float z,const float *q,float approach_x,float approach_y,float approach_z) + int32_t latitude,int32_t longitude,int32_t altitude,float x,float y,float z,const float *q,float approach_x,float approach_y,float approach_z,uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HOME_POSITION_LEN]; @@ -148,6 +155,7 @@ static inline uint16_t mavlink_msg_home_position_pack_chan(uint8_t system_id, ui _mav_put_float(buf, 40, approach_x); _mav_put_float(buf, 44, approach_y); _mav_put_float(buf, 48, approach_z); + _mav_put_uint64_t(buf, 52, time_usec); _mav_put_float_array(buf, 24, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HOME_POSITION_LEN); #else @@ -161,6 +169,7 @@ static inline uint16_t mavlink_msg_home_position_pack_chan(uint8_t system_id, ui packet.approach_x = approach_x; packet.approach_y = approach_y; packet.approach_z = approach_z; + packet.time_usec = time_usec; mav_array_memcpy(packet.q, q, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HOME_POSITION_LEN); #endif @@ -179,7 +188,7 @@ static inline uint16_t mavlink_msg_home_position_pack_chan(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_home_position_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_home_position_t* home_position) { - return mavlink_msg_home_position_pack(system_id, component_id, msg, home_position->latitude, home_position->longitude, home_position->altitude, home_position->x, home_position->y, home_position->z, home_position->q, home_position->approach_x, home_position->approach_y, home_position->approach_z); + return mavlink_msg_home_position_pack(system_id, component_id, msg, home_position->latitude, home_position->longitude, home_position->altitude, home_position->x, home_position->y, home_position->z, home_position->q, home_position->approach_x, home_position->approach_y, home_position->approach_z, home_position->time_usec); } /** @@ -193,7 +202,7 @@ static inline uint16_t mavlink_msg_home_position_encode(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_home_position_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_home_position_t* home_position) { - return mavlink_msg_home_position_pack_chan(system_id, component_id, chan, msg, home_position->latitude, home_position->longitude, home_position->altitude, home_position->x, home_position->y, home_position->z, home_position->q, home_position->approach_x, home_position->approach_y, home_position->approach_z); + return mavlink_msg_home_position_pack_chan(system_id, component_id, chan, msg, home_position->latitude, home_position->longitude, home_position->altitude, home_position->x, home_position->y, home_position->z, home_position->q, home_position->approach_x, home_position->approach_y, home_position->approach_z, home_position->time_usec); } /** @@ -210,10 +219,11 @@ static inline uint16_t mavlink_msg_home_position_encode_chan(uint8_t system_id, * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_z [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_home_position_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z) +static inline void mavlink_msg_home_position_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HOME_POSITION_LEN]; @@ -226,6 +236,7 @@ static inline void mavlink_msg_home_position_send(mavlink_channel_t chan, int32_ _mav_put_float(buf, 40, approach_x); _mav_put_float(buf, 44, approach_y); _mav_put_float(buf, 48, approach_z); + _mav_put_uint64_t(buf, 52, time_usec); _mav_put_float_array(buf, 24, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HOME_POSITION, buf, MAVLINK_MSG_ID_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_HOME_POSITION_LEN, MAVLINK_MSG_ID_HOME_POSITION_CRC); #else @@ -239,6 +250,7 @@ static inline void mavlink_msg_home_position_send(mavlink_channel_t chan, int32_ packet.approach_x = approach_x; packet.approach_y = approach_y; packet.approach_z = approach_z; + packet.time_usec = time_usec; mav_array_memcpy(packet.q, q, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HOME_POSITION, (const char *)&packet, MAVLINK_MSG_ID_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_HOME_POSITION_LEN, MAVLINK_MSG_ID_HOME_POSITION_CRC); #endif @@ -252,7 +264,7 @@ static inline void mavlink_msg_home_position_send(mavlink_channel_t chan, int32_ static inline void mavlink_msg_home_position_send_struct(mavlink_channel_t chan, const mavlink_home_position_t* home_position) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_home_position_send(chan, home_position->latitude, home_position->longitude, home_position->altitude, home_position->x, home_position->y, home_position->z, home_position->q, home_position->approach_x, home_position->approach_y, home_position->approach_z); + mavlink_msg_home_position_send(chan, home_position->latitude, home_position->longitude, home_position->altitude, home_position->x, home_position->y, home_position->z, home_position->q, home_position->approach_x, home_position->approach_y, home_position->approach_z, home_position->time_usec); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HOME_POSITION, (const char *)home_position, MAVLINK_MSG_ID_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_HOME_POSITION_LEN, MAVLINK_MSG_ID_HOME_POSITION_CRC); #endif @@ -266,7 +278,7 @@ static inline void mavlink_msg_home_position_send_struct(mavlink_channel_t chan, is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_home_position_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z) +static inline void mavlink_msg_home_position_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -279,6 +291,7 @@ static inline void mavlink_msg_home_position_send_buf(mavlink_message_t *msgbuf, _mav_put_float(buf, 40, approach_x); _mav_put_float(buf, 44, approach_y); _mav_put_float(buf, 48, approach_z); + _mav_put_uint64_t(buf, 52, time_usec); _mav_put_float_array(buf, 24, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HOME_POSITION, buf, MAVLINK_MSG_ID_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_HOME_POSITION_LEN, MAVLINK_MSG_ID_HOME_POSITION_CRC); #else @@ -292,6 +305,7 @@ static inline void mavlink_msg_home_position_send_buf(mavlink_message_t *msgbuf, packet->approach_x = approach_x; packet->approach_y = approach_y; packet->approach_z = approach_z; + packet->time_usec = time_usec; mav_array_memcpy(packet->q, q, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HOME_POSITION, (const char *)packet, MAVLINK_MSG_ID_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_HOME_POSITION_LEN, MAVLINK_MSG_ID_HOME_POSITION_CRC); #endif @@ -403,6 +417,16 @@ static inline float mavlink_msg_home_position_get_approach_z(const mavlink_messa return _MAV_RETURN_float(msg, 48); } +/** + * @brief Get field time_usec from home_position message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_home_position_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 52); +} + /** * @brief Decode a home_position message into a struct * @@ -422,6 +446,7 @@ static inline void mavlink_msg_home_position_decode(const mavlink_message_t* msg home_position->approach_x = mavlink_msg_home_position_get_approach_x(msg); home_position->approach_y = mavlink_msg_home_position_get_approach_y(msg); home_position->approach_z = mavlink_msg_home_position_get_approach_z(msg); + home_position->time_usec = mavlink_msg_home_position_get_time_usec(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_HOME_POSITION_LEN? msg->len : MAVLINK_MSG_ID_HOME_POSITION_LEN; memset(home_position, 0, MAVLINK_MSG_ID_HOME_POSITION_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_isbd_link_status.h b/lib/main/MAVLink/common/mavlink_msg_isbd_link_status.h new file mode 100644 index 0000000000..a0c53745f9 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_isbd_link_status.h @@ -0,0 +1,388 @@ +#pragma once +// MESSAGE ISBD_LINK_STATUS PACKING + +#define MAVLINK_MSG_ID_ISBD_LINK_STATUS 335 + + +typedef struct __mavlink_isbd_link_status_t { + uint64_t timestamp; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + uint64_t last_heartbeat; /*< [us] Timestamp of the last successful sbd session. The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + uint16_t failed_sessions; /*< Number of failed SBD sessions.*/ + uint16_t successful_sessions; /*< Number of successful SBD sessions.*/ + uint8_t signal_quality; /*< Signal quality equal to the number of bars displayed on the ISU signal strength indicator. Range is 0 to 5, where 0 indicates no signal and 5 indicates maximum signal strength.*/ + uint8_t ring_pending; /*< 1: Ring call pending, 0: No call pending.*/ + uint8_t tx_session_pending; /*< 1: Transmission session pending, 0: No transmission session pending.*/ + uint8_t rx_session_pending; /*< 1: Receiving session pending, 0: No receiving session pending.*/ +} mavlink_isbd_link_status_t; + +#define MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN 24 +#define MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN 24 +#define MAVLINK_MSG_ID_335_LEN 24 +#define MAVLINK_MSG_ID_335_MIN_LEN 24 + +#define MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC 225 +#define MAVLINK_MSG_ID_335_CRC 225 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_ISBD_LINK_STATUS { \ + 335, \ + "ISBD_LINK_STATUS", \ + 8, \ + { { "timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_isbd_link_status_t, timestamp) }, \ + { "last_heartbeat", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_isbd_link_status_t, last_heartbeat) }, \ + { "failed_sessions", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_isbd_link_status_t, failed_sessions) }, \ + { "successful_sessions", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_isbd_link_status_t, successful_sessions) }, \ + { "signal_quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_isbd_link_status_t, signal_quality) }, \ + { "ring_pending", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_isbd_link_status_t, ring_pending) }, \ + { "tx_session_pending", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_isbd_link_status_t, tx_session_pending) }, \ + { "rx_session_pending", NULL, MAVLINK_TYPE_UINT8_T, 0, 23, offsetof(mavlink_isbd_link_status_t, rx_session_pending) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_ISBD_LINK_STATUS { \ + "ISBD_LINK_STATUS", \ + 8, \ + { { "timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_isbd_link_status_t, timestamp) }, \ + { "last_heartbeat", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_isbd_link_status_t, last_heartbeat) }, \ + { "failed_sessions", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_isbd_link_status_t, failed_sessions) }, \ + { "successful_sessions", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_isbd_link_status_t, successful_sessions) }, \ + { "signal_quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_isbd_link_status_t, signal_quality) }, \ + { "ring_pending", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_isbd_link_status_t, ring_pending) }, \ + { "tx_session_pending", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_isbd_link_status_t, tx_session_pending) }, \ + { "rx_session_pending", NULL, MAVLINK_TYPE_UINT8_T, 0, 23, offsetof(mavlink_isbd_link_status_t, rx_session_pending) }, \ + } \ +} +#endif + +/** + * @brief Pack a isbd_link_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param timestamp [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param last_heartbeat [us] Timestamp of the last successful sbd session. The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param failed_sessions Number of failed SBD sessions. + * @param successful_sessions Number of successful SBD sessions. + * @param signal_quality Signal quality equal to the number of bars displayed on the ISU signal strength indicator. Range is 0 to 5, where 0 indicates no signal and 5 indicates maximum signal strength. + * @param ring_pending 1: Ring call pending, 0: No call pending. + * @param tx_session_pending 1: Transmission session pending, 0: No transmission session pending. + * @param rx_session_pending 1: Receiving session pending, 0: No receiving session pending. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_isbd_link_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t timestamp, uint64_t last_heartbeat, uint16_t failed_sessions, uint16_t successful_sessions, uint8_t signal_quality, uint8_t ring_pending, uint8_t tx_session_pending, uint8_t rx_session_pending) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, timestamp); + _mav_put_uint64_t(buf, 8, last_heartbeat); + _mav_put_uint16_t(buf, 16, failed_sessions); + _mav_put_uint16_t(buf, 18, successful_sessions); + _mav_put_uint8_t(buf, 20, signal_quality); + _mav_put_uint8_t(buf, 21, ring_pending); + _mav_put_uint8_t(buf, 22, tx_session_pending); + _mav_put_uint8_t(buf, 23, rx_session_pending); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN); +#else + mavlink_isbd_link_status_t packet; + packet.timestamp = timestamp; + packet.last_heartbeat = last_heartbeat; + packet.failed_sessions = failed_sessions; + packet.successful_sessions = successful_sessions; + packet.signal_quality = signal_quality; + packet.ring_pending = ring_pending; + packet.tx_session_pending = tx_session_pending; + packet.rx_session_pending = rx_session_pending; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ISBD_LINK_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC); +} + +/** + * @brief Pack a isbd_link_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param timestamp [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param last_heartbeat [us] Timestamp of the last successful sbd session. The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param failed_sessions Number of failed SBD sessions. + * @param successful_sessions Number of successful SBD sessions. + * @param signal_quality Signal quality equal to the number of bars displayed on the ISU signal strength indicator. Range is 0 to 5, where 0 indicates no signal and 5 indicates maximum signal strength. + * @param ring_pending 1: Ring call pending, 0: No call pending. + * @param tx_session_pending 1: Transmission session pending, 0: No transmission session pending. + * @param rx_session_pending 1: Receiving session pending, 0: No receiving session pending. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_isbd_link_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t timestamp,uint64_t last_heartbeat,uint16_t failed_sessions,uint16_t successful_sessions,uint8_t signal_quality,uint8_t ring_pending,uint8_t tx_session_pending,uint8_t rx_session_pending) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, timestamp); + _mav_put_uint64_t(buf, 8, last_heartbeat); + _mav_put_uint16_t(buf, 16, failed_sessions); + _mav_put_uint16_t(buf, 18, successful_sessions); + _mav_put_uint8_t(buf, 20, signal_quality); + _mav_put_uint8_t(buf, 21, ring_pending); + _mav_put_uint8_t(buf, 22, tx_session_pending); + _mav_put_uint8_t(buf, 23, rx_session_pending); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN); +#else + mavlink_isbd_link_status_t packet; + packet.timestamp = timestamp; + packet.last_heartbeat = last_heartbeat; + packet.failed_sessions = failed_sessions; + packet.successful_sessions = successful_sessions; + packet.signal_quality = signal_quality; + packet.ring_pending = ring_pending; + packet.tx_session_pending = tx_session_pending; + packet.rx_session_pending = rx_session_pending; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ISBD_LINK_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC); +} + +/** + * @brief Encode a isbd_link_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param isbd_link_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_isbd_link_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_isbd_link_status_t* isbd_link_status) +{ + return mavlink_msg_isbd_link_status_pack(system_id, component_id, msg, isbd_link_status->timestamp, isbd_link_status->last_heartbeat, isbd_link_status->failed_sessions, isbd_link_status->successful_sessions, isbd_link_status->signal_quality, isbd_link_status->ring_pending, isbd_link_status->tx_session_pending, isbd_link_status->rx_session_pending); +} + +/** + * @brief Encode a isbd_link_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param isbd_link_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_isbd_link_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_isbd_link_status_t* isbd_link_status) +{ + return mavlink_msg_isbd_link_status_pack_chan(system_id, component_id, chan, msg, isbd_link_status->timestamp, isbd_link_status->last_heartbeat, isbd_link_status->failed_sessions, isbd_link_status->successful_sessions, isbd_link_status->signal_quality, isbd_link_status->ring_pending, isbd_link_status->tx_session_pending, isbd_link_status->rx_session_pending); +} + +/** + * @brief Send a isbd_link_status message + * @param chan MAVLink channel to send the message + * + * @param timestamp [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param last_heartbeat [us] Timestamp of the last successful sbd session. The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param failed_sessions Number of failed SBD sessions. + * @param successful_sessions Number of successful SBD sessions. + * @param signal_quality Signal quality equal to the number of bars displayed on the ISU signal strength indicator. Range is 0 to 5, where 0 indicates no signal and 5 indicates maximum signal strength. + * @param ring_pending 1: Ring call pending, 0: No call pending. + * @param tx_session_pending 1: Transmission session pending, 0: No transmission session pending. + * @param rx_session_pending 1: Receiving session pending, 0: No receiving session pending. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_isbd_link_status_send(mavlink_channel_t chan, uint64_t timestamp, uint64_t last_heartbeat, uint16_t failed_sessions, uint16_t successful_sessions, uint8_t signal_quality, uint8_t ring_pending, uint8_t tx_session_pending, uint8_t rx_session_pending) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, timestamp); + _mav_put_uint64_t(buf, 8, last_heartbeat); + _mav_put_uint16_t(buf, 16, failed_sessions); + _mav_put_uint16_t(buf, 18, successful_sessions); + _mav_put_uint8_t(buf, 20, signal_quality); + _mav_put_uint8_t(buf, 21, ring_pending); + _mav_put_uint8_t(buf, 22, tx_session_pending); + _mav_put_uint8_t(buf, 23, rx_session_pending); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ISBD_LINK_STATUS, buf, MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC); +#else + mavlink_isbd_link_status_t packet; + packet.timestamp = timestamp; + packet.last_heartbeat = last_heartbeat; + packet.failed_sessions = failed_sessions; + packet.successful_sessions = successful_sessions; + packet.signal_quality = signal_quality; + packet.ring_pending = ring_pending; + packet.tx_session_pending = tx_session_pending; + packet.rx_session_pending = rx_session_pending; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ISBD_LINK_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC); +#endif +} + +/** + * @brief Send a isbd_link_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_isbd_link_status_send_struct(mavlink_channel_t chan, const mavlink_isbd_link_status_t* isbd_link_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_isbd_link_status_send(chan, isbd_link_status->timestamp, isbd_link_status->last_heartbeat, isbd_link_status->failed_sessions, isbd_link_status->successful_sessions, isbd_link_status->signal_quality, isbd_link_status->ring_pending, isbd_link_status->tx_session_pending, isbd_link_status->rx_session_pending); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ISBD_LINK_STATUS, (const char *)isbd_link_status, MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_isbd_link_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t timestamp, uint64_t last_heartbeat, uint16_t failed_sessions, uint16_t successful_sessions, uint8_t signal_quality, uint8_t ring_pending, uint8_t tx_session_pending, uint8_t rx_session_pending) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, timestamp); + _mav_put_uint64_t(buf, 8, last_heartbeat); + _mav_put_uint16_t(buf, 16, failed_sessions); + _mav_put_uint16_t(buf, 18, successful_sessions); + _mav_put_uint8_t(buf, 20, signal_quality); + _mav_put_uint8_t(buf, 21, ring_pending); + _mav_put_uint8_t(buf, 22, tx_session_pending); + _mav_put_uint8_t(buf, 23, rx_session_pending); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ISBD_LINK_STATUS, buf, MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC); +#else + mavlink_isbd_link_status_t *packet = (mavlink_isbd_link_status_t *)msgbuf; + packet->timestamp = timestamp; + packet->last_heartbeat = last_heartbeat; + packet->failed_sessions = failed_sessions; + packet->successful_sessions = successful_sessions; + packet->signal_quality = signal_quality; + packet->ring_pending = ring_pending; + packet->tx_session_pending = tx_session_pending; + packet->rx_session_pending = rx_session_pending; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ISBD_LINK_STATUS, (const char *)packet, MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE ISBD_LINK_STATUS UNPACKING + + +/** + * @brief Get field timestamp from isbd_link_status message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_isbd_link_status_get_timestamp(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field last_heartbeat from isbd_link_status message + * + * @return [us] Timestamp of the last successful sbd session. The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_isbd_link_status_get_last_heartbeat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 8); +} + +/** + * @brief Get field failed_sessions from isbd_link_status message + * + * @return Number of failed SBD sessions. + */ +static inline uint16_t mavlink_msg_isbd_link_status_get_failed_sessions(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field successful_sessions from isbd_link_status message + * + * @return Number of successful SBD sessions. + */ +static inline uint16_t mavlink_msg_isbd_link_status_get_successful_sessions(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field signal_quality from isbd_link_status message + * + * @return Signal quality equal to the number of bars displayed on the ISU signal strength indicator. Range is 0 to 5, where 0 indicates no signal and 5 indicates maximum signal strength. + */ +static inline uint8_t mavlink_msg_isbd_link_status_get_signal_quality(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field ring_pending from isbd_link_status message + * + * @return 1: Ring call pending, 0: No call pending. + */ +static inline uint8_t mavlink_msg_isbd_link_status_get_ring_pending(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 21); +} + +/** + * @brief Get field tx_session_pending from isbd_link_status message + * + * @return 1: Transmission session pending, 0: No transmission session pending. + */ +static inline uint8_t mavlink_msg_isbd_link_status_get_tx_session_pending(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 22); +} + +/** + * @brief Get field rx_session_pending from isbd_link_status message + * + * @return 1: Receiving session pending, 0: No receiving session pending. + */ +static inline uint8_t mavlink_msg_isbd_link_status_get_rx_session_pending(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 23); +} + +/** + * @brief Decode a isbd_link_status message into a struct + * + * @param msg The message to decode + * @param isbd_link_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_isbd_link_status_decode(const mavlink_message_t* msg, mavlink_isbd_link_status_t* isbd_link_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + isbd_link_status->timestamp = mavlink_msg_isbd_link_status_get_timestamp(msg); + isbd_link_status->last_heartbeat = mavlink_msg_isbd_link_status_get_last_heartbeat(msg); + isbd_link_status->failed_sessions = mavlink_msg_isbd_link_status_get_failed_sessions(msg); + isbd_link_status->successful_sessions = mavlink_msg_isbd_link_status_get_successful_sessions(msg); + isbd_link_status->signal_quality = mavlink_msg_isbd_link_status_get_signal_quality(msg); + isbd_link_status->ring_pending = mavlink_msg_isbd_link_status_get_ring_pending(msg); + isbd_link_status->tx_session_pending = mavlink_msg_isbd_link_status_get_tx_session_pending(msg); + isbd_link_status->rx_session_pending = mavlink_msg_isbd_link_status_get_rx_session_pending(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN? msg->len : MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN; + memset(isbd_link_status, 0, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN); + memcpy(isbd_link_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_landing_target.h b/lib/main/MAVLink/common/mavlink_msg_landing_target.h index b591cb3cfe..1d1b41ed8e 100755 --- a/lib/main/MAVLink/common/mavlink_msg_landing_target.h +++ b/lib/main/MAVLink/common/mavlink_msg_landing_target.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_LANDING_TARGET 149 - +MAVPACKED( typedef struct __mavlink_landing_target_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ float angle_x; /*< [rad] X-axis angular offset of the target from the center of the image*/ @@ -13,23 +13,29 @@ typedef struct __mavlink_landing_target_t { float size_y; /*< [rad] Size of target along y-axis*/ uint8_t target_num; /*< The ID of the target if multiple targets are present*/ uint8_t frame; /*< Coordinate frame used for following fields.*/ -} mavlink_landing_target_t; + float x; /*< [m] X Position of the landing target in MAV_FRAME*/ + float y; /*< [m] Y Position of the landing target in MAV_FRAME*/ + float z; /*< [m] Z Position of the landing target in MAV_FRAME*/ + float q[4]; /*< Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0)*/ + uint8_t type; /*< Type of landing target*/ + uint8_t position_valid; /*< Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid).*/ +}) mavlink_landing_target_t; -#define MAVLINK_MSG_ID_LANDING_TARGET_LEN 30 +#define MAVLINK_MSG_ID_LANDING_TARGET_LEN 60 #define MAVLINK_MSG_ID_LANDING_TARGET_MIN_LEN 30 -#define MAVLINK_MSG_ID_149_LEN 30 +#define MAVLINK_MSG_ID_149_LEN 60 #define MAVLINK_MSG_ID_149_MIN_LEN 30 #define MAVLINK_MSG_ID_LANDING_TARGET_CRC 200 #define MAVLINK_MSG_ID_149_CRC 200 - +#define MAVLINK_MSG_LANDING_TARGET_FIELD_Q_LEN 4 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_LANDING_TARGET { \ 149, \ "LANDING_TARGET", \ - 8, \ + 14, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_landing_target_t, time_usec) }, \ { "target_num", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_landing_target_t, target_num) }, \ { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_landing_target_t, frame) }, \ @@ -38,12 +44,18 @@ typedef struct __mavlink_landing_target_t { { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_landing_target_t, distance) }, \ { "size_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_landing_target_t, size_x) }, \ { "size_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_landing_target_t, size_y) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 30, offsetof(mavlink_landing_target_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 34, offsetof(mavlink_landing_target_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 38, offsetof(mavlink_landing_target_t, z) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 42, offsetof(mavlink_landing_target_t, q) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 58, offsetof(mavlink_landing_target_t, type) }, \ + { "position_valid", NULL, MAVLINK_TYPE_UINT8_T, 0, 59, offsetof(mavlink_landing_target_t, position_valid) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_LANDING_TARGET { \ "LANDING_TARGET", \ - 8, \ + 14, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_landing_target_t, time_usec) }, \ { "target_num", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_landing_target_t, target_num) }, \ { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_landing_target_t, frame) }, \ @@ -52,6 +64,12 @@ typedef struct __mavlink_landing_target_t { { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_landing_target_t, distance) }, \ { "size_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_landing_target_t, size_x) }, \ { "size_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_landing_target_t, size_y) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 30, offsetof(mavlink_landing_target_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 34, offsetof(mavlink_landing_target_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 38, offsetof(mavlink_landing_target_t, z) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 42, offsetof(mavlink_landing_target_t, q) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 58, offsetof(mavlink_landing_target_t, type) }, \ + { "position_valid", NULL, MAVLINK_TYPE_UINT8_T, 0, 59, offsetof(mavlink_landing_target_t, position_valid) }, \ } \ } #endif @@ -70,10 +88,16 @@ typedef struct __mavlink_landing_target_t { * @param distance [m] Distance to the target from the vehicle * @param size_x [rad] Size of target along x-axis * @param size_y [rad] Size of target along y-axis + * @param x [m] X Position of the landing target in MAV_FRAME + * @param y [m] Y Position of the landing target in MAV_FRAME + * @param z [m] Z Position of the landing target in MAV_FRAME + * @param q Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param type Type of landing target + * @param position_valid Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_landing_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t target_num, uint8_t frame, float angle_x, float angle_y, float distance, float size_x, float size_y) + uint64_t time_usec, uint8_t target_num, uint8_t frame, float angle_x, float angle_y, float distance, float size_x, float size_y, float x, float y, float z, const float *q, uint8_t type, uint8_t position_valid) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_LANDING_TARGET_LEN]; @@ -85,7 +109,12 @@ static inline uint16_t mavlink_msg_landing_target_pack(uint8_t system_id, uint8_ _mav_put_float(buf, 24, size_y); _mav_put_uint8_t(buf, 28, target_num); _mav_put_uint8_t(buf, 29, frame); - + _mav_put_float(buf, 30, x); + _mav_put_float(buf, 34, y); + _mav_put_float(buf, 38, z); + _mav_put_uint8_t(buf, 58, type); + _mav_put_uint8_t(buf, 59, position_valid); + _mav_put_float_array(buf, 42, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LANDING_TARGET_LEN); #else mavlink_landing_target_t packet; @@ -97,7 +126,12 @@ static inline uint16_t mavlink_msg_landing_target_pack(uint8_t system_id, uint8_ packet.size_y = size_y; packet.target_num = target_num; packet.frame = frame; - + packet.x = x; + packet.y = y; + packet.z = z; + packet.type = type; + packet.position_valid = position_valid; + mav_array_memcpy(packet.q, q, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LANDING_TARGET_LEN); #endif @@ -119,11 +153,17 @@ static inline uint16_t mavlink_msg_landing_target_pack(uint8_t system_id, uint8_ * @param distance [m] Distance to the target from the vehicle * @param size_x [rad] Size of target along x-axis * @param size_y [rad] Size of target along y-axis + * @param x [m] X Position of the landing target in MAV_FRAME + * @param y [m] Y Position of the landing target in MAV_FRAME + * @param z [m] Z Position of the landing target in MAV_FRAME + * @param q Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param type Type of landing target + * @param position_valid Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_landing_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,uint8_t target_num,uint8_t frame,float angle_x,float angle_y,float distance,float size_x,float size_y) + uint64_t time_usec,uint8_t target_num,uint8_t frame,float angle_x,float angle_y,float distance,float size_x,float size_y,float x,float y,float z,const float *q,uint8_t type,uint8_t position_valid) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_LANDING_TARGET_LEN]; @@ -135,7 +175,12 @@ static inline uint16_t mavlink_msg_landing_target_pack_chan(uint8_t system_id, u _mav_put_float(buf, 24, size_y); _mav_put_uint8_t(buf, 28, target_num); _mav_put_uint8_t(buf, 29, frame); - + _mav_put_float(buf, 30, x); + _mav_put_float(buf, 34, y); + _mav_put_float(buf, 38, z); + _mav_put_uint8_t(buf, 58, type); + _mav_put_uint8_t(buf, 59, position_valid); + _mav_put_float_array(buf, 42, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LANDING_TARGET_LEN); #else mavlink_landing_target_t packet; @@ -147,7 +192,12 @@ static inline uint16_t mavlink_msg_landing_target_pack_chan(uint8_t system_id, u packet.size_y = size_y; packet.target_num = target_num; packet.frame = frame; - + packet.x = x; + packet.y = y; + packet.z = z; + packet.type = type; + packet.position_valid = position_valid; + mav_array_memcpy(packet.q, q, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LANDING_TARGET_LEN); #endif @@ -165,7 +215,7 @@ static inline uint16_t mavlink_msg_landing_target_pack_chan(uint8_t system_id, u */ static inline uint16_t mavlink_msg_landing_target_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_landing_target_t* landing_target) { - return mavlink_msg_landing_target_pack(system_id, component_id, msg, landing_target->time_usec, landing_target->target_num, landing_target->frame, landing_target->angle_x, landing_target->angle_y, landing_target->distance, landing_target->size_x, landing_target->size_y); + return mavlink_msg_landing_target_pack(system_id, component_id, msg, landing_target->time_usec, landing_target->target_num, landing_target->frame, landing_target->angle_x, landing_target->angle_y, landing_target->distance, landing_target->size_x, landing_target->size_y, landing_target->x, landing_target->y, landing_target->z, landing_target->q, landing_target->type, landing_target->position_valid); } /** @@ -179,7 +229,7 @@ static inline uint16_t mavlink_msg_landing_target_encode(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_landing_target_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_landing_target_t* landing_target) { - return mavlink_msg_landing_target_pack_chan(system_id, component_id, chan, msg, landing_target->time_usec, landing_target->target_num, landing_target->frame, landing_target->angle_x, landing_target->angle_y, landing_target->distance, landing_target->size_x, landing_target->size_y); + return mavlink_msg_landing_target_pack_chan(system_id, component_id, chan, msg, landing_target->time_usec, landing_target->target_num, landing_target->frame, landing_target->angle_x, landing_target->angle_y, landing_target->distance, landing_target->size_x, landing_target->size_y, landing_target->x, landing_target->y, landing_target->z, landing_target->q, landing_target->type, landing_target->position_valid); } /** @@ -194,10 +244,16 @@ static inline uint16_t mavlink_msg_landing_target_encode_chan(uint8_t system_id, * @param distance [m] Distance to the target from the vehicle * @param size_x [rad] Size of target along x-axis * @param size_y [rad] Size of target along y-axis + * @param x [m] X Position of the landing target in MAV_FRAME + * @param y [m] Y Position of the landing target in MAV_FRAME + * @param z [m] Z Position of the landing target in MAV_FRAME + * @param q Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param type Type of landing target + * @param position_valid Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_landing_target_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t target_num, uint8_t frame, float angle_x, float angle_y, float distance, float size_x, float size_y) +static inline void mavlink_msg_landing_target_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t target_num, uint8_t frame, float angle_x, float angle_y, float distance, float size_x, float size_y, float x, float y, float z, const float *q, uint8_t type, uint8_t position_valid) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_LANDING_TARGET_LEN]; @@ -209,7 +265,12 @@ static inline void mavlink_msg_landing_target_send(mavlink_channel_t chan, uint6 _mav_put_float(buf, 24, size_y); _mav_put_uint8_t(buf, 28, target_num); _mav_put_uint8_t(buf, 29, frame); - + _mav_put_float(buf, 30, x); + _mav_put_float(buf, 34, y); + _mav_put_float(buf, 38, z); + _mav_put_uint8_t(buf, 58, type); + _mav_put_uint8_t(buf, 59, position_valid); + _mav_put_float_array(buf, 42, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, buf, MAVLINK_MSG_ID_LANDING_TARGET_MIN_LEN, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); #else mavlink_landing_target_t packet; @@ -221,7 +282,12 @@ static inline void mavlink_msg_landing_target_send(mavlink_channel_t chan, uint6 packet.size_y = size_y; packet.target_num = target_num; packet.frame = frame; - + packet.x = x; + packet.y = y; + packet.z = z; + packet.type = type; + packet.position_valid = position_valid; + mav_array_memcpy(packet.q, q, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, (const char *)&packet, MAVLINK_MSG_ID_LANDING_TARGET_MIN_LEN, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); #endif } @@ -234,7 +300,7 @@ static inline void mavlink_msg_landing_target_send(mavlink_channel_t chan, uint6 static inline void mavlink_msg_landing_target_send_struct(mavlink_channel_t chan, const mavlink_landing_target_t* landing_target) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_landing_target_send(chan, landing_target->time_usec, landing_target->target_num, landing_target->frame, landing_target->angle_x, landing_target->angle_y, landing_target->distance, landing_target->size_x, landing_target->size_y); + mavlink_msg_landing_target_send(chan, landing_target->time_usec, landing_target->target_num, landing_target->frame, landing_target->angle_x, landing_target->angle_y, landing_target->distance, landing_target->size_x, landing_target->size_y, landing_target->x, landing_target->y, landing_target->z, landing_target->q, landing_target->type, landing_target->position_valid); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, (const char *)landing_target, MAVLINK_MSG_ID_LANDING_TARGET_MIN_LEN, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); #endif @@ -248,7 +314,7 @@ static inline void mavlink_msg_landing_target_send_struct(mavlink_channel_t chan is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_landing_target_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t target_num, uint8_t frame, float angle_x, float angle_y, float distance, float size_x, float size_y) +static inline void mavlink_msg_landing_target_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t target_num, uint8_t frame, float angle_x, float angle_y, float distance, float size_x, float size_y, float x, float y, float z, const float *q, uint8_t type, uint8_t position_valid) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -260,7 +326,12 @@ static inline void mavlink_msg_landing_target_send_buf(mavlink_message_t *msgbuf _mav_put_float(buf, 24, size_y); _mav_put_uint8_t(buf, 28, target_num); _mav_put_uint8_t(buf, 29, frame); - + _mav_put_float(buf, 30, x); + _mav_put_float(buf, 34, y); + _mav_put_float(buf, 38, z); + _mav_put_uint8_t(buf, 58, type); + _mav_put_uint8_t(buf, 59, position_valid); + _mav_put_float_array(buf, 42, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, buf, MAVLINK_MSG_ID_LANDING_TARGET_MIN_LEN, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); #else mavlink_landing_target_t *packet = (mavlink_landing_target_t *)msgbuf; @@ -272,7 +343,12 @@ static inline void mavlink_msg_landing_target_send_buf(mavlink_message_t *msgbuf packet->size_y = size_y; packet->target_num = target_num; packet->frame = frame; - + packet->x = x; + packet->y = y; + packet->z = z; + packet->type = type; + packet->position_valid = position_valid; + mav_array_memcpy(packet->q, q, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, (const char *)packet, MAVLINK_MSG_ID_LANDING_TARGET_MIN_LEN, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); #endif } @@ -363,6 +439,66 @@ static inline float mavlink_msg_landing_target_get_size_y(const mavlink_message_ return _MAV_RETURN_float(msg, 24); } +/** + * @brief Get field x from landing_target message + * + * @return [m] X Position of the landing target in MAV_FRAME + */ +static inline float mavlink_msg_landing_target_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 30); +} + +/** + * @brief Get field y from landing_target message + * + * @return [m] Y Position of the landing target in MAV_FRAME + */ +static inline float mavlink_msg_landing_target_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 34); +} + +/** + * @brief Get field z from landing_target message + * + * @return [m] Z Position of the landing target in MAV_FRAME + */ +static inline float mavlink_msg_landing_target_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 38); +} + +/** + * @brief Get field q from landing_target message + * + * @return Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + */ +static inline uint16_t mavlink_msg_landing_target_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 42); +} + +/** + * @brief Get field type from landing_target message + * + * @return Type of landing target + */ +static inline uint8_t mavlink_msg_landing_target_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 58); +} + +/** + * @brief Get field position_valid from landing_target message + * + * @return Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid). + */ +static inline uint8_t mavlink_msg_landing_target_get_position_valid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 59); +} + /** * @brief Decode a landing_target message into a struct * @@ -380,6 +516,12 @@ static inline void mavlink_msg_landing_target_decode(const mavlink_message_t* ms landing_target->size_y = mavlink_msg_landing_target_get_size_y(msg); landing_target->target_num = mavlink_msg_landing_target_get_target_num(msg); landing_target->frame = mavlink_msg_landing_target_get_frame(msg); + landing_target->x = mavlink_msg_landing_target_get_x(msg); + landing_target->y = mavlink_msg_landing_target_get_y(msg); + landing_target->z = mavlink_msg_landing_target_get_z(msg); + mavlink_msg_landing_target_get_q(msg, landing_target->q); + landing_target->type = mavlink_msg_landing_target_get_type(msg); + landing_target->position_valid = mavlink_msg_landing_target_get_position_valid(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_LANDING_TARGET_LEN? msg->len : MAVLINK_MSG_ID_LANDING_TARGET_LEN; memset(landing_target, 0, MAVLINK_MSG_ID_LANDING_TARGET_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_logging_ack.h b/lib/main/MAVLink/common/mavlink_msg_logging_ack.h new file mode 100644 index 0000000000..56dab387de --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_logging_ack.h @@ -0,0 +1,263 @@ +#pragma once +// MESSAGE LOGGING_ACK PACKING + +#define MAVLINK_MSG_ID_LOGGING_ACK 268 + + +typedef struct __mavlink_logging_ack_t { + uint16_t sequence; /*< sequence number (must match the one in LOGGING_DATA_ACKED)*/ + uint8_t target_system; /*< system ID of the target*/ + uint8_t target_component; /*< component ID of the target*/ +} mavlink_logging_ack_t; + +#define MAVLINK_MSG_ID_LOGGING_ACK_LEN 4 +#define MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN 4 +#define MAVLINK_MSG_ID_268_LEN 4 +#define MAVLINK_MSG_ID_268_MIN_LEN 4 + +#define MAVLINK_MSG_ID_LOGGING_ACK_CRC 14 +#define MAVLINK_MSG_ID_268_CRC 14 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_LOGGING_ACK { \ + 268, \ + "LOGGING_ACK", \ + 3, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_logging_ack_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_logging_ack_t, target_component) }, \ + { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_logging_ack_t, sequence) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_LOGGING_ACK { \ + "LOGGING_ACK", \ + 3, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_logging_ack_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_logging_ack_t, target_component) }, \ + { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_logging_ack_t, sequence) }, \ + } \ +} +#endif + +/** + * @brief Pack a logging_ack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (must match the one in LOGGING_DATA_ACKED) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_logging_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t sequence) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_ACK_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOGGING_ACK_LEN); +#else + mavlink_logging_ack_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOGGING_ACK; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN, MAVLINK_MSG_ID_LOGGING_ACK_LEN, MAVLINK_MSG_ID_LOGGING_ACK_CRC); +} + +/** + * @brief Pack a logging_ack message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (must match the one in LOGGING_DATA_ACKED) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_logging_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t sequence) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_ACK_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOGGING_ACK_LEN); +#else + mavlink_logging_ack_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOGGING_ACK; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN, MAVLINK_MSG_ID_LOGGING_ACK_LEN, MAVLINK_MSG_ID_LOGGING_ACK_CRC); +} + +/** + * @brief Encode a logging_ack struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param logging_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_logging_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_logging_ack_t* logging_ack) +{ + return mavlink_msg_logging_ack_pack(system_id, component_id, msg, logging_ack->target_system, logging_ack->target_component, logging_ack->sequence); +} + +/** + * @brief Encode a logging_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param logging_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_logging_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_logging_ack_t* logging_ack) +{ + return mavlink_msg_logging_ack_pack_chan(system_id, component_id, chan, msg, logging_ack->target_system, logging_ack->target_component, logging_ack->sequence); +} + +/** + * @brief Send a logging_ack message + * @param chan MAVLink channel to send the message + * + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (must match the one in LOGGING_DATA_ACKED) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_logging_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t sequence) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_ACK_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_ACK, buf, MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN, MAVLINK_MSG_ID_LOGGING_ACK_LEN, MAVLINK_MSG_ID_LOGGING_ACK_CRC); +#else + mavlink_logging_ack_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_ACK, (const char *)&packet, MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN, MAVLINK_MSG_ID_LOGGING_ACK_LEN, MAVLINK_MSG_ID_LOGGING_ACK_CRC); +#endif +} + +/** + * @brief Send a logging_ack message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_logging_ack_send_struct(mavlink_channel_t chan, const mavlink_logging_ack_t* logging_ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_logging_ack_send(chan, logging_ack->target_system, logging_ack->target_component, logging_ack->sequence); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_ACK, (const char *)logging_ack, MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN, MAVLINK_MSG_ID_LOGGING_ACK_LEN, MAVLINK_MSG_ID_LOGGING_ACK_CRC); +#endif +} + +#if MAVLINK_MSG_ID_LOGGING_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_logging_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t sequence) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_ACK, buf, MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN, MAVLINK_MSG_ID_LOGGING_ACK_LEN, MAVLINK_MSG_ID_LOGGING_ACK_CRC); +#else + mavlink_logging_ack_t *packet = (mavlink_logging_ack_t *)msgbuf; + packet->sequence = sequence; + packet->target_system = target_system; + packet->target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_ACK, (const char *)packet, MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN, MAVLINK_MSG_ID_LOGGING_ACK_LEN, MAVLINK_MSG_ID_LOGGING_ACK_CRC); +#endif +} +#endif + +#endif + +// MESSAGE LOGGING_ACK UNPACKING + + +/** + * @brief Get field target_system from logging_ack message + * + * @return system ID of the target + */ +static inline uint8_t mavlink_msg_logging_ack_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field target_component from logging_ack message + * + * @return component ID of the target + */ +static inline uint8_t mavlink_msg_logging_ack_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field sequence from logging_ack message + * + * @return sequence number (must match the one in LOGGING_DATA_ACKED) + */ +static inline uint16_t mavlink_msg_logging_ack_get_sequence(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Decode a logging_ack message into a struct + * + * @param msg The message to decode + * @param logging_ack C-struct to decode the message contents into + */ +static inline void mavlink_msg_logging_ack_decode(const mavlink_message_t* msg, mavlink_logging_ack_t* logging_ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + logging_ack->sequence = mavlink_msg_logging_ack_get_sequence(msg); + logging_ack->target_system = mavlink_msg_logging_ack_get_target_system(msg); + logging_ack->target_component = mavlink_msg_logging_ack_get_target_component(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_LOGGING_ACK_LEN? msg->len : MAVLINK_MSG_ID_LOGGING_ACK_LEN; + memset(logging_ack, 0, MAVLINK_MSG_ID_LOGGING_ACK_LEN); + memcpy(logging_ack, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_logging_data.h b/lib/main/MAVLink/common/mavlink_msg_logging_data.h new file mode 100644 index 0000000000..c9bf0559de --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_logging_data.h @@ -0,0 +1,330 @@ +#pragma once +// MESSAGE LOGGING_DATA PACKING + +#define MAVLINK_MSG_ID_LOGGING_DATA 266 + + +typedef struct __mavlink_logging_data_t { + uint16_t sequence; /*< sequence number (can wrap)*/ + uint8_t target_system; /*< system ID of the target*/ + uint8_t target_component; /*< component ID of the target*/ + uint8_t length; /*< [bytes] data length*/ + uint8_t first_message_offset; /*< [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists).*/ + uint8_t data[249]; /*< logged data*/ +} mavlink_logging_data_t; + +#define MAVLINK_MSG_ID_LOGGING_DATA_LEN 255 +#define MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN 255 +#define MAVLINK_MSG_ID_266_LEN 255 +#define MAVLINK_MSG_ID_266_MIN_LEN 255 + +#define MAVLINK_MSG_ID_LOGGING_DATA_CRC 193 +#define MAVLINK_MSG_ID_266_CRC 193 + +#define MAVLINK_MSG_LOGGING_DATA_FIELD_DATA_LEN 249 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_LOGGING_DATA { \ + 266, \ + "LOGGING_DATA", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_logging_data_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_logging_data_t, target_component) }, \ + { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_logging_data_t, sequence) }, \ + { "length", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_logging_data_t, length) }, \ + { "first_message_offset", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_logging_data_t, first_message_offset) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 249, 6, offsetof(mavlink_logging_data_t, data) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_LOGGING_DATA { \ + "LOGGING_DATA", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_logging_data_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_logging_data_t, target_component) }, \ + { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_logging_data_t, sequence) }, \ + { "length", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_logging_data_t, length) }, \ + { "first_message_offset", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_logging_data_t, first_message_offset) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 249, 6, offsetof(mavlink_logging_data_t, data) }, \ + } \ +} +#endif + +/** + * @brief Pack a logging_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (can wrap) + * @param length [bytes] data length + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param data logged data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_logging_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message_offset, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_DATA_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOGGING_DATA_LEN); +#else + mavlink_logging_data_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + packet.length = length; + packet.first_message_offset = first_message_offset; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOGGING_DATA; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); +} + +/** + * @brief Pack a logging_data message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (can wrap) + * @param length [bytes] data length + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param data logged data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_logging_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t sequence,uint8_t length,uint8_t first_message_offset,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_DATA_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOGGING_DATA_LEN); +#else + mavlink_logging_data_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + packet.length = length; + packet.first_message_offset = first_message_offset; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOGGING_DATA; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); +} + +/** + * @brief Encode a logging_data struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param logging_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_logging_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_logging_data_t* logging_data) +{ + return mavlink_msg_logging_data_pack(system_id, component_id, msg, logging_data->target_system, logging_data->target_component, logging_data->sequence, logging_data->length, logging_data->first_message_offset, logging_data->data); +} + +/** + * @brief Encode a logging_data struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param logging_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_logging_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_logging_data_t* logging_data) +{ + return mavlink_msg_logging_data_pack_chan(system_id, component_id, chan, msg, logging_data->target_system, logging_data->target_component, logging_data->sequence, logging_data->length, logging_data->first_message_offset, logging_data->data); +} + +/** + * @brief Send a logging_data message + * @param chan MAVLink channel to send the message + * + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (can wrap) + * @param length [bytes] data length + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param data logged data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_logging_data_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message_offset, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_DATA_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA, buf, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); +#else + mavlink_logging_data_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + packet.length = length; + packet.first_message_offset = first_message_offset; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); +#endif +} + +/** + * @brief Send a logging_data message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_logging_data_send_struct(mavlink_channel_t chan, const mavlink_logging_data_t* logging_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_logging_data_send(chan, logging_data->target_system, logging_data->target_component, logging_data->sequence, logging_data->length, logging_data->first_message_offset, logging_data->data); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA, (const char *)logging_data, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); +#endif +} + +#if MAVLINK_MSG_ID_LOGGING_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_logging_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message_offset, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA, buf, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); +#else + mavlink_logging_data_t *packet = (mavlink_logging_data_t *)msgbuf; + packet->sequence = sequence; + packet->target_system = target_system; + packet->target_component = target_component; + packet->length = length; + packet->first_message_offset = first_message_offset; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*249); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA, (const char *)packet, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); +#endif +} +#endif + +#endif + +// MESSAGE LOGGING_DATA UNPACKING + + +/** + * @brief Get field target_system from logging_data message + * + * @return system ID of the target + */ +static inline uint8_t mavlink_msg_logging_data_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field target_component from logging_data message + * + * @return component ID of the target + */ +static inline uint8_t mavlink_msg_logging_data_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field sequence from logging_data message + * + * @return sequence number (can wrap) + */ +static inline uint16_t mavlink_msg_logging_data_get_sequence(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field length from logging_data message + * + * @return [bytes] data length + */ +static inline uint8_t mavlink_msg_logging_data_get_length(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field first_message_offset from logging_data message + * + * @return [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + */ +static inline uint8_t mavlink_msg_logging_data_get_first_message_offset(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field data from logging_data message + * + * @return logged data + */ +static inline uint16_t mavlink_msg_logging_data_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 249, 6); +} + +/** + * @brief Decode a logging_data message into a struct + * + * @param msg The message to decode + * @param logging_data C-struct to decode the message contents into + */ +static inline void mavlink_msg_logging_data_decode(const mavlink_message_t* msg, mavlink_logging_data_t* logging_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + logging_data->sequence = mavlink_msg_logging_data_get_sequence(msg); + logging_data->target_system = mavlink_msg_logging_data_get_target_system(msg); + logging_data->target_component = mavlink_msg_logging_data_get_target_component(msg); + logging_data->length = mavlink_msg_logging_data_get_length(msg); + logging_data->first_message_offset = mavlink_msg_logging_data_get_first_message_offset(msg); + mavlink_msg_logging_data_get_data(msg, logging_data->data); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_LOGGING_DATA_LEN? msg->len : MAVLINK_MSG_ID_LOGGING_DATA_LEN; + memset(logging_data, 0, MAVLINK_MSG_ID_LOGGING_DATA_LEN); + memcpy(logging_data, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_logging_data_acked.h b/lib/main/MAVLink/common/mavlink_msg_logging_data_acked.h new file mode 100644 index 0000000000..a179387c72 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_logging_data_acked.h @@ -0,0 +1,330 @@ +#pragma once +// MESSAGE LOGGING_DATA_ACKED PACKING + +#define MAVLINK_MSG_ID_LOGGING_DATA_ACKED 267 + + +typedef struct __mavlink_logging_data_acked_t { + uint16_t sequence; /*< sequence number (can wrap)*/ + uint8_t target_system; /*< system ID of the target*/ + uint8_t target_component; /*< component ID of the target*/ + uint8_t length; /*< [bytes] data length*/ + uint8_t first_message_offset; /*< [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists).*/ + uint8_t data[249]; /*< logged data*/ +} mavlink_logging_data_acked_t; + +#define MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN 255 +#define MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN 255 +#define MAVLINK_MSG_ID_267_LEN 255 +#define MAVLINK_MSG_ID_267_MIN_LEN 255 + +#define MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC 35 +#define MAVLINK_MSG_ID_267_CRC 35 + +#define MAVLINK_MSG_LOGGING_DATA_ACKED_FIELD_DATA_LEN 249 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_LOGGING_DATA_ACKED { \ + 267, \ + "LOGGING_DATA_ACKED", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_logging_data_acked_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_logging_data_acked_t, target_component) }, \ + { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_logging_data_acked_t, sequence) }, \ + { "length", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_logging_data_acked_t, length) }, \ + { "first_message_offset", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_logging_data_acked_t, first_message_offset) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 249, 6, offsetof(mavlink_logging_data_acked_t, data) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_LOGGING_DATA_ACKED { \ + "LOGGING_DATA_ACKED", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_logging_data_acked_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_logging_data_acked_t, target_component) }, \ + { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_logging_data_acked_t, sequence) }, \ + { "length", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_logging_data_acked_t, length) }, \ + { "first_message_offset", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_logging_data_acked_t, first_message_offset) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 249, 6, offsetof(mavlink_logging_data_acked_t, data) }, \ + } \ +} +#endif + +/** + * @brief Pack a logging_data_acked message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (can wrap) + * @param length [bytes] data length + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param data logged data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_logging_data_acked_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message_offset, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN); +#else + mavlink_logging_data_acked_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + packet.length = length; + packet.first_message_offset = first_message_offset; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOGGING_DATA_ACKED; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); +} + +/** + * @brief Pack a logging_data_acked message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (can wrap) + * @param length [bytes] data length + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param data logged data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_logging_data_acked_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t sequence,uint8_t length,uint8_t first_message_offset,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN); +#else + mavlink_logging_data_acked_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + packet.length = length; + packet.first_message_offset = first_message_offset; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOGGING_DATA_ACKED; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); +} + +/** + * @brief Encode a logging_data_acked struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param logging_data_acked C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_logging_data_acked_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_logging_data_acked_t* logging_data_acked) +{ + return mavlink_msg_logging_data_acked_pack(system_id, component_id, msg, logging_data_acked->target_system, logging_data_acked->target_component, logging_data_acked->sequence, logging_data_acked->length, logging_data_acked->first_message_offset, logging_data_acked->data); +} + +/** + * @brief Encode a logging_data_acked struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param logging_data_acked C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_logging_data_acked_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_logging_data_acked_t* logging_data_acked) +{ + return mavlink_msg_logging_data_acked_pack_chan(system_id, component_id, chan, msg, logging_data_acked->target_system, logging_data_acked->target_component, logging_data_acked->sequence, logging_data_acked->length, logging_data_acked->first_message_offset, logging_data_acked->data); +} + +/** + * @brief Send a logging_data_acked message + * @param chan MAVLink channel to send the message + * + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (can wrap) + * @param length [bytes] data length + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param data logged data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_logging_data_acked_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message_offset, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA_ACKED, buf, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); +#else + mavlink_logging_data_acked_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + packet.length = length; + packet.first_message_offset = first_message_offset; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA_ACKED, (const char *)&packet, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); +#endif +} + +/** + * @brief Send a logging_data_acked message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_logging_data_acked_send_struct(mavlink_channel_t chan, const mavlink_logging_data_acked_t* logging_data_acked) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_logging_data_acked_send(chan, logging_data_acked->target_system, logging_data_acked->target_component, logging_data_acked->sequence, logging_data_acked->length, logging_data_acked->first_message_offset, logging_data_acked->data); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA_ACKED, (const char *)logging_data_acked, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); +#endif +} + +#if MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_logging_data_acked_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message_offset, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA_ACKED, buf, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); +#else + mavlink_logging_data_acked_t *packet = (mavlink_logging_data_acked_t *)msgbuf; + packet->sequence = sequence; + packet->target_system = target_system; + packet->target_component = target_component; + packet->length = length; + packet->first_message_offset = first_message_offset; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*249); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA_ACKED, (const char *)packet, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); +#endif +} +#endif + +#endif + +// MESSAGE LOGGING_DATA_ACKED UNPACKING + + +/** + * @brief Get field target_system from logging_data_acked message + * + * @return system ID of the target + */ +static inline uint8_t mavlink_msg_logging_data_acked_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field target_component from logging_data_acked message + * + * @return component ID of the target + */ +static inline uint8_t mavlink_msg_logging_data_acked_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field sequence from logging_data_acked message + * + * @return sequence number (can wrap) + */ +static inline uint16_t mavlink_msg_logging_data_acked_get_sequence(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field length from logging_data_acked message + * + * @return [bytes] data length + */ +static inline uint8_t mavlink_msg_logging_data_acked_get_length(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field first_message_offset from logging_data_acked message + * + * @return [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + */ +static inline uint8_t mavlink_msg_logging_data_acked_get_first_message_offset(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field data from logging_data_acked message + * + * @return logged data + */ +static inline uint16_t mavlink_msg_logging_data_acked_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 249, 6); +} + +/** + * @brief Decode a logging_data_acked message into a struct + * + * @param msg The message to decode + * @param logging_data_acked C-struct to decode the message contents into + */ +static inline void mavlink_msg_logging_data_acked_decode(const mavlink_message_t* msg, mavlink_logging_data_acked_t* logging_data_acked) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + logging_data_acked->sequence = mavlink_msg_logging_data_acked_get_sequence(msg); + logging_data_acked->target_system = mavlink_msg_logging_data_acked_get_target_system(msg); + logging_data_acked->target_component = mavlink_msg_logging_data_acked_get_target_component(msg); + logging_data_acked->length = mavlink_msg_logging_data_acked_get_length(msg); + logging_data_acked->first_message_offset = mavlink_msg_logging_data_acked_get_first_message_offset(msg); + mavlink_msg_logging_data_acked_get_data(msg, logging_data_acked->data); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN? msg->len : MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN; + memset(logging_data_acked, 0, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN); + memcpy(logging_data_acked, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_mag_cal_report.h b/lib/main/MAVLink/common/mavlink_msg_mag_cal_report.h new file mode 100644 index 0000000000..6694501e17 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_mag_cal_report.h @@ -0,0 +1,638 @@ +#pragma once +// MESSAGE MAG_CAL_REPORT PACKING + +#define MAVLINK_MSG_ID_MAG_CAL_REPORT 192 + +MAVPACKED( +typedef struct __mavlink_mag_cal_report_t { + float fitness; /*< [mgauss] RMS milligauss residuals.*/ + float ofs_x; /*< X offset.*/ + float ofs_y; /*< Y offset.*/ + float ofs_z; /*< Z offset.*/ + float diag_x; /*< X diagonal (matrix 11).*/ + float diag_y; /*< Y diagonal (matrix 22).*/ + float diag_z; /*< Z diagonal (matrix 33).*/ + float offdiag_x; /*< X off-diagonal (matrix 12 and 21).*/ + float offdiag_y; /*< Y off-diagonal (matrix 13 and 31).*/ + float offdiag_z; /*< Z off-diagonal (matrix 32 and 23).*/ + uint8_t compass_id; /*< Compass being calibrated.*/ + uint8_t cal_mask; /*< Bitmask of compasses being calibrated.*/ + uint8_t cal_status; /*< Calibration Status.*/ + uint8_t autosaved; /*< 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters.*/ + float orientation_confidence; /*< Confidence in orientation (higher is better).*/ + uint8_t old_orientation; /*< orientation before calibration.*/ + uint8_t new_orientation; /*< orientation after calibration.*/ + float scale_factor; /*< field radius correction factor*/ +}) mavlink_mag_cal_report_t; + +#define MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN 54 +#define MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN 44 +#define MAVLINK_MSG_ID_192_LEN 54 +#define MAVLINK_MSG_ID_192_MIN_LEN 44 + +#define MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC 36 +#define MAVLINK_MSG_ID_192_CRC 36 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT { \ + 192, \ + "MAG_CAL_REPORT", \ + 18, \ + { { "compass_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_mag_cal_report_t, compass_id) }, \ + { "cal_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_mag_cal_report_t, cal_mask) }, \ + { "cal_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_mag_cal_report_t, cal_status) }, \ + { "autosaved", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_mag_cal_report_t, autosaved) }, \ + { "fitness", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mag_cal_report_t, fitness) }, \ + { "ofs_x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mag_cal_report_t, ofs_x) }, \ + { "ofs_y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mag_cal_report_t, ofs_y) }, \ + { "ofs_z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mag_cal_report_t, ofs_z) }, \ + { "diag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mag_cal_report_t, diag_x) }, \ + { "diag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mag_cal_report_t, diag_y) }, \ + { "diag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mag_cal_report_t, diag_z) }, \ + { "offdiag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_mag_cal_report_t, offdiag_x) }, \ + { "offdiag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_mag_cal_report_t, offdiag_y) }, \ + { "offdiag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_mag_cal_report_t, offdiag_z) }, \ + { "orientation_confidence", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_mag_cal_report_t, orientation_confidence) }, \ + { "old_orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_mag_cal_report_t, old_orientation) }, \ + { "new_orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 49, offsetof(mavlink_mag_cal_report_t, new_orientation) }, \ + { "scale_factor", NULL, MAVLINK_TYPE_FLOAT, 0, 50, offsetof(mavlink_mag_cal_report_t, scale_factor) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT { \ + "MAG_CAL_REPORT", \ + 18, \ + { { "compass_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_mag_cal_report_t, compass_id) }, \ + { "cal_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_mag_cal_report_t, cal_mask) }, \ + { "cal_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_mag_cal_report_t, cal_status) }, \ + { "autosaved", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_mag_cal_report_t, autosaved) }, \ + { "fitness", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mag_cal_report_t, fitness) }, \ + { "ofs_x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mag_cal_report_t, ofs_x) }, \ + { "ofs_y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mag_cal_report_t, ofs_y) }, \ + { "ofs_z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mag_cal_report_t, ofs_z) }, \ + { "diag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mag_cal_report_t, diag_x) }, \ + { "diag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mag_cal_report_t, diag_y) }, \ + { "diag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mag_cal_report_t, diag_z) }, \ + { "offdiag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_mag_cal_report_t, offdiag_x) }, \ + { "offdiag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_mag_cal_report_t, offdiag_y) }, \ + { "offdiag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_mag_cal_report_t, offdiag_z) }, \ + { "orientation_confidence", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_mag_cal_report_t, orientation_confidence) }, \ + { "old_orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_mag_cal_report_t, old_orientation) }, \ + { "new_orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 49, offsetof(mavlink_mag_cal_report_t, new_orientation) }, \ + { "scale_factor", NULL, MAVLINK_TYPE_FLOAT, 0, 50, offsetof(mavlink_mag_cal_report_t, scale_factor) }, \ + } \ +} +#endif + +/** + * @brief Pack a mag_cal_report message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param compass_id Compass being calibrated. + * @param cal_mask Bitmask of compasses being calibrated. + * @param cal_status Calibration Status. + * @param autosaved 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters. + * @param fitness [mgauss] RMS milligauss residuals. + * @param ofs_x X offset. + * @param ofs_y Y offset. + * @param ofs_z Z offset. + * @param diag_x X diagonal (matrix 11). + * @param diag_y Y diagonal (matrix 22). + * @param diag_z Z diagonal (matrix 33). + * @param offdiag_x X off-diagonal (matrix 12 and 21). + * @param offdiag_y Y off-diagonal (matrix 13 and 31). + * @param offdiag_z Z off-diagonal (matrix 32 and 23). + * @param orientation_confidence Confidence in orientation (higher is better). + * @param old_orientation orientation before calibration. + * @param new_orientation orientation after calibration. + * @param scale_factor field radius correction factor + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_report_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z, float orientation_confidence, uint8_t old_orientation, uint8_t new_orientation, float scale_factor) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + _mav_put_float(buf, 44, orientation_confidence); + _mav_put_uint8_t(buf, 48, old_orientation); + _mav_put_uint8_t(buf, 49, new_orientation); + _mav_put_float(buf, 50, scale_factor); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#else + mavlink_mag_cal_report_t packet; + packet.fitness = fitness; + packet.ofs_x = ofs_x; + packet.ofs_y = ofs_y; + packet.ofs_z = ofs_z; + packet.diag_x = diag_x; + packet.diag_y = diag_y; + packet.diag_z = diag_z; + packet.offdiag_x = offdiag_x; + packet.offdiag_y = offdiag_y; + packet.offdiag_z = offdiag_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.autosaved = autosaved; + packet.orientation_confidence = orientation_confidence; + packet.old_orientation = old_orientation; + packet.new_orientation = new_orientation; + packet.scale_factor = scale_factor; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_REPORT; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +} + +/** + * @brief Pack a mag_cal_report message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param compass_id Compass being calibrated. + * @param cal_mask Bitmask of compasses being calibrated. + * @param cal_status Calibration Status. + * @param autosaved 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters. + * @param fitness [mgauss] RMS milligauss residuals. + * @param ofs_x X offset. + * @param ofs_y Y offset. + * @param ofs_z Z offset. + * @param diag_x X diagonal (matrix 11). + * @param diag_y Y diagonal (matrix 22). + * @param diag_z Z diagonal (matrix 33). + * @param offdiag_x X off-diagonal (matrix 12 and 21). + * @param offdiag_y Y off-diagonal (matrix 13 and 31). + * @param offdiag_z Z off-diagonal (matrix 32 and 23). + * @param orientation_confidence Confidence in orientation (higher is better). + * @param old_orientation orientation before calibration. + * @param new_orientation orientation after calibration. + * @param scale_factor field radius correction factor + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_report_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t compass_id,uint8_t cal_mask,uint8_t cal_status,uint8_t autosaved,float fitness,float ofs_x,float ofs_y,float ofs_z,float diag_x,float diag_y,float diag_z,float offdiag_x,float offdiag_y,float offdiag_z,float orientation_confidence,uint8_t old_orientation,uint8_t new_orientation,float scale_factor) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + _mav_put_float(buf, 44, orientation_confidence); + _mav_put_uint8_t(buf, 48, old_orientation); + _mav_put_uint8_t(buf, 49, new_orientation); + _mav_put_float(buf, 50, scale_factor); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#else + mavlink_mag_cal_report_t packet; + packet.fitness = fitness; + packet.ofs_x = ofs_x; + packet.ofs_y = ofs_y; + packet.ofs_z = ofs_z; + packet.diag_x = diag_x; + packet.diag_y = diag_y; + packet.diag_z = diag_z; + packet.offdiag_x = offdiag_x; + packet.offdiag_y = offdiag_y; + packet.offdiag_z = offdiag_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.autosaved = autosaved; + packet.orientation_confidence = orientation_confidence; + packet.old_orientation = old_orientation; + packet.new_orientation = new_orientation; + packet.scale_factor = scale_factor; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_REPORT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +} + +/** + * @brief Encode a mag_cal_report struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mag_cal_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_report_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mag_cal_report_t* mag_cal_report) +{ + return mavlink_msg_mag_cal_report_pack(system_id, component_id, msg, mag_cal_report->compass_id, mag_cal_report->cal_mask, mag_cal_report->cal_status, mag_cal_report->autosaved, mag_cal_report->fitness, mag_cal_report->ofs_x, mag_cal_report->ofs_y, mag_cal_report->ofs_z, mag_cal_report->diag_x, mag_cal_report->diag_y, mag_cal_report->diag_z, mag_cal_report->offdiag_x, mag_cal_report->offdiag_y, mag_cal_report->offdiag_z, mag_cal_report->orientation_confidence, mag_cal_report->old_orientation, mag_cal_report->new_orientation, mag_cal_report->scale_factor); +} + +/** + * @brief Encode a mag_cal_report struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mag_cal_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_report_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mag_cal_report_t* mag_cal_report) +{ + return mavlink_msg_mag_cal_report_pack_chan(system_id, component_id, chan, msg, mag_cal_report->compass_id, mag_cal_report->cal_mask, mag_cal_report->cal_status, mag_cal_report->autosaved, mag_cal_report->fitness, mag_cal_report->ofs_x, mag_cal_report->ofs_y, mag_cal_report->ofs_z, mag_cal_report->diag_x, mag_cal_report->diag_y, mag_cal_report->diag_z, mag_cal_report->offdiag_x, mag_cal_report->offdiag_y, mag_cal_report->offdiag_z, mag_cal_report->orientation_confidence, mag_cal_report->old_orientation, mag_cal_report->new_orientation, mag_cal_report->scale_factor); +} + +/** + * @brief Send a mag_cal_report message + * @param chan MAVLink channel to send the message + * + * @param compass_id Compass being calibrated. + * @param cal_mask Bitmask of compasses being calibrated. + * @param cal_status Calibration Status. + * @param autosaved 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters. + * @param fitness [mgauss] RMS milligauss residuals. + * @param ofs_x X offset. + * @param ofs_y Y offset. + * @param ofs_z Z offset. + * @param diag_x X diagonal (matrix 11). + * @param diag_y Y diagonal (matrix 22). + * @param diag_z Z diagonal (matrix 33). + * @param offdiag_x X off-diagonal (matrix 12 and 21). + * @param offdiag_y Y off-diagonal (matrix 13 and 31). + * @param offdiag_z Z off-diagonal (matrix 32 and 23). + * @param orientation_confidence Confidence in orientation (higher is better). + * @param old_orientation orientation before calibration. + * @param new_orientation orientation after calibration. + * @param scale_factor field radius correction factor + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mag_cal_report_send(mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z, float orientation_confidence, uint8_t old_orientation, uint8_t new_orientation, float scale_factor) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + _mav_put_float(buf, 44, orientation_confidence); + _mav_put_uint8_t(buf, 48, old_orientation); + _mav_put_uint8_t(buf, 49, new_orientation); + _mav_put_float(buf, 50, scale_factor); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + mavlink_mag_cal_report_t packet; + packet.fitness = fitness; + packet.ofs_x = ofs_x; + packet.ofs_y = ofs_y; + packet.ofs_z = ofs_z; + packet.diag_x = diag_x; + packet.diag_y = diag_y; + packet.diag_z = diag_z; + packet.offdiag_x = offdiag_x; + packet.offdiag_y = offdiag_y; + packet.offdiag_z = offdiag_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.autosaved = autosaved; + packet.orientation_confidence = orientation_confidence; + packet.old_orientation = old_orientation; + packet.new_orientation = new_orientation; + packet.scale_factor = scale_factor; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#endif +} + +/** + * @brief Send a mag_cal_report message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_mag_cal_report_send_struct(mavlink_channel_t chan, const mavlink_mag_cal_report_t* mag_cal_report) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_mag_cal_report_send(chan, mag_cal_report->compass_id, mag_cal_report->cal_mask, mag_cal_report->cal_status, mag_cal_report->autosaved, mag_cal_report->fitness, mag_cal_report->ofs_x, mag_cal_report->ofs_y, mag_cal_report->ofs_z, mag_cal_report->diag_x, mag_cal_report->diag_y, mag_cal_report->diag_z, mag_cal_report->offdiag_x, mag_cal_report->offdiag_y, mag_cal_report->offdiag_z, mag_cal_report->orientation_confidence, mag_cal_report->old_orientation, mag_cal_report->new_orientation, mag_cal_report->scale_factor); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)mag_cal_report, MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#endif +} + +#if MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mag_cal_report_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z, float orientation_confidence, uint8_t old_orientation, uint8_t new_orientation, float scale_factor) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + _mav_put_float(buf, 44, orientation_confidence); + _mav_put_uint8_t(buf, 48, old_orientation); + _mav_put_uint8_t(buf, 49, new_orientation); + _mav_put_float(buf, 50, scale_factor); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + mavlink_mag_cal_report_t *packet = (mavlink_mag_cal_report_t *)msgbuf; + packet->fitness = fitness; + packet->ofs_x = ofs_x; + packet->ofs_y = ofs_y; + packet->ofs_z = ofs_z; + packet->diag_x = diag_x; + packet->diag_y = diag_y; + packet->diag_z = diag_z; + packet->offdiag_x = offdiag_x; + packet->offdiag_y = offdiag_y; + packet->offdiag_z = offdiag_z; + packet->compass_id = compass_id; + packet->cal_mask = cal_mask; + packet->cal_status = cal_status; + packet->autosaved = autosaved; + packet->orientation_confidence = orientation_confidence; + packet->old_orientation = old_orientation; + packet->new_orientation = new_orientation; + packet->scale_factor = scale_factor; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#endif +} +#endif + +#endif + +// MESSAGE MAG_CAL_REPORT UNPACKING + + +/** + * @brief Get field compass_id from mag_cal_report message + * + * @return Compass being calibrated. + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_compass_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 40); +} + +/** + * @brief Get field cal_mask from mag_cal_report message + * + * @return Bitmask of compasses being calibrated. + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_cal_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 41); +} + +/** + * @brief Get field cal_status from mag_cal_report message + * + * @return Calibration Status. + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_cal_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 42); +} + +/** + * @brief Get field autosaved from mag_cal_report message + * + * @return 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters. + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_autosaved(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 43); +} + +/** + * @brief Get field fitness from mag_cal_report message + * + * @return [mgauss] RMS milligauss residuals. + */ +static inline float mavlink_msg_mag_cal_report_get_fitness(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field ofs_x from mag_cal_report message + * + * @return X offset. + */ +static inline float mavlink_msg_mag_cal_report_get_ofs_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field ofs_y from mag_cal_report message + * + * @return Y offset. + */ +static inline float mavlink_msg_mag_cal_report_get_ofs_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field ofs_z from mag_cal_report message + * + * @return Z offset. + */ +static inline float mavlink_msg_mag_cal_report_get_ofs_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field diag_x from mag_cal_report message + * + * @return X diagonal (matrix 11). + */ +static inline float mavlink_msg_mag_cal_report_get_diag_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field diag_y from mag_cal_report message + * + * @return Y diagonal (matrix 22). + */ +static inline float mavlink_msg_mag_cal_report_get_diag_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field diag_z from mag_cal_report message + * + * @return Z diagonal (matrix 33). + */ +static inline float mavlink_msg_mag_cal_report_get_diag_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field offdiag_x from mag_cal_report message + * + * @return X off-diagonal (matrix 12 and 21). + */ +static inline float mavlink_msg_mag_cal_report_get_offdiag_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field offdiag_y from mag_cal_report message + * + * @return Y off-diagonal (matrix 13 and 31). + */ +static inline float mavlink_msg_mag_cal_report_get_offdiag_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field offdiag_z from mag_cal_report message + * + * @return Z off-diagonal (matrix 32 and 23). + */ +static inline float mavlink_msg_mag_cal_report_get_offdiag_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field orientation_confidence from mag_cal_report message + * + * @return Confidence in orientation (higher is better). + */ +static inline float mavlink_msg_mag_cal_report_get_orientation_confidence(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field old_orientation from mag_cal_report message + * + * @return orientation before calibration. + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_old_orientation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 48); +} + +/** + * @brief Get field new_orientation from mag_cal_report message + * + * @return orientation after calibration. + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_new_orientation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 49); +} + +/** + * @brief Get field scale_factor from mag_cal_report message + * + * @return field radius correction factor + */ +static inline float mavlink_msg_mag_cal_report_get_scale_factor(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 50); +} + +/** + * @brief Decode a mag_cal_report message into a struct + * + * @param msg The message to decode + * @param mag_cal_report C-struct to decode the message contents into + */ +static inline void mavlink_msg_mag_cal_report_decode(const mavlink_message_t* msg, mavlink_mag_cal_report_t* mag_cal_report) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mag_cal_report->fitness = mavlink_msg_mag_cal_report_get_fitness(msg); + mag_cal_report->ofs_x = mavlink_msg_mag_cal_report_get_ofs_x(msg); + mag_cal_report->ofs_y = mavlink_msg_mag_cal_report_get_ofs_y(msg); + mag_cal_report->ofs_z = mavlink_msg_mag_cal_report_get_ofs_z(msg); + mag_cal_report->diag_x = mavlink_msg_mag_cal_report_get_diag_x(msg); + mag_cal_report->diag_y = mavlink_msg_mag_cal_report_get_diag_y(msg); + mag_cal_report->diag_z = mavlink_msg_mag_cal_report_get_diag_z(msg); + mag_cal_report->offdiag_x = mavlink_msg_mag_cal_report_get_offdiag_x(msg); + mag_cal_report->offdiag_y = mavlink_msg_mag_cal_report_get_offdiag_y(msg); + mag_cal_report->offdiag_z = mavlink_msg_mag_cal_report_get_offdiag_z(msg); + mag_cal_report->compass_id = mavlink_msg_mag_cal_report_get_compass_id(msg); + mag_cal_report->cal_mask = mavlink_msg_mag_cal_report_get_cal_mask(msg); + mag_cal_report->cal_status = mavlink_msg_mag_cal_report_get_cal_status(msg); + mag_cal_report->autosaved = mavlink_msg_mag_cal_report_get_autosaved(msg); + mag_cal_report->orientation_confidence = mavlink_msg_mag_cal_report_get_orientation_confidence(msg); + mag_cal_report->old_orientation = mavlink_msg_mag_cal_report_get_old_orientation(msg); + mag_cal_report->new_orientation = mavlink_msg_mag_cal_report_get_new_orientation(msg); + mag_cal_report->scale_factor = mavlink_msg_mag_cal_report_get_scale_factor(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN? msg->len : MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN; + memset(mag_cal_report, 0, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); + memcpy(mag_cal_report, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_ack.h b/lib/main/MAVLink/common/mavlink_msg_mission_ack.h index 182825a5ea..916b94d38d 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_ack.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_ack.h @@ -8,11 +8,12 @@ typedef struct __mavlink_mission_ack_t { uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ uint8_t type; /*< Mission result.*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_ack_t; -#define MAVLINK_MSG_ID_MISSION_ACK_LEN 3 +#define MAVLINK_MSG_ID_MISSION_ACK_LEN 4 #define MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN 3 -#define MAVLINK_MSG_ID_47_LEN 3 +#define MAVLINK_MSG_ID_47_LEN 4 #define MAVLINK_MSG_ID_47_MIN_LEN 3 #define MAVLINK_MSG_ID_MISSION_ACK_CRC 153 @@ -24,19 +25,21 @@ typedef struct __mavlink_mission_ack_t { #define MAVLINK_MESSAGE_INFO_MISSION_ACK { \ 47, \ "MISSION_ACK", \ - 3, \ + 4, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_ack_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_ack_t, target_component) }, \ { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_ack_t, type) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_ack_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_ACK { \ "MISSION_ACK", \ - 3, \ + 4, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_ack_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_ack_t, target_component) }, \ { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_ack_t, type) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_ack_t, mission_type) }, \ } \ } #endif @@ -50,16 +53,18 @@ typedef struct __mavlink_mission_ack_t { * @param target_system System ID * @param target_component Component ID * @param type Mission result. + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t type) + uint8_t target_system, uint8_t target_component, uint8_t type, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); _mav_put_uint8_t(buf, 2, type); + _mav_put_uint8_t(buf, 3, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); #else @@ -67,6 +72,7 @@ static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t c packet.target_system = target_system; packet.target_component = target_component; packet.type = type; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); #endif @@ -84,17 +90,19 @@ static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t c * @param target_system System ID * @param target_component Component ID * @param type Mission result. + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t type) + uint8_t target_system,uint8_t target_component,uint8_t type,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); _mav_put_uint8_t(buf, 2, type); + _mav_put_uint8_t(buf, 3, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); #else @@ -102,6 +110,7 @@ static inline uint16_t mavlink_msg_mission_ack_pack_chan(uint8_t system_id, uint packet.target_system = target_system; packet.target_component = target_component; packet.type = type; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); #endif @@ -120,7 +129,7 @@ static inline uint16_t mavlink_msg_mission_ack_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_mission_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_ack_t* mission_ack) { - return mavlink_msg_mission_ack_pack(system_id, component_id, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type); + return mavlink_msg_mission_ack_pack(system_id, component_id, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type, mission_ack->mission_type); } /** @@ -134,7 +143,7 @@ static inline uint16_t mavlink_msg_mission_ack_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_mission_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_ack_t* mission_ack) { - return mavlink_msg_mission_ack_pack_chan(system_id, component_id, chan, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type); + return mavlink_msg_mission_ack_pack_chan(system_id, component_id, chan, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type, mission_ack->mission_type); } /** @@ -144,16 +153,18 @@ static inline uint16_t mavlink_msg_mission_ack_encode_chan(uint8_t system_id, ui * @param target_system System ID * @param target_component Component ID * @param type Mission result. + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) +static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); _mav_put_uint8_t(buf, 2, type); + _mav_put_uint8_t(buf, 3, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); #else @@ -161,6 +172,7 @@ static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t packet.target_system = target_system; packet.target_component = target_component; packet.type = type; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); #endif @@ -174,7 +186,7 @@ static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t static inline void mavlink_msg_mission_ack_send_struct(mavlink_channel_t chan, const mavlink_mission_ack_t* mission_ack) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_ack_send(chan, mission_ack->target_system, mission_ack->target_component, mission_ack->type); + mavlink_msg_mission_ack_send(chan, mission_ack->target_system, mission_ack->target_component, mission_ack->type, mission_ack->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)mission_ack, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); #endif @@ -188,13 +200,14 @@ static inline void mavlink_msg_mission_ack_send_struct(mavlink_channel_t chan, c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) +static inline void mavlink_msg_mission_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); _mav_put_uint8_t(buf, 2, type); + _mav_put_uint8_t(buf, 3, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); #else @@ -202,6 +215,7 @@ static inline void mavlink_msg_mission_ack_send_buf(mavlink_message_t *msgbuf, m packet->target_system = target_system; packet->target_component = target_component; packet->type = type; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)packet, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); #endif @@ -243,6 +257,16 @@ static inline uint8_t mavlink_msg_mission_ack_get_type(const mavlink_message_t* return _MAV_RETURN_uint8_t(msg, 2); } +/** + * @brief Get field mission_type from mission_ack message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_ack_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + /** * @brief Decode a mission_ack message into a struct * @@ -255,6 +279,7 @@ static inline void mavlink_msg_mission_ack_decode(const mavlink_message_t* msg, mission_ack->target_system = mavlink_msg_mission_ack_get_target_system(msg); mission_ack->target_component = mavlink_msg_mission_ack_get_target_component(msg); mission_ack->type = mavlink_msg_mission_ack_get_type(msg); + mission_ack->mission_type = mavlink_msg_mission_ack_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_ACK_LEN? msg->len : MAVLINK_MSG_ID_MISSION_ACK_LEN; memset(mission_ack, 0, MAVLINK_MSG_ID_MISSION_ACK_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_clear_all.h b/lib/main/MAVLink/common/mavlink_msg_mission_clear_all.h index bfed2d3fc2..1e585f9fca 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_clear_all.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_clear_all.h @@ -7,11 +7,12 @@ typedef struct __mavlink_mission_clear_all_t { uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_clear_all_t; -#define MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN 2 +#define MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN 3 #define MAVLINK_MSG_ID_MISSION_CLEAR_ALL_MIN_LEN 2 -#define MAVLINK_MSG_ID_45_LEN 2 +#define MAVLINK_MSG_ID_45_LEN 3 #define MAVLINK_MSG_ID_45_MIN_LEN 2 #define MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC 232 @@ -23,17 +24,19 @@ typedef struct __mavlink_mission_clear_all_t { #define MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL { \ 45, \ "MISSION_CLEAR_ALL", \ - 2, \ + 3, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_clear_all_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_clear_all_t, target_component) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_clear_all_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL { \ "MISSION_CLEAR_ALL", \ - 2, \ + 3, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_clear_all_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_clear_all_t, target_component) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_clear_all_t, mission_type) }, \ } \ } #endif @@ -46,21 +49,24 @@ typedef struct __mavlink_mission_clear_all_t { * * @param target_system System ID * @param target_component Component ID + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_clear_all_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component) + uint8_t target_system, uint8_t target_component, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); #else mavlink_mission_clear_all_t packet; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); #endif @@ -77,22 +83,25 @@ static inline uint16_t mavlink_msg_mission_clear_all_pack(uint8_t system_id, uin * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_clear_all_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component) + uint8_t target_system,uint8_t target_component,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); #else mavlink_mission_clear_all_t packet; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); #endif @@ -111,7 +120,7 @@ static inline uint16_t mavlink_msg_mission_clear_all_pack_chan(uint8_t system_id */ static inline uint16_t mavlink_msg_mission_clear_all_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_clear_all_t* mission_clear_all) { - return mavlink_msg_mission_clear_all_pack(system_id, component_id, msg, mission_clear_all->target_system, mission_clear_all->target_component); + return mavlink_msg_mission_clear_all_pack(system_id, component_id, msg, mission_clear_all->target_system, mission_clear_all->target_component, mission_clear_all->mission_type); } /** @@ -125,7 +134,7 @@ static inline uint16_t mavlink_msg_mission_clear_all_encode(uint8_t system_id, u */ static inline uint16_t mavlink_msg_mission_clear_all_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_clear_all_t* mission_clear_all) { - return mavlink_msg_mission_clear_all_pack_chan(system_id, component_id, chan, msg, mission_clear_all->target_system, mission_clear_all->target_component); + return mavlink_msg_mission_clear_all_pack_chan(system_id, component_id, chan, msg, mission_clear_all->target_system, mission_clear_all->target_component, mission_clear_all->mission_type); } /** @@ -134,21 +143,24 @@ static inline uint16_t mavlink_msg_mission_clear_all_encode_chan(uint8_t system_ * * @param target_system System ID * @param target_component Component ID + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +static inline void mavlink_msg_mission_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_MIN_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); #else mavlink_mission_clear_all_t packet; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)&packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_MIN_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); #endif @@ -162,7 +174,7 @@ static inline void mavlink_msg_mission_clear_all_send(mavlink_channel_t chan, ui static inline void mavlink_msg_mission_clear_all_send_struct(mavlink_channel_t chan, const mavlink_mission_clear_all_t* mission_clear_all) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_clear_all_send(chan, mission_clear_all->target_system, mission_clear_all->target_component); + mavlink_msg_mission_clear_all_send(chan, mission_clear_all->target_system, mission_clear_all->target_component, mission_clear_all->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)mission_clear_all, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_MIN_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); #endif @@ -176,18 +188,20 @@ static inline void mavlink_msg_mission_clear_all_send_struct(mavlink_channel_t c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_clear_all_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +static inline void mavlink_msg_mission_clear_all_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_MIN_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); #else mavlink_mission_clear_all_t *packet = (mavlink_mission_clear_all_t *)msgbuf; packet->target_system = target_system; packet->target_component = target_component; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_MIN_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); #endif @@ -219,6 +233,16 @@ static inline uint8_t mavlink_msg_mission_clear_all_get_target_component(const m return _MAV_RETURN_uint8_t(msg, 1); } +/** + * @brief Get field mission_type from mission_clear_all message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_clear_all_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + /** * @brief Decode a mission_clear_all message into a struct * @@ -230,6 +254,7 @@ static inline void mavlink_msg_mission_clear_all_decode(const mavlink_message_t* #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS mission_clear_all->target_system = mavlink_msg_mission_clear_all_get_target_system(msg); mission_clear_all->target_component = mavlink_msg_mission_clear_all_get_target_component(msg); + mission_clear_all->mission_type = mavlink_msg_mission_clear_all_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN? msg->len : MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN; memset(mission_clear_all, 0, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_count.h b/lib/main/MAVLink/common/mavlink_msg_mission_count.h index 8c0cd1c6d0..53afd49002 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_count.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_count.h @@ -8,11 +8,12 @@ typedef struct __mavlink_mission_count_t { uint16_t count; /*< Number of mission items in the sequence*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_count_t; -#define MAVLINK_MSG_ID_MISSION_COUNT_LEN 4 +#define MAVLINK_MSG_ID_MISSION_COUNT_LEN 5 #define MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN 4 -#define MAVLINK_MSG_ID_44_LEN 4 +#define MAVLINK_MSG_ID_44_LEN 5 #define MAVLINK_MSG_ID_44_MIN_LEN 4 #define MAVLINK_MSG_ID_MISSION_COUNT_CRC 221 @@ -24,19 +25,21 @@ typedef struct __mavlink_mission_count_t { #define MAVLINK_MESSAGE_INFO_MISSION_COUNT { \ 44, \ "MISSION_COUNT", \ - 3, \ + 4, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_count_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_count_t, target_component) }, \ { "count", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_count_t, count) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_count_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_COUNT { \ "MISSION_COUNT", \ - 3, \ + 4, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_count_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_count_t, target_component) }, \ { "count", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_count_t, count) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_count_t, mission_type) }, \ } \ } #endif @@ -50,16 +53,18 @@ typedef struct __mavlink_mission_count_t { * @param target_system System ID * @param target_component Component ID * @param count Number of mission items in the sequence + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t count) + uint8_t target_system, uint8_t target_component, uint16_t count, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN]; _mav_put_uint16_t(buf, 0, count); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); #else @@ -67,6 +72,7 @@ static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t packet.count = count; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); #endif @@ -84,17 +90,19 @@ static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t * @param target_system System ID * @param target_component Component ID * @param count Number of mission items in the sequence + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t count) + uint8_t target_system,uint8_t target_component,uint16_t count,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN]; _mav_put_uint16_t(buf, 0, count); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); #else @@ -102,6 +110,7 @@ static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, ui packet.count = count; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); #endif @@ -120,7 +129,7 @@ static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_mission_count_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_count_t* mission_count) { - return mavlink_msg_mission_count_pack(system_id, component_id, msg, mission_count->target_system, mission_count->target_component, mission_count->count); + return mavlink_msg_mission_count_pack(system_id, component_id, msg, mission_count->target_system, mission_count->target_component, mission_count->count, mission_count->mission_type); } /** @@ -134,7 +143,7 @@ static inline uint16_t mavlink_msg_mission_count_encode(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_mission_count_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_count_t* mission_count) { - return mavlink_msg_mission_count_pack_chan(system_id, component_id, chan, msg, mission_count->target_system, mission_count->target_component, mission_count->count); + return mavlink_msg_mission_count_pack_chan(system_id, component_id, chan, msg, mission_count->target_system, mission_count->target_component, mission_count->count, mission_count->mission_type); } /** @@ -144,16 +153,18 @@ static inline uint16_t mavlink_msg_mission_count_encode_chan(uint8_t system_id, * @param target_system System ID * @param target_component Component ID * @param count Number of mission items in the sequence + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) +static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN]; _mav_put_uint16_t(buf, 0, count); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); #else @@ -161,6 +172,7 @@ static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_ packet.count = count; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); #endif @@ -174,7 +186,7 @@ static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_ static inline void mavlink_msg_mission_count_send_struct(mavlink_channel_t chan, const mavlink_mission_count_t* mission_count) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_count_send(chan, mission_count->target_system, mission_count->target_component, mission_count->count); + mavlink_msg_mission_count_send(chan, mission_count->target_system, mission_count->target_component, mission_count->count, mission_count->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)mission_count, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); #endif @@ -188,13 +200,14 @@ static inline void mavlink_msg_mission_count_send_struct(mavlink_channel_t chan, is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_count_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) +static inline void mavlink_msg_mission_count_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint16_t(buf, 0, count); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); #else @@ -202,6 +215,7 @@ static inline void mavlink_msg_mission_count_send_buf(mavlink_message_t *msgbuf, packet->count = count; packet->target_system = target_system; packet->target_component = target_component; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)packet, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); #endif @@ -243,6 +257,16 @@ static inline uint16_t mavlink_msg_mission_count_get_count(const mavlink_message return _MAV_RETURN_uint16_t(msg, 0); } +/** + * @brief Get field mission_type from mission_count message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_count_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + /** * @brief Decode a mission_count message into a struct * @@ -255,6 +279,7 @@ static inline void mavlink_msg_mission_count_decode(const mavlink_message_t* msg mission_count->count = mavlink_msg_mission_count_get_count(msg); mission_count->target_system = mavlink_msg_mission_count_get_target_system(msg); mission_count->target_component = mavlink_msg_mission_count_get_target_component(msg); + mission_count->mission_type = mavlink_msg_mission_count_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_COUNT_LEN? msg->len : MAVLINK_MSG_ID_MISSION_COUNT_LEN; memset(mission_count, 0, MAVLINK_MSG_ID_MISSION_COUNT_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_item.h b/lib/main/MAVLink/common/mavlink_msg_mission_item.h index 0a50b542a5..b14d151b28 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_item.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_item.h @@ -19,11 +19,12 @@ typedef struct __mavlink_mission_item_t { uint8_t frame; /*< The coordinate system of the waypoint.*/ uint8_t current; /*< false:0, true:1*/ uint8_t autocontinue; /*< Autocontinue to next waypoint*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_item_t; -#define MAVLINK_MSG_ID_MISSION_ITEM_LEN 37 +#define MAVLINK_MSG_ID_MISSION_ITEM_LEN 38 #define MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN 37 -#define MAVLINK_MSG_ID_39_LEN 37 +#define MAVLINK_MSG_ID_39_LEN 38 #define MAVLINK_MSG_ID_39_MIN_LEN 37 #define MAVLINK_MSG_ID_MISSION_ITEM_CRC 254 @@ -35,7 +36,7 @@ typedef struct __mavlink_mission_item_t { #define MAVLINK_MESSAGE_INFO_MISSION_ITEM { \ 39, \ "MISSION_ITEM", \ - 14, \ + 15, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_mission_item_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_mission_item_t, target_component) }, \ { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_mission_item_t, seq) }, \ @@ -50,12 +51,13 @@ typedef struct __mavlink_mission_item_t { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mission_item_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mission_item_t, y) }, \ { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mission_item_t, z) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_mission_item_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_ITEM { \ "MISSION_ITEM", \ - 14, \ + 15, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_mission_item_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_mission_item_t, target_component) }, \ { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_mission_item_t, seq) }, \ @@ -70,6 +72,7 @@ typedef struct __mavlink_mission_item_t { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mission_item_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mission_item_t, y) }, \ { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mission_item_t, z) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_mission_item_t, mission_type) }, \ } \ } #endif @@ -94,10 +97,11 @@ typedef struct __mavlink_mission_item_t { * @param x PARAM5 / local: X coordinate, global: latitude * @param y PARAM6 / local: Y coordinate, global: longitude * @param z PARAM7 / local: Z coordinate, global: altitude (relative or absolute, depending on frame). + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) + uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ITEM_LEN]; @@ -115,6 +119,7 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t _mav_put_uint8_t(buf, 34, frame); _mav_put_uint8_t(buf, 35, current); _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN); #else @@ -133,6 +138,7 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t packet.frame = frame; packet.current = current; packet.autocontinue = autocontinue; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN); #endif @@ -161,11 +167,12 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t * @param x PARAM5 / local: X coordinate, global: latitude * @param y PARAM6 / local: Y coordinate, global: longitude * @param z PARAM7 / local: Z coordinate, global: altitude (relative or absolute, depending on frame). + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t frame,uint16_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,float x,float y,float z) + uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t frame,uint16_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,float x,float y,float z,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ITEM_LEN]; @@ -183,6 +190,7 @@ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uin _mav_put_uint8_t(buf, 34, frame); _mav_put_uint8_t(buf, 35, current); _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN); #else @@ -201,6 +209,7 @@ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uin packet.frame = frame; packet.current = current; packet.autocontinue = autocontinue; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN); #endif @@ -219,7 +228,7 @@ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_mission_item_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_item_t* mission_item) { - return mavlink_msg_mission_item_pack(system_id, component_id, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z); + return mavlink_msg_mission_item_pack(system_id, component_id, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z, mission_item->mission_type); } /** @@ -233,7 +242,7 @@ static inline uint16_t mavlink_msg_mission_item_encode(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_mission_item_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_t* mission_item) { - return mavlink_msg_mission_item_pack_chan(system_id, component_id, chan, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z); + return mavlink_msg_mission_item_pack_chan(system_id, component_id, chan, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z, mission_item->mission_type); } /** @@ -254,10 +263,11 @@ static inline uint16_t mavlink_msg_mission_item_encode_chan(uint8_t system_id, u * @param x PARAM5 / local: X coordinate, global: latitude * @param y PARAM6 / local: Y coordinate, global: longitude * @param z PARAM7 / local: Z coordinate, global: altitude (relative or absolute, depending on frame). + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) +static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ITEM_LEN]; @@ -275,6 +285,7 @@ static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t _mav_put_uint8_t(buf, 34, frame); _mav_put_uint8_t(buf, 35, current); _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); #else @@ -293,6 +304,7 @@ static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t packet.frame = frame; packet.current = current; packet.autocontinue = autocontinue; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); #endif @@ -306,7 +318,7 @@ static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t static inline void mavlink_msg_mission_item_send_struct(mavlink_channel_t chan, const mavlink_mission_item_t* mission_item) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_item_send(chan, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z); + mavlink_msg_mission_item_send(chan, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z, mission_item->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)mission_item, MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); #endif @@ -320,7 +332,7 @@ static inline void mavlink_msg_mission_item_send_struct(mavlink_channel_t chan, is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_item_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) +static inline void mavlink_msg_mission_item_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -338,6 +350,7 @@ static inline void mavlink_msg_mission_item_send_buf(mavlink_message_t *msgbuf, _mav_put_uint8_t(buf, 34, frame); _mav_put_uint8_t(buf, 35, current); _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); #else @@ -356,6 +369,7 @@ static inline void mavlink_msg_mission_item_send_buf(mavlink_message_t *msgbuf, packet->frame = frame; packet->current = current; packet->autocontinue = autocontinue; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); #endif @@ -507,6 +521,16 @@ static inline float mavlink_msg_mission_item_get_z(const mavlink_message_t* msg) return _MAV_RETURN_float(msg, 24); } +/** + * @brief Get field mission_type from mission_item message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_item_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 37); +} + /** * @brief Decode a mission_item message into a struct * @@ -530,6 +554,7 @@ static inline void mavlink_msg_mission_item_decode(const mavlink_message_t* msg, mission_item->frame = mavlink_msg_mission_item_get_frame(msg); mission_item->current = mavlink_msg_mission_item_get_current(msg); mission_item->autocontinue = mavlink_msg_mission_item_get_autocontinue(msg); + mission_item->mission_type = mavlink_msg_mission_item_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_ITEM_LEN? msg->len : MAVLINK_MSG_ID_MISSION_ITEM_LEN; memset(mission_item, 0, MAVLINK_MSG_ID_MISSION_ITEM_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_item_int.h b/lib/main/MAVLink/common/mavlink_msg_mission_item_int.h index 64572671c9..74ad3a5ed5 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_item_int.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_item_int.h @@ -19,11 +19,12 @@ typedef struct __mavlink_mission_item_int_t { uint8_t frame; /*< The coordinate system of the waypoint.*/ uint8_t current; /*< false:0, true:1*/ uint8_t autocontinue; /*< Autocontinue to next waypoint*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_item_int_t; -#define MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN 37 +#define MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN 38 #define MAVLINK_MSG_ID_MISSION_ITEM_INT_MIN_LEN 37 -#define MAVLINK_MSG_ID_73_LEN 37 +#define MAVLINK_MSG_ID_73_LEN 38 #define MAVLINK_MSG_ID_73_MIN_LEN 37 #define MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC 38 @@ -35,7 +36,7 @@ typedef struct __mavlink_mission_item_int_t { #define MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT { \ 73, \ "MISSION_ITEM_INT", \ - 14, \ + 15, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_mission_item_int_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_mission_item_int_t, target_component) }, \ { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_mission_item_int_t, seq) }, \ @@ -50,12 +51,13 @@ typedef struct __mavlink_mission_item_int_t { { "x", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_mission_item_int_t, x) }, \ { "y", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_mission_item_int_t, y) }, \ { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mission_item_int_t, z) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_mission_item_int_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT { \ "MISSION_ITEM_INT", \ - 14, \ + 15, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_mission_item_int_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_mission_item_int_t, target_component) }, \ { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_mission_item_int_t, seq) }, \ @@ -70,6 +72,7 @@ typedef struct __mavlink_mission_item_int_t { { "x", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_mission_item_int_t, x) }, \ { "y", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_mission_item_int_t, y) }, \ { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mission_item_int_t, z) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_mission_item_int_t, mission_type) }, \ } \ } #endif @@ -94,10 +97,11 @@ typedef struct __mavlink_mission_item_int_t { * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 * @param y PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_item_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) + uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN]; @@ -115,6 +119,7 @@ static inline uint16_t mavlink_msg_mission_item_int_pack(uint8_t system_id, uint _mav_put_uint8_t(buf, 34, frame); _mav_put_uint8_t(buf, 35, current); _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); #else @@ -133,6 +138,7 @@ static inline uint16_t mavlink_msg_mission_item_int_pack(uint8_t system_id, uint packet.frame = frame; packet.current = current; packet.autocontinue = autocontinue; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); #endif @@ -161,11 +167,12 @@ static inline uint16_t mavlink_msg_mission_item_int_pack(uint8_t system_id, uint * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 * @param y PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_item_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t frame,uint16_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,int32_t x,int32_t y,float z) + uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t frame,uint16_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,int32_t x,int32_t y,float z,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN]; @@ -183,6 +190,7 @@ static inline uint16_t mavlink_msg_mission_item_int_pack_chan(uint8_t system_id, _mav_put_uint8_t(buf, 34, frame); _mav_put_uint8_t(buf, 35, current); _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); #else @@ -201,6 +209,7 @@ static inline uint16_t mavlink_msg_mission_item_int_pack_chan(uint8_t system_id, packet.frame = frame; packet.current = current; packet.autocontinue = autocontinue; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); #endif @@ -219,7 +228,7 @@ static inline uint16_t mavlink_msg_mission_item_int_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_mission_item_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_item_int_t* mission_item_int) { - return mavlink_msg_mission_item_int_pack(system_id, component_id, msg, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z); + return mavlink_msg_mission_item_int_pack(system_id, component_id, msg, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z, mission_item_int->mission_type); } /** @@ -233,7 +242,7 @@ static inline uint16_t mavlink_msg_mission_item_int_encode(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_mission_item_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_int_t* mission_item_int) { - return mavlink_msg_mission_item_int_pack_chan(system_id, component_id, chan, msg, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z); + return mavlink_msg_mission_item_int_pack_chan(system_id, component_id, chan, msg, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z, mission_item_int->mission_type); } /** @@ -254,10 +263,11 @@ static inline uint16_t mavlink_msg_mission_item_int_encode_chan(uint8_t system_i * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 * @param y PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_item_int_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) +static inline void mavlink_msg_mission_item_int_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN]; @@ -275,6 +285,7 @@ static inline void mavlink_msg_mission_item_int_send(mavlink_channel_t chan, uin _mav_put_uint8_t(buf, 34, frame); _mav_put_uint8_t(buf, 35, current); _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); #else @@ -293,6 +304,7 @@ static inline void mavlink_msg_mission_item_int_send(mavlink_channel_t chan, uin packet.frame = frame; packet.current = current; packet.autocontinue = autocontinue; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); #endif @@ -306,7 +318,7 @@ static inline void mavlink_msg_mission_item_int_send(mavlink_channel_t chan, uin static inline void mavlink_msg_mission_item_int_send_struct(mavlink_channel_t chan, const mavlink_mission_item_int_t* mission_item_int) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_item_int_send(chan, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z); + mavlink_msg_mission_item_int_send(chan, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z, mission_item_int->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, (const char *)mission_item_int, MAVLINK_MSG_ID_MISSION_ITEM_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); #endif @@ -320,7 +332,7 @@ static inline void mavlink_msg_mission_item_int_send_struct(mavlink_channel_t ch is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_item_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) +static inline void mavlink_msg_mission_item_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -338,6 +350,7 @@ static inline void mavlink_msg_mission_item_int_send_buf(mavlink_message_t *msgb _mav_put_uint8_t(buf, 34, frame); _mav_put_uint8_t(buf, 35, current); _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); #else @@ -356,6 +369,7 @@ static inline void mavlink_msg_mission_item_int_send_buf(mavlink_message_t *msgb packet->frame = frame; packet->current = current; packet->autocontinue = autocontinue; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); #endif @@ -507,6 +521,16 @@ static inline float mavlink_msg_mission_item_int_get_z(const mavlink_message_t* return _MAV_RETURN_float(msg, 24); } +/** + * @brief Get field mission_type from mission_item_int message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_item_int_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 37); +} + /** * @brief Decode a mission_item_int message into a struct * @@ -530,6 +554,7 @@ static inline void mavlink_msg_mission_item_int_decode(const mavlink_message_t* mission_item_int->frame = mavlink_msg_mission_item_int_get_frame(msg); mission_item_int->current = mavlink_msg_mission_item_int_get_current(msg); mission_item_int->autocontinue = mavlink_msg_mission_item_int_get_autocontinue(msg); + mission_item_int->mission_type = mavlink_msg_mission_item_int_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN? msg->len : MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN; memset(mission_item_int, 0, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_request.h b/lib/main/MAVLink/common/mavlink_msg_mission_request.h index e3b33cc3ec..4f8c0fe77c 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_request.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_request.h @@ -8,11 +8,12 @@ typedef struct __mavlink_mission_request_t { uint16_t seq; /*< Sequence*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_request_t; -#define MAVLINK_MSG_ID_MISSION_REQUEST_LEN 4 +#define MAVLINK_MSG_ID_MISSION_REQUEST_LEN 5 #define MAVLINK_MSG_ID_MISSION_REQUEST_MIN_LEN 4 -#define MAVLINK_MSG_ID_40_LEN 4 +#define MAVLINK_MSG_ID_40_LEN 5 #define MAVLINK_MSG_ID_40_MIN_LEN 4 #define MAVLINK_MSG_ID_MISSION_REQUEST_CRC 230 @@ -24,19 +25,21 @@ typedef struct __mavlink_mission_request_t { #define MAVLINK_MESSAGE_INFO_MISSION_REQUEST { \ 40, \ "MISSION_REQUEST", \ - 3, \ + 4, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_request_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_request_t, target_component) }, \ { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_request_t, seq) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_request_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_REQUEST { \ "MISSION_REQUEST", \ - 3, \ + 4, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_request_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_request_t, target_component) }, \ { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_request_t, seq) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_request_t, mission_type) }, \ } \ } #endif @@ -50,16 +53,18 @@ typedef struct __mavlink_mission_request_t { * @param target_system System ID * @param target_component Component ID * @param seq Sequence + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_request_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t seq) + uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LEN]; _mav_put_uint16_t(buf, 0, seq); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); #else @@ -67,6 +72,7 @@ static inline uint16_t mavlink_msg_mission_request_pack(uint8_t system_id, uint8 packet.seq = seq; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); #endif @@ -84,17 +90,19 @@ static inline uint16_t mavlink_msg_mission_request_pack(uint8_t system_id, uint8 * @param target_system System ID * @param target_component Component ID * @param seq Sequence + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_request_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t seq) + uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LEN]; _mav_put_uint16_t(buf, 0, seq); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); #else @@ -102,6 +110,7 @@ static inline uint16_t mavlink_msg_mission_request_pack_chan(uint8_t system_id, packet.seq = seq; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); #endif @@ -120,7 +129,7 @@ static inline uint16_t mavlink_msg_mission_request_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_mission_request_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_t* mission_request) { - return mavlink_msg_mission_request_pack(system_id, component_id, msg, mission_request->target_system, mission_request->target_component, mission_request->seq); + return mavlink_msg_mission_request_pack(system_id, component_id, msg, mission_request->target_system, mission_request->target_component, mission_request->seq, mission_request->mission_type); } /** @@ -134,7 +143,7 @@ static inline uint16_t mavlink_msg_mission_request_encode(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_mission_request_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_t* mission_request) { - return mavlink_msg_mission_request_pack_chan(system_id, component_id, chan, msg, mission_request->target_system, mission_request->target_component, mission_request->seq); + return mavlink_msg_mission_request_pack_chan(system_id, component_id, chan, msg, mission_request->target_system, mission_request->target_component, mission_request->seq, mission_request->mission_type); } /** @@ -144,16 +153,18 @@ static inline uint16_t mavlink_msg_mission_request_encode_chan(uint8_t system_id * @param target_system System ID * @param target_component Component ID * @param seq Sequence + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +static inline void mavlink_msg_mission_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LEN]; _mav_put_uint16_t(buf, 0, seq); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); #else @@ -161,6 +172,7 @@ static inline void mavlink_msg_mission_request_send(mavlink_channel_t chan, uint packet.seq = seq; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); #endif @@ -174,7 +186,7 @@ static inline void mavlink_msg_mission_request_send(mavlink_channel_t chan, uint static inline void mavlink_msg_mission_request_send_struct(mavlink_channel_t chan, const mavlink_mission_request_t* mission_request) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_request_send(chan, mission_request->target_system, mission_request->target_component, mission_request->seq); + mavlink_msg_mission_request_send(chan, mission_request->target_system, mission_request->target_component, mission_request->seq, mission_request->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)mission_request, MAVLINK_MSG_ID_MISSION_REQUEST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); #endif @@ -188,13 +200,14 @@ static inline void mavlink_msg_mission_request_send_struct(mavlink_channel_t cha is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_request_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +static inline void mavlink_msg_mission_request_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint16_t(buf, 0, seq); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); #else @@ -202,6 +215,7 @@ static inline void mavlink_msg_mission_request_send_buf(mavlink_message_t *msgbu packet->seq = seq; packet->target_system = target_system; packet->target_component = target_component; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); #endif @@ -243,6 +257,16 @@ static inline uint16_t mavlink_msg_mission_request_get_seq(const mavlink_message return _MAV_RETURN_uint16_t(msg, 0); } +/** + * @brief Get field mission_type from mission_request message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_request_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + /** * @brief Decode a mission_request message into a struct * @@ -255,6 +279,7 @@ static inline void mavlink_msg_mission_request_decode(const mavlink_message_t* m mission_request->seq = mavlink_msg_mission_request_get_seq(msg); mission_request->target_system = mavlink_msg_mission_request_get_target_system(msg); mission_request->target_component = mavlink_msg_mission_request_get_target_component(msg); + mission_request->mission_type = mavlink_msg_mission_request_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_REQUEST_LEN? msg->len : MAVLINK_MSG_ID_MISSION_REQUEST_LEN; memset(mission_request, 0, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_request_int.h b/lib/main/MAVLink/common/mavlink_msg_mission_request_int.h index a6c2df9119..c131f78f7b 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_request_int.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_request_int.h @@ -8,11 +8,12 @@ typedef struct __mavlink_mission_request_int_t { uint16_t seq; /*< Sequence*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_request_int_t; -#define MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN 4 +#define MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN 5 #define MAVLINK_MSG_ID_MISSION_REQUEST_INT_MIN_LEN 4 -#define MAVLINK_MSG_ID_51_LEN 4 +#define MAVLINK_MSG_ID_51_LEN 5 #define MAVLINK_MSG_ID_51_MIN_LEN 4 #define MAVLINK_MSG_ID_MISSION_REQUEST_INT_CRC 196 @@ -24,19 +25,21 @@ typedef struct __mavlink_mission_request_int_t { #define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_INT { \ 51, \ "MISSION_REQUEST_INT", \ - 3, \ + 4, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_request_int_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_request_int_t, target_component) }, \ { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_request_int_t, seq) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_request_int_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_INT { \ "MISSION_REQUEST_INT", \ - 3, \ + 4, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_request_int_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_request_int_t, target_component) }, \ { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_request_int_t, seq) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_request_int_t, mission_type) }, \ } \ } #endif @@ -50,16 +53,18 @@ typedef struct __mavlink_mission_request_int_t { * @param target_system System ID * @param target_component Component ID * @param seq Sequence + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_request_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t seq) + uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN]; _mav_put_uint16_t(buf, 0, seq); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN); #else @@ -67,6 +72,7 @@ static inline uint16_t mavlink_msg_mission_request_int_pack(uint8_t system_id, u packet.seq = seq; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN); #endif @@ -84,17 +90,19 @@ static inline uint16_t mavlink_msg_mission_request_int_pack(uint8_t system_id, u * @param target_system System ID * @param target_component Component ID * @param seq Sequence + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_request_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t seq) + uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN]; _mav_put_uint16_t(buf, 0, seq); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN); #else @@ -102,6 +110,7 @@ static inline uint16_t mavlink_msg_mission_request_int_pack_chan(uint8_t system_ packet.seq = seq; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN); #endif @@ -120,7 +129,7 @@ static inline uint16_t mavlink_msg_mission_request_int_pack_chan(uint8_t system_ */ static inline uint16_t mavlink_msg_mission_request_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_int_t* mission_request_int) { - return mavlink_msg_mission_request_int_pack(system_id, component_id, msg, mission_request_int->target_system, mission_request_int->target_component, mission_request_int->seq); + return mavlink_msg_mission_request_int_pack(system_id, component_id, msg, mission_request_int->target_system, mission_request_int->target_component, mission_request_int->seq, mission_request_int->mission_type); } /** @@ -134,7 +143,7 @@ static inline uint16_t mavlink_msg_mission_request_int_encode(uint8_t system_id, */ static inline uint16_t mavlink_msg_mission_request_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_int_t* mission_request_int) { - return mavlink_msg_mission_request_int_pack_chan(system_id, component_id, chan, msg, mission_request_int->target_system, mission_request_int->target_component, mission_request_int->seq); + return mavlink_msg_mission_request_int_pack_chan(system_id, component_id, chan, msg, mission_request_int->target_system, mission_request_int->target_component, mission_request_int->seq, mission_request_int->mission_type); } /** @@ -144,16 +153,18 @@ static inline uint16_t mavlink_msg_mission_request_int_encode_chan(uint8_t syste * @param target_system System ID * @param target_component Component ID * @param seq Sequence + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_request_int_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +static inline void mavlink_msg_mission_request_int_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN]; _mav_put_uint16_t(buf, 0, seq); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_INT, buf, MAVLINK_MSG_ID_MISSION_REQUEST_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_CRC); #else @@ -161,6 +172,7 @@ static inline void mavlink_msg_mission_request_int_send(mavlink_channel_t chan, packet.seq = seq; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_INT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_CRC); #endif @@ -174,7 +186,7 @@ static inline void mavlink_msg_mission_request_int_send(mavlink_channel_t chan, static inline void mavlink_msg_mission_request_int_send_struct(mavlink_channel_t chan, const mavlink_mission_request_int_t* mission_request_int) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_request_int_send(chan, mission_request_int->target_system, mission_request_int->target_component, mission_request_int->seq); + mavlink_msg_mission_request_int_send(chan, mission_request_int->target_system, mission_request_int->target_component, mission_request_int->seq, mission_request_int->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_INT, (const char *)mission_request_int, MAVLINK_MSG_ID_MISSION_REQUEST_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_CRC); #endif @@ -188,13 +200,14 @@ static inline void mavlink_msg_mission_request_int_send_struct(mavlink_channel_t is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_request_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +static inline void mavlink_msg_mission_request_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint16_t(buf, 0, seq); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_INT, buf, MAVLINK_MSG_ID_MISSION_REQUEST_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_CRC); #else @@ -202,6 +215,7 @@ static inline void mavlink_msg_mission_request_int_send_buf(mavlink_message_t *m packet->seq = seq; packet->target_system = target_system; packet->target_component = target_component; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_INT, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_CRC); #endif @@ -243,6 +257,16 @@ static inline uint16_t mavlink_msg_mission_request_int_get_seq(const mavlink_mes return _MAV_RETURN_uint16_t(msg, 0); } +/** + * @brief Get field mission_type from mission_request_int message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_request_int_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + /** * @brief Decode a mission_request_int message into a struct * @@ -255,6 +279,7 @@ static inline void mavlink_msg_mission_request_int_decode(const mavlink_message_ mission_request_int->seq = mavlink_msg_mission_request_int_get_seq(msg); mission_request_int->target_system = mavlink_msg_mission_request_int_get_target_system(msg); mission_request_int->target_component = mavlink_msg_mission_request_int_get_target_component(msg); + mission_request_int->mission_type = mavlink_msg_mission_request_int_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN? msg->len : MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN; memset(mission_request_int, 0, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_request_list.h b/lib/main/MAVLink/common/mavlink_msg_mission_request_list.h index 8b6659fde7..a1633f43f4 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_request_list.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_request_list.h @@ -7,11 +7,12 @@ typedef struct __mavlink_mission_request_list_t { uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_request_list_t; -#define MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN 2 +#define MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN 3 #define MAVLINK_MSG_ID_MISSION_REQUEST_LIST_MIN_LEN 2 -#define MAVLINK_MSG_ID_43_LEN 2 +#define MAVLINK_MSG_ID_43_LEN 3 #define MAVLINK_MSG_ID_43_MIN_LEN 2 #define MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC 132 @@ -23,17 +24,19 @@ typedef struct __mavlink_mission_request_list_t { #define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST { \ 43, \ "MISSION_REQUEST_LIST", \ - 2, \ + 3, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_request_list_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_request_list_t, target_component) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_request_list_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST { \ "MISSION_REQUEST_LIST", \ - 2, \ + 3, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_request_list_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_request_list_t, target_component) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_request_list_t, mission_type) }, \ } \ } #endif @@ -46,21 +49,24 @@ typedef struct __mavlink_mission_request_list_t { * * @param target_system System ID * @param target_component Component ID + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component) + uint8_t target_system, uint8_t target_component, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); #else mavlink_mission_request_list_t packet; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); #endif @@ -77,22 +83,25 @@ static inline uint16_t mavlink_msg_mission_request_list_pack(uint8_t system_id, * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component) + uint8_t target_system,uint8_t target_component,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); #else mavlink_mission_request_list_t packet; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); #endif @@ -111,7 +120,7 @@ static inline uint16_t mavlink_msg_mission_request_list_pack_chan(uint8_t system */ static inline uint16_t mavlink_msg_mission_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_list_t* mission_request_list) { - return mavlink_msg_mission_request_list_pack(system_id, component_id, msg, mission_request_list->target_system, mission_request_list->target_component); + return mavlink_msg_mission_request_list_pack(system_id, component_id, msg, mission_request_list->target_system, mission_request_list->target_component, mission_request_list->mission_type); } /** @@ -125,7 +134,7 @@ static inline uint16_t mavlink_msg_mission_request_list_encode(uint8_t system_id */ static inline uint16_t mavlink_msg_mission_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_list_t* mission_request_list) { - return mavlink_msg_mission_request_list_pack_chan(system_id, component_id, chan, msg, mission_request_list->target_system, mission_request_list->target_component); + return mavlink_msg_mission_request_list_pack_chan(system_id, component_id, chan, msg, mission_request_list->target_system, mission_request_list->target_component, mission_request_list->mission_type); } /** @@ -134,21 +143,24 @@ static inline uint16_t mavlink_msg_mission_request_list_encode_chan(uint8_t syst * * @param target_system System ID * @param target_component Component ID + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +static inline void mavlink_msg_mission_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); #else mavlink_mission_request_list_t packet; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); #endif @@ -162,7 +174,7 @@ static inline void mavlink_msg_mission_request_list_send(mavlink_channel_t chan, static inline void mavlink_msg_mission_request_list_send_struct(mavlink_channel_t chan, const mavlink_mission_request_list_t* mission_request_list) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_request_list_send(chan, mission_request_list->target_system, mission_request_list->target_component); + mavlink_msg_mission_request_list_send(chan, mission_request_list->target_system, mission_request_list->target_component, mission_request_list->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)mission_request_list, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); #endif @@ -176,18 +188,20 @@ static inline void mavlink_msg_mission_request_list_send_struct(mavlink_channel_ is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +static inline void mavlink_msg_mission_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); #else mavlink_mission_request_list_t *packet = (mavlink_mission_request_list_t *)msgbuf; packet->target_system = target_system; packet->target_component = target_component; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); #endif @@ -219,6 +233,16 @@ static inline uint8_t mavlink_msg_mission_request_list_get_target_component(cons return _MAV_RETURN_uint8_t(msg, 1); } +/** + * @brief Get field mission_type from mission_request_list message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_request_list_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + /** * @brief Decode a mission_request_list message into a struct * @@ -230,6 +254,7 @@ static inline void mavlink_msg_mission_request_list_decode(const mavlink_message #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS mission_request_list->target_system = mavlink_msg_mission_request_list_get_target_system(msg); mission_request_list->target_component = mavlink_msg_mission_request_list_get_target_component(msg); + mission_request_list->mission_type = mavlink_msg_mission_request_list_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN? msg->len : MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN; memset(mission_request_list, 0, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_request_partial_list.h b/lib/main/MAVLink/common/mavlink_msg_mission_request_partial_list.h index 0eda74772e..93906934b9 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_request_partial_list.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_request_partial_list.h @@ -9,11 +9,12 @@ typedef struct __mavlink_mission_request_partial_list_t { int16_t end_index; /*< End index, -1 by default (-1: send list to end). Else a valid index of the list*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_request_partial_list_t; -#define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN 6 +#define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN 7 #define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_MIN_LEN 6 -#define MAVLINK_MSG_ID_37_LEN 6 +#define MAVLINK_MSG_ID_37_LEN 7 #define MAVLINK_MSG_ID_37_MIN_LEN 6 #define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC 212 @@ -25,21 +26,23 @@ typedef struct __mavlink_mission_request_partial_list_t { #define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST { \ 37, \ "MISSION_REQUEST_PARTIAL_LIST", \ - 4, \ + 5, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_request_partial_list_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_request_partial_list_t, target_component) }, \ { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_request_partial_list_t, start_index) }, \ { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_request_partial_list_t, end_index) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_mission_request_partial_list_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST { \ "MISSION_REQUEST_PARTIAL_LIST", \ - 4, \ + 5, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_request_partial_list_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_request_partial_list_t, target_component) }, \ { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_request_partial_list_t, start_index) }, \ { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_request_partial_list_t, end_index) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_mission_request_partial_list_t, mission_type) }, \ } \ } #endif @@ -54,10 +57,11 @@ typedef struct __mavlink_mission_request_partial_list_t { * @param target_component Component ID * @param start_index Start index * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) + uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN]; @@ -65,6 +69,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t sys _mav_put_int16_t(buf, 2, end_index); _mav_put_uint8_t(buf, 4, target_system); _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); #else @@ -73,6 +78,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t sys packet.end_index = end_index; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); #endif @@ -91,11 +97,12 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t sys * @param target_component Component ID * @param start_index Start index * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_request_partial_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,int16_t start_index,int16_t end_index) + uint8_t target_system,uint8_t target_component,int16_t start_index,int16_t end_index,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN]; @@ -103,6 +110,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack_chan(uint8_ _mav_put_int16_t(buf, 2, end_index); _mav_put_uint8_t(buf, 4, target_system); _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); #else @@ -111,6 +119,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack_chan(uint8_ packet.end_index = end_index; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); #endif @@ -129,7 +138,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack_chan(uint8_ */ static inline uint16_t mavlink_msg_mission_request_partial_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_partial_list_t* mission_request_partial_list) { - return mavlink_msg_mission_request_partial_list_pack(system_id, component_id, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index); + return mavlink_msg_mission_request_partial_list_pack(system_id, component_id, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index, mission_request_partial_list->mission_type); } /** @@ -143,7 +152,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_encode(uint8_t s */ static inline uint16_t mavlink_msg_mission_request_partial_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_partial_list_t* mission_request_partial_list) { - return mavlink_msg_mission_request_partial_list_pack_chan(system_id, component_id, chan, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index); + return mavlink_msg_mission_request_partial_list_pack_chan(system_id, component_id, chan, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index, mission_request_partial_list->mission_type); } /** @@ -154,10 +163,11 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_encode_chan(uint * @param target_component Component ID * @param start_index Start index * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN]; @@ -165,6 +175,7 @@ static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel _mav_put_int16_t(buf, 2, end_index); _mav_put_uint8_t(buf, 4, target_system); _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); #else @@ -173,6 +184,7 @@ static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel packet.end_index = end_index; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); #endif @@ -186,7 +198,7 @@ static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel static inline void mavlink_msg_mission_request_partial_list_send_struct(mavlink_channel_t chan, const mavlink_mission_request_partial_list_t* mission_request_partial_list) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_request_partial_list_send(chan, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index); + mavlink_msg_mission_request_partial_list_send(chan, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index, mission_request_partial_list->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)mission_request_partial_list, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); #endif @@ -200,7 +212,7 @@ static inline void mavlink_msg_mission_request_partial_list_send_struct(mavlink_ is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_request_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +static inline void mavlink_msg_mission_request_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +220,7 @@ static inline void mavlink_msg_mission_request_partial_list_send_buf(mavlink_mes _mav_put_int16_t(buf, 2, end_index); _mav_put_uint8_t(buf, 4, target_system); _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); #else @@ -216,6 +229,7 @@ static inline void mavlink_msg_mission_request_partial_list_send_buf(mavlink_mes packet->end_index = end_index; packet->target_system = target_system; packet->target_component = target_component; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); #endif @@ -267,6 +281,16 @@ static inline int16_t mavlink_msg_mission_request_partial_list_get_end_index(con return _MAV_RETURN_int16_t(msg, 2); } +/** + * @brief Get field mission_type from mission_request_partial_list message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_request_partial_list_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + /** * @brief Decode a mission_request_partial_list message into a struct * @@ -280,6 +304,7 @@ static inline void mavlink_msg_mission_request_partial_list_decode(const mavlink mission_request_partial_list->end_index = mavlink_msg_mission_request_partial_list_get_end_index(msg); mission_request_partial_list->target_system = mavlink_msg_mission_request_partial_list_get_target_system(msg); mission_request_partial_list->target_component = mavlink_msg_mission_request_partial_list_get_target_component(msg); + mission_request_partial_list->mission_type = mavlink_msg_mission_request_partial_list_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN? msg->len : MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN; memset(mission_request_partial_list, 0, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_write_partial_list.h b/lib/main/MAVLink/common/mavlink_msg_mission_write_partial_list.h index c1ede1b215..9f8bd08f8c 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_write_partial_list.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_write_partial_list.h @@ -9,11 +9,12 @@ typedef struct __mavlink_mission_write_partial_list_t { int16_t end_index; /*< End index, equal or greater than start index.*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_write_partial_list_t; -#define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN 6 +#define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN 7 #define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_MIN_LEN 6 -#define MAVLINK_MSG_ID_38_LEN 6 +#define MAVLINK_MSG_ID_38_LEN 7 #define MAVLINK_MSG_ID_38_MIN_LEN 6 #define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC 9 @@ -25,21 +26,23 @@ typedef struct __mavlink_mission_write_partial_list_t { #define MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST { \ 38, \ "MISSION_WRITE_PARTIAL_LIST", \ - 4, \ + 5, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_write_partial_list_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_write_partial_list_t, target_component) }, \ { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_write_partial_list_t, start_index) }, \ { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_write_partial_list_t, end_index) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_mission_write_partial_list_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST { \ "MISSION_WRITE_PARTIAL_LIST", \ - 4, \ + 5, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_write_partial_list_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_write_partial_list_t, target_component) }, \ { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_write_partial_list_t, start_index) }, \ { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_write_partial_list_t, end_index) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_mission_write_partial_list_t, mission_type) }, \ } \ } #endif @@ -54,10 +57,11 @@ typedef struct __mavlink_mission_write_partial_list_t { * @param target_component Component ID * @param start_index Start index. Must be smaller / equal to the largest index of the current onboard list. * @param end_index End index, equal or greater than start index. + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) + uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN]; @@ -65,6 +69,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t syste _mav_put_int16_t(buf, 2, end_index); _mav_put_uint8_t(buf, 4, target_system); _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); #else @@ -73,6 +78,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t syste packet.end_index = end_index; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); #endif @@ -91,11 +97,12 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t syste * @param target_component Component ID * @param start_index Start index. Must be smaller / equal to the largest index of the current onboard list. * @param end_index End index, equal or greater than start index. + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_write_partial_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,int16_t start_index,int16_t end_index) + uint8_t target_system,uint8_t target_component,int16_t start_index,int16_t end_index,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN]; @@ -103,6 +110,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack_chan(uint8_t _mav_put_int16_t(buf, 2, end_index); _mav_put_uint8_t(buf, 4, target_system); _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); #else @@ -111,6 +119,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack_chan(uint8_t packet.end_index = end_index; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); #endif @@ -129,7 +138,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack_chan(uint8_t */ static inline uint16_t mavlink_msg_mission_write_partial_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_write_partial_list_t* mission_write_partial_list) { - return mavlink_msg_mission_write_partial_list_pack(system_id, component_id, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index); + return mavlink_msg_mission_write_partial_list_pack(system_id, component_id, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index, mission_write_partial_list->mission_type); } /** @@ -143,7 +152,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_encode(uint8_t sys */ static inline uint16_t mavlink_msg_mission_write_partial_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_write_partial_list_t* mission_write_partial_list) { - return mavlink_msg_mission_write_partial_list_pack_chan(system_id, component_id, chan, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index); + return mavlink_msg_mission_write_partial_list_pack_chan(system_id, component_id, chan, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index, mission_write_partial_list->mission_type); } /** @@ -154,10 +163,11 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_encode_chan(uint8_ * @param target_component Component ID * @param start_index Start index. Must be smaller / equal to the largest index of the current onboard list. * @param end_index End index, equal or greater than start index. + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN]; @@ -165,6 +175,7 @@ static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t _mav_put_int16_t(buf, 2, end_index); _mav_put_uint8_t(buf, 4, target_system); _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); #else @@ -173,6 +184,7 @@ static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t packet.end_index = end_index; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); #endif @@ -186,7 +198,7 @@ static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t static inline void mavlink_msg_mission_write_partial_list_send_struct(mavlink_channel_t chan, const mavlink_mission_write_partial_list_t* mission_write_partial_list) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_write_partial_list_send(chan, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index); + mavlink_msg_mission_write_partial_list_send(chan, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index, mission_write_partial_list->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)mission_write_partial_list, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); #endif @@ -200,7 +212,7 @@ static inline void mavlink_msg_mission_write_partial_list_send_struct(mavlink_ch is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_write_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +static inline void mavlink_msg_mission_write_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +220,7 @@ static inline void mavlink_msg_mission_write_partial_list_send_buf(mavlink_messa _mav_put_int16_t(buf, 2, end_index); _mav_put_uint8_t(buf, 4, target_system); _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); #else @@ -216,6 +229,7 @@ static inline void mavlink_msg_mission_write_partial_list_send_buf(mavlink_messa packet->end_index = end_index; packet->target_system = target_system; packet->target_component = target_component; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); #endif @@ -267,6 +281,16 @@ static inline int16_t mavlink_msg_mission_write_partial_list_get_end_index(const return _MAV_RETURN_int16_t(msg, 2); } +/** + * @brief Get field mission_type from mission_write_partial_list message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_write_partial_list_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + /** * @brief Decode a mission_write_partial_list message into a struct * @@ -280,6 +304,7 @@ static inline void mavlink_msg_mission_write_partial_list_decode(const mavlink_m mission_write_partial_list->end_index = mavlink_msg_mission_write_partial_list_get_end_index(msg); mission_write_partial_list->target_system = mavlink_msg_mission_write_partial_list_get_target_system(msg); mission_write_partial_list->target_component = mavlink_msg_mission_write_partial_list_get_target_component(msg); + mission_write_partial_list->mission_type = mavlink_msg_mission_write_partial_list_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN? msg->len : MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN; memset(mission_write_partial_list, 0, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mount_orientation.h b/lib/main/MAVLink/common/mavlink_msg_mount_orientation.h new file mode 100644 index 0000000000..96bfda2501 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_mount_orientation.h @@ -0,0 +1,313 @@ +#pragma once +// MESSAGE MOUNT_ORIENTATION PACKING + +#define MAVLINK_MSG_ID_MOUNT_ORIENTATION 265 + + +typedef struct __mavlink_mount_orientation_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + float roll; /*< [deg] Roll in global frame (set to NaN for invalid).*/ + float pitch; /*< [deg] Pitch in global frame (set to NaN for invalid).*/ + float yaw; /*< [deg] Yaw relative to vehicle (set to NaN for invalid).*/ + float yaw_absolute; /*< [deg] Yaw in absolute frame relative to Earth's North, north is 0 (set to NaN for invalid).*/ +} mavlink_mount_orientation_t; + +#define MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN 20 +#define MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN 16 +#define MAVLINK_MSG_ID_265_LEN 20 +#define MAVLINK_MSG_ID_265_MIN_LEN 16 + +#define MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC 26 +#define MAVLINK_MSG_ID_265_CRC 26 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_MOUNT_ORIENTATION { \ + 265, \ + "MOUNT_ORIENTATION", \ + 5, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_mount_orientation_t, time_boot_ms) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mount_orientation_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mount_orientation_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mount_orientation_t, yaw) }, \ + { "yaw_absolute", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mount_orientation_t, yaw_absolute) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_MOUNT_ORIENTATION { \ + "MOUNT_ORIENTATION", \ + 5, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_mount_orientation_t, time_boot_ms) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mount_orientation_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mount_orientation_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mount_orientation_t, yaw) }, \ + { "yaw_absolute", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mount_orientation_t, yaw_absolute) }, \ + } \ +} +#endif + +/** + * @brief Pack a mount_orientation message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param roll [deg] Roll in global frame (set to NaN for invalid). + * @param pitch [deg] Pitch in global frame (set to NaN for invalid). + * @param yaw [deg] Yaw relative to vehicle (set to NaN for invalid). + * @param yaw_absolute [deg] Yaw in absolute frame relative to Earth's North, north is 0 (set to NaN for invalid). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mount_orientation_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, float roll, float pitch, float yaw, float yaw_absolute) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, yaw_absolute); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN); +#else + mavlink_mount_orientation_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.yaw_absolute = yaw_absolute; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_ORIENTATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC); +} + +/** + * @brief Pack a mount_orientation message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param roll [deg] Roll in global frame (set to NaN for invalid). + * @param pitch [deg] Pitch in global frame (set to NaN for invalid). + * @param yaw [deg] Yaw relative to vehicle (set to NaN for invalid). + * @param yaw_absolute [deg] Yaw in absolute frame relative to Earth's North, north is 0 (set to NaN for invalid). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mount_orientation_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,float roll,float pitch,float yaw,float yaw_absolute) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, yaw_absolute); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN); +#else + mavlink_mount_orientation_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.yaw_absolute = yaw_absolute; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_ORIENTATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC); +} + +/** + * @brief Encode a mount_orientation struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mount_orientation C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_orientation_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mount_orientation_t* mount_orientation) +{ + return mavlink_msg_mount_orientation_pack(system_id, component_id, msg, mount_orientation->time_boot_ms, mount_orientation->roll, mount_orientation->pitch, mount_orientation->yaw, mount_orientation->yaw_absolute); +} + +/** + * @brief Encode a mount_orientation struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mount_orientation C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_orientation_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_orientation_t* mount_orientation) +{ + return mavlink_msg_mount_orientation_pack_chan(system_id, component_id, chan, msg, mount_orientation->time_boot_ms, mount_orientation->roll, mount_orientation->pitch, mount_orientation->yaw, mount_orientation->yaw_absolute); +} + +/** + * @brief Send a mount_orientation message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param roll [deg] Roll in global frame (set to NaN for invalid). + * @param pitch [deg] Pitch in global frame (set to NaN for invalid). + * @param yaw [deg] Yaw relative to vehicle (set to NaN for invalid). + * @param yaw_absolute [deg] Yaw in absolute frame relative to Earth's North, north is 0 (set to NaN for invalid). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mount_orientation_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float yaw_absolute) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, yaw_absolute); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_ORIENTATION, buf, MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC); +#else + mavlink_mount_orientation_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.yaw_absolute = yaw_absolute; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_ORIENTATION, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC); +#endif +} + +/** + * @brief Send a mount_orientation message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_mount_orientation_send_struct(mavlink_channel_t chan, const mavlink_mount_orientation_t* mount_orientation) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_mount_orientation_send(chan, mount_orientation->time_boot_ms, mount_orientation->roll, mount_orientation->pitch, mount_orientation->yaw, mount_orientation->yaw_absolute); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_ORIENTATION, (const char *)mount_orientation, MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mount_orientation_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float yaw_absolute) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, yaw_absolute); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_ORIENTATION, buf, MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC); +#else + mavlink_mount_orientation_t *packet = (mavlink_mount_orientation_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->yaw_absolute = yaw_absolute; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_ORIENTATION, (const char *)packet, MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE MOUNT_ORIENTATION UNPACKING + + +/** + * @brief Get field time_boot_ms from mount_orientation message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_mount_orientation_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field roll from mount_orientation message + * + * @return [deg] Roll in global frame (set to NaN for invalid). + */ +static inline float mavlink_msg_mount_orientation_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field pitch from mount_orientation message + * + * @return [deg] Pitch in global frame (set to NaN for invalid). + */ +static inline float mavlink_msg_mount_orientation_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field yaw from mount_orientation message + * + * @return [deg] Yaw relative to vehicle (set to NaN for invalid). + */ +static inline float mavlink_msg_mount_orientation_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field yaw_absolute from mount_orientation message + * + * @return [deg] Yaw in absolute frame relative to Earth's North, north is 0 (set to NaN for invalid). + */ +static inline float mavlink_msg_mount_orientation_get_yaw_absolute(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Decode a mount_orientation message into a struct + * + * @param msg The message to decode + * @param mount_orientation C-struct to decode the message contents into + */ +static inline void mavlink_msg_mount_orientation_decode(const mavlink_message_t* msg, mavlink_mount_orientation_t* mount_orientation) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mount_orientation->time_boot_ms = mavlink_msg_mount_orientation_get_time_boot_ms(msg); + mount_orientation->roll = mavlink_msg_mount_orientation_get_roll(msg); + mount_orientation->pitch = mavlink_msg_mount_orientation_get_pitch(msg); + mount_orientation->yaw = mavlink_msg_mount_orientation_get_yaw(msg); + mount_orientation->yaw_absolute = mavlink_msg_mount_orientation_get_yaw_absolute(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN? msg->len : MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN; + memset(mount_orientation, 0, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN); + memcpy(mount_orientation, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_obstacle_distance.h b/lib/main/MAVLink/common/mavlink_msg_obstacle_distance.h new file mode 100644 index 0000000000..641f2627fe --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_obstacle_distance.h @@ -0,0 +1,405 @@ +#pragma once +// MESSAGE OBSTACLE_DISTANCE PACKING + +#define MAVLINK_MSG_ID_OBSTACLE_DISTANCE 330 + +MAVPACKED( +typedef struct __mavlink_obstacle_distance_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + uint16_t distances[72]; /*< [cm] Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm.*/ + uint16_t min_distance; /*< [cm] Minimum distance the sensor can measure.*/ + uint16_t max_distance; /*< [cm] Maximum distance the sensor can measure.*/ + uint8_t sensor_type; /*< Class id of the distance sensor type.*/ + uint8_t increment; /*< [deg] Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero.*/ + float increment_f; /*< [deg] Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise.*/ + float angle_offset; /*< [deg] Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise.*/ + uint8_t frame; /*< Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned.*/ +}) mavlink_obstacle_distance_t; + +#define MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN 167 +#define MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN 158 +#define MAVLINK_MSG_ID_330_LEN 167 +#define MAVLINK_MSG_ID_330_MIN_LEN 158 + +#define MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC 23 +#define MAVLINK_MSG_ID_330_CRC 23 + +#define MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN 72 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OBSTACLE_DISTANCE { \ + 330, \ + "OBSTACLE_DISTANCE", \ + 9, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_obstacle_distance_t, time_usec) }, \ + { "sensor_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 156, offsetof(mavlink_obstacle_distance_t, sensor_type) }, \ + { "distances", NULL, MAVLINK_TYPE_UINT16_T, 72, 8, offsetof(mavlink_obstacle_distance_t, distances) }, \ + { "increment", NULL, MAVLINK_TYPE_UINT8_T, 0, 157, offsetof(mavlink_obstacle_distance_t, increment) }, \ + { "min_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 152, offsetof(mavlink_obstacle_distance_t, min_distance) }, \ + { "max_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 154, offsetof(mavlink_obstacle_distance_t, max_distance) }, \ + { "increment_f", NULL, MAVLINK_TYPE_FLOAT, 0, 158, offsetof(mavlink_obstacle_distance_t, increment_f) }, \ + { "angle_offset", NULL, MAVLINK_TYPE_FLOAT, 0, 162, offsetof(mavlink_obstacle_distance_t, angle_offset) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 166, offsetof(mavlink_obstacle_distance_t, frame) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OBSTACLE_DISTANCE { \ + "OBSTACLE_DISTANCE", \ + 9, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_obstacle_distance_t, time_usec) }, \ + { "sensor_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 156, offsetof(mavlink_obstacle_distance_t, sensor_type) }, \ + { "distances", NULL, MAVLINK_TYPE_UINT16_T, 72, 8, offsetof(mavlink_obstacle_distance_t, distances) }, \ + { "increment", NULL, MAVLINK_TYPE_UINT8_T, 0, 157, offsetof(mavlink_obstacle_distance_t, increment) }, \ + { "min_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 152, offsetof(mavlink_obstacle_distance_t, min_distance) }, \ + { "max_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 154, offsetof(mavlink_obstacle_distance_t, max_distance) }, \ + { "increment_f", NULL, MAVLINK_TYPE_FLOAT, 0, 158, offsetof(mavlink_obstacle_distance_t, increment_f) }, \ + { "angle_offset", NULL, MAVLINK_TYPE_FLOAT, 0, 162, offsetof(mavlink_obstacle_distance_t, angle_offset) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 166, offsetof(mavlink_obstacle_distance_t, frame) }, \ + } \ +} +#endif + +/** + * @brief Pack a obstacle_distance message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param sensor_type Class id of the distance sensor type. + * @param distances [cm] Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm. + * @param increment [deg] Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero. + * @param min_distance [cm] Minimum distance the sensor can measure. + * @param max_distance [cm] Maximum distance the sensor can measure. + * @param increment_f [deg] Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise. + * @param angle_offset [deg] Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise. + * @param frame Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_obstacle_distance_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t sensor_type, const uint16_t *distances, uint8_t increment, uint16_t min_distance, uint16_t max_distance, float increment_f, float angle_offset, uint8_t frame) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 152, min_distance); + _mav_put_uint16_t(buf, 154, max_distance); + _mav_put_uint8_t(buf, 156, sensor_type); + _mav_put_uint8_t(buf, 157, increment); + _mav_put_float(buf, 158, increment_f); + _mav_put_float(buf, 162, angle_offset); + _mav_put_uint8_t(buf, 166, frame); + _mav_put_uint16_t_array(buf, 8, distances, 72); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN); +#else + mavlink_obstacle_distance_t packet; + packet.time_usec = time_usec; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.sensor_type = sensor_type; + packet.increment = increment; + packet.increment_f = increment_f; + packet.angle_offset = angle_offset; + packet.frame = frame; + mav_array_memcpy(packet.distances, distances, sizeof(uint16_t)*72); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OBSTACLE_DISTANCE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); +} + +/** + * @brief Pack a obstacle_distance message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param sensor_type Class id of the distance sensor type. + * @param distances [cm] Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm. + * @param increment [deg] Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero. + * @param min_distance [cm] Minimum distance the sensor can measure. + * @param max_distance [cm] Maximum distance the sensor can measure. + * @param increment_f [deg] Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise. + * @param angle_offset [deg] Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise. + * @param frame Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_obstacle_distance_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t sensor_type,const uint16_t *distances,uint8_t increment,uint16_t min_distance,uint16_t max_distance,float increment_f,float angle_offset,uint8_t frame) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 152, min_distance); + _mav_put_uint16_t(buf, 154, max_distance); + _mav_put_uint8_t(buf, 156, sensor_type); + _mav_put_uint8_t(buf, 157, increment); + _mav_put_float(buf, 158, increment_f); + _mav_put_float(buf, 162, angle_offset); + _mav_put_uint8_t(buf, 166, frame); + _mav_put_uint16_t_array(buf, 8, distances, 72); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN); +#else + mavlink_obstacle_distance_t packet; + packet.time_usec = time_usec; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.sensor_type = sensor_type; + packet.increment = increment; + packet.increment_f = increment_f; + packet.angle_offset = angle_offset; + packet.frame = frame; + mav_array_memcpy(packet.distances, distances, sizeof(uint16_t)*72); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OBSTACLE_DISTANCE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); +} + +/** + * @brief Encode a obstacle_distance struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param obstacle_distance C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obstacle_distance_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obstacle_distance_t* obstacle_distance) +{ + return mavlink_msg_obstacle_distance_pack(system_id, component_id, msg, obstacle_distance->time_usec, obstacle_distance->sensor_type, obstacle_distance->distances, obstacle_distance->increment, obstacle_distance->min_distance, obstacle_distance->max_distance, obstacle_distance->increment_f, obstacle_distance->angle_offset, obstacle_distance->frame); +} + +/** + * @brief Encode a obstacle_distance struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obstacle_distance C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obstacle_distance_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obstacle_distance_t* obstacle_distance) +{ + return mavlink_msg_obstacle_distance_pack_chan(system_id, component_id, chan, msg, obstacle_distance->time_usec, obstacle_distance->sensor_type, obstacle_distance->distances, obstacle_distance->increment, obstacle_distance->min_distance, obstacle_distance->max_distance, obstacle_distance->increment_f, obstacle_distance->angle_offset, obstacle_distance->frame); +} + +/** + * @brief Send a obstacle_distance message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param sensor_type Class id of the distance sensor type. + * @param distances [cm] Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm. + * @param increment [deg] Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero. + * @param min_distance [cm] Minimum distance the sensor can measure. + * @param max_distance [cm] Maximum distance the sensor can measure. + * @param increment_f [deg] Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise. + * @param angle_offset [deg] Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise. + * @param frame Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_obstacle_distance_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_type, const uint16_t *distances, uint8_t increment, uint16_t min_distance, uint16_t max_distance, float increment_f, float angle_offset, uint8_t frame) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 152, min_distance); + _mav_put_uint16_t(buf, 154, max_distance); + _mav_put_uint8_t(buf, 156, sensor_type); + _mav_put_uint8_t(buf, 157, increment); + _mav_put_float(buf, 158, increment_f); + _mav_put_float(buf, 162, angle_offset); + _mav_put_uint8_t(buf, 166, frame); + _mav_put_uint16_t_array(buf, 8, distances, 72); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE, buf, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); +#else + mavlink_obstacle_distance_t packet; + packet.time_usec = time_usec; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.sensor_type = sensor_type; + packet.increment = increment; + packet.increment_f = increment_f; + packet.angle_offset = angle_offset; + packet.frame = frame; + mav_array_memcpy(packet.distances, distances, sizeof(uint16_t)*72); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE, (const char *)&packet, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); +#endif +} + +/** + * @brief Send a obstacle_distance message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_obstacle_distance_send_struct(mavlink_channel_t chan, const mavlink_obstacle_distance_t* obstacle_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_obstacle_distance_send(chan, obstacle_distance->time_usec, obstacle_distance->sensor_type, obstacle_distance->distances, obstacle_distance->increment, obstacle_distance->min_distance, obstacle_distance->max_distance, obstacle_distance->increment_f, obstacle_distance->angle_offset, obstacle_distance->frame); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE, (const char *)obstacle_distance, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obstacle_distance_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_type, const uint16_t *distances, uint8_t increment, uint16_t min_distance, uint16_t max_distance, float increment_f, float angle_offset, uint8_t frame) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 152, min_distance); + _mav_put_uint16_t(buf, 154, max_distance); + _mav_put_uint8_t(buf, 156, sensor_type); + _mav_put_uint8_t(buf, 157, increment); + _mav_put_float(buf, 158, increment_f); + _mav_put_float(buf, 162, angle_offset); + _mav_put_uint8_t(buf, 166, frame); + _mav_put_uint16_t_array(buf, 8, distances, 72); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE, buf, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); +#else + mavlink_obstacle_distance_t *packet = (mavlink_obstacle_distance_t *)msgbuf; + packet->time_usec = time_usec; + packet->min_distance = min_distance; + packet->max_distance = max_distance; + packet->sensor_type = sensor_type; + packet->increment = increment; + packet->increment_f = increment_f; + packet->angle_offset = angle_offset; + packet->frame = frame; + mav_array_memcpy(packet->distances, distances, sizeof(uint16_t)*72); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE, (const char *)packet, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OBSTACLE_DISTANCE UNPACKING + + +/** + * @brief Get field time_usec from obstacle_distance message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_obstacle_distance_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field sensor_type from obstacle_distance message + * + * @return Class id of the distance sensor type. + */ +static inline uint8_t mavlink_msg_obstacle_distance_get_sensor_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 156); +} + +/** + * @brief Get field distances from obstacle_distance message + * + * @return [cm] Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm. + */ +static inline uint16_t mavlink_msg_obstacle_distance_get_distances(const mavlink_message_t* msg, uint16_t *distances) +{ + return _MAV_RETURN_uint16_t_array(msg, distances, 72, 8); +} + +/** + * @brief Get field increment from obstacle_distance message + * + * @return [deg] Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero. + */ +static inline uint8_t mavlink_msg_obstacle_distance_get_increment(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 157); +} + +/** + * @brief Get field min_distance from obstacle_distance message + * + * @return [cm] Minimum distance the sensor can measure. + */ +static inline uint16_t mavlink_msg_obstacle_distance_get_min_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 152); +} + +/** + * @brief Get field max_distance from obstacle_distance message + * + * @return [cm] Maximum distance the sensor can measure. + */ +static inline uint16_t mavlink_msg_obstacle_distance_get_max_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 154); +} + +/** + * @brief Get field increment_f from obstacle_distance message + * + * @return [deg] Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise. + */ +static inline float mavlink_msg_obstacle_distance_get_increment_f(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 158); +} + +/** + * @brief Get field angle_offset from obstacle_distance message + * + * @return [deg] Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise. + */ +static inline float mavlink_msg_obstacle_distance_get_angle_offset(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 162); +} + +/** + * @brief Get field frame from obstacle_distance message + * + * @return Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned. + */ +static inline uint8_t mavlink_msg_obstacle_distance_get_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 166); +} + +/** + * @brief Decode a obstacle_distance message into a struct + * + * @param msg The message to decode + * @param obstacle_distance C-struct to decode the message contents into + */ +static inline void mavlink_msg_obstacle_distance_decode(const mavlink_message_t* msg, mavlink_obstacle_distance_t* obstacle_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + obstacle_distance->time_usec = mavlink_msg_obstacle_distance_get_time_usec(msg); + mavlink_msg_obstacle_distance_get_distances(msg, obstacle_distance->distances); + obstacle_distance->min_distance = mavlink_msg_obstacle_distance_get_min_distance(msg); + obstacle_distance->max_distance = mavlink_msg_obstacle_distance_get_max_distance(msg); + obstacle_distance->sensor_type = mavlink_msg_obstacle_distance_get_sensor_type(msg); + obstacle_distance->increment = mavlink_msg_obstacle_distance_get_increment(msg); + obstacle_distance->increment_f = mavlink_msg_obstacle_distance_get_increment_f(msg); + obstacle_distance->angle_offset = mavlink_msg_obstacle_distance_get_angle_offset(msg); + obstacle_distance->frame = mavlink_msg_obstacle_distance_get_frame(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN? msg->len : MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN; + memset(obstacle_distance, 0, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN); + memcpy(obstacle_distance, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_odometry.h b/lib/main/MAVLink/common/mavlink_msg_odometry.h new file mode 100644 index 0000000000..93869b7916 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_odometry.h @@ -0,0 +1,607 @@ +#pragma once +// MESSAGE ODOMETRY PACKING + +#define MAVLINK_MSG_ID_ODOMETRY 331 + + +typedef struct __mavlink_odometry_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + float x; /*< [m] X Position*/ + float y; /*< [m] Y Position*/ + float z; /*< [m] Z Position*/ + float q[4]; /*< Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation)*/ + float vx; /*< [m/s] X linear speed*/ + float vy; /*< [m/s] Y linear speed*/ + float vz; /*< [m/s] Z linear speed*/ + float rollspeed; /*< [rad/s] Roll angular speed*/ + float pitchspeed; /*< [rad/s] Pitch angular speed*/ + float yawspeed; /*< [rad/s] Yaw angular speed*/ + float pose_covariance[21]; /*< Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.*/ + float velocity_covariance[21]; /*< Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.*/ + uint8_t frame_id; /*< Coordinate frame of reference for the pose data.*/ + uint8_t child_frame_id; /*< Coordinate frame of reference for the velocity in free space (twist) data.*/ + uint8_t reset_counter; /*< Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps.*/ + uint8_t estimator_type; /*< Type of estimator that is providing the odometry.*/ +} mavlink_odometry_t; + +#define MAVLINK_MSG_ID_ODOMETRY_LEN 232 +#define MAVLINK_MSG_ID_ODOMETRY_MIN_LEN 230 +#define MAVLINK_MSG_ID_331_LEN 232 +#define MAVLINK_MSG_ID_331_MIN_LEN 230 + +#define MAVLINK_MSG_ID_ODOMETRY_CRC 91 +#define MAVLINK_MSG_ID_331_CRC 91 + +#define MAVLINK_MSG_ODOMETRY_FIELD_Q_LEN 4 +#define MAVLINK_MSG_ODOMETRY_FIELD_POSE_COVARIANCE_LEN 21 +#define MAVLINK_MSG_ODOMETRY_FIELD_VELOCITY_COVARIANCE_LEN 21 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_ODOMETRY { \ + 331, \ + "ODOMETRY", \ + 17, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_odometry_t, time_usec) }, \ + { "frame_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 228, offsetof(mavlink_odometry_t, frame_id) }, \ + { "child_frame_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 229, offsetof(mavlink_odometry_t, child_frame_id) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_odometry_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_odometry_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_odometry_t, z) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 20, offsetof(mavlink_odometry_t, q) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_odometry_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_odometry_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_odometry_t, vz) }, \ + { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_odometry_t, rollspeed) }, \ + { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_odometry_t, pitchspeed) }, \ + { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_odometry_t, yawspeed) }, \ + { "pose_covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 60, offsetof(mavlink_odometry_t, pose_covariance) }, \ + { "velocity_covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 144, offsetof(mavlink_odometry_t, velocity_covariance) }, \ + { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 230, offsetof(mavlink_odometry_t, reset_counter) }, \ + { "estimator_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 231, offsetof(mavlink_odometry_t, estimator_type) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_ODOMETRY { \ + "ODOMETRY", \ + 17, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_odometry_t, time_usec) }, \ + { "frame_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 228, offsetof(mavlink_odometry_t, frame_id) }, \ + { "child_frame_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 229, offsetof(mavlink_odometry_t, child_frame_id) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_odometry_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_odometry_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_odometry_t, z) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 20, offsetof(mavlink_odometry_t, q) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_odometry_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_odometry_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_odometry_t, vz) }, \ + { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_odometry_t, rollspeed) }, \ + { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_odometry_t, pitchspeed) }, \ + { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_odometry_t, yawspeed) }, \ + { "pose_covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 60, offsetof(mavlink_odometry_t, pose_covariance) }, \ + { "velocity_covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 144, offsetof(mavlink_odometry_t, velocity_covariance) }, \ + { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 230, offsetof(mavlink_odometry_t, reset_counter) }, \ + { "estimator_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 231, offsetof(mavlink_odometry_t, estimator_type) }, \ + } \ +} +#endif + +/** + * @brief Pack a odometry message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param frame_id Coordinate frame of reference for the pose data. + * @param child_frame_id Coordinate frame of reference for the velocity in free space (twist) data. + * @param x [m] X Position + * @param y [m] Y Position + * @param z [m] Z Position + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + * @param vx [m/s] X linear speed + * @param vy [m/s] Y linear speed + * @param vz [m/s] Z linear speed + * @param rollspeed [rad/s] Roll angular speed + * @param pitchspeed [rad/s] Pitch angular speed + * @param yawspeed [rad/s] Yaw angular speed + * @param pose_covariance Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param velocity_covariance Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + * @param estimator_type Type of estimator that is providing the odometry. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_odometry_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t frame_id, uint8_t child_frame_id, float x, float y, float z, const float *q, float vx, float vy, float vz, float rollspeed, float pitchspeed, float yawspeed, const float *pose_covariance, const float *velocity_covariance, uint8_t reset_counter, uint8_t estimator_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ODOMETRY_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 36, vx); + _mav_put_float(buf, 40, vy); + _mav_put_float(buf, 44, vz); + _mav_put_float(buf, 48, rollspeed); + _mav_put_float(buf, 52, pitchspeed); + _mav_put_float(buf, 56, yawspeed); + _mav_put_uint8_t(buf, 228, frame_id); + _mav_put_uint8_t(buf, 229, child_frame_id); + _mav_put_uint8_t(buf, 230, reset_counter); + _mav_put_uint8_t(buf, 231, estimator_type); + _mav_put_float_array(buf, 20, q, 4); + _mav_put_float_array(buf, 60, pose_covariance, 21); + _mav_put_float_array(buf, 144, velocity_covariance, 21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ODOMETRY_LEN); +#else + mavlink_odometry_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.frame_id = frame_id; + packet.child_frame_id = child_frame_id; + packet.reset_counter = reset_counter; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.pose_covariance, pose_covariance, sizeof(float)*21); + mav_array_memcpy(packet.velocity_covariance, velocity_covariance, sizeof(float)*21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ODOMETRY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ODOMETRY; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); +} + +/** + * @brief Pack a odometry message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param frame_id Coordinate frame of reference for the pose data. + * @param child_frame_id Coordinate frame of reference for the velocity in free space (twist) data. + * @param x [m] X Position + * @param y [m] Y Position + * @param z [m] Z Position + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + * @param vx [m/s] X linear speed + * @param vy [m/s] Y linear speed + * @param vz [m/s] Z linear speed + * @param rollspeed [rad/s] Roll angular speed + * @param pitchspeed [rad/s] Pitch angular speed + * @param yawspeed [rad/s] Yaw angular speed + * @param pose_covariance Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param velocity_covariance Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + * @param estimator_type Type of estimator that is providing the odometry. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_odometry_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t frame_id,uint8_t child_frame_id,float x,float y,float z,const float *q,float vx,float vy,float vz,float rollspeed,float pitchspeed,float yawspeed,const float *pose_covariance,const float *velocity_covariance,uint8_t reset_counter,uint8_t estimator_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ODOMETRY_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 36, vx); + _mav_put_float(buf, 40, vy); + _mav_put_float(buf, 44, vz); + _mav_put_float(buf, 48, rollspeed); + _mav_put_float(buf, 52, pitchspeed); + _mav_put_float(buf, 56, yawspeed); + _mav_put_uint8_t(buf, 228, frame_id); + _mav_put_uint8_t(buf, 229, child_frame_id); + _mav_put_uint8_t(buf, 230, reset_counter); + _mav_put_uint8_t(buf, 231, estimator_type); + _mav_put_float_array(buf, 20, q, 4); + _mav_put_float_array(buf, 60, pose_covariance, 21); + _mav_put_float_array(buf, 144, velocity_covariance, 21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ODOMETRY_LEN); +#else + mavlink_odometry_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.frame_id = frame_id; + packet.child_frame_id = child_frame_id; + packet.reset_counter = reset_counter; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.pose_covariance, pose_covariance, sizeof(float)*21); + mav_array_memcpy(packet.velocity_covariance, velocity_covariance, sizeof(float)*21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ODOMETRY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ODOMETRY; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); +} + +/** + * @brief Encode a odometry struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param odometry C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_odometry_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_odometry_t* odometry) +{ + return mavlink_msg_odometry_pack(system_id, component_id, msg, odometry->time_usec, odometry->frame_id, odometry->child_frame_id, odometry->x, odometry->y, odometry->z, odometry->q, odometry->vx, odometry->vy, odometry->vz, odometry->rollspeed, odometry->pitchspeed, odometry->yawspeed, odometry->pose_covariance, odometry->velocity_covariance, odometry->reset_counter, odometry->estimator_type); +} + +/** + * @brief Encode a odometry struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param odometry C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_odometry_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_odometry_t* odometry) +{ + return mavlink_msg_odometry_pack_chan(system_id, component_id, chan, msg, odometry->time_usec, odometry->frame_id, odometry->child_frame_id, odometry->x, odometry->y, odometry->z, odometry->q, odometry->vx, odometry->vy, odometry->vz, odometry->rollspeed, odometry->pitchspeed, odometry->yawspeed, odometry->pose_covariance, odometry->velocity_covariance, odometry->reset_counter, odometry->estimator_type); +} + +/** + * @brief Send a odometry message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param frame_id Coordinate frame of reference for the pose data. + * @param child_frame_id Coordinate frame of reference for the velocity in free space (twist) data. + * @param x [m] X Position + * @param y [m] Y Position + * @param z [m] Z Position + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + * @param vx [m/s] X linear speed + * @param vy [m/s] Y linear speed + * @param vz [m/s] Z linear speed + * @param rollspeed [rad/s] Roll angular speed + * @param pitchspeed [rad/s] Pitch angular speed + * @param yawspeed [rad/s] Yaw angular speed + * @param pose_covariance Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param velocity_covariance Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + * @param estimator_type Type of estimator that is providing the odometry. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_odometry_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t frame_id, uint8_t child_frame_id, float x, float y, float z, const float *q, float vx, float vy, float vz, float rollspeed, float pitchspeed, float yawspeed, const float *pose_covariance, const float *velocity_covariance, uint8_t reset_counter, uint8_t estimator_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ODOMETRY_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 36, vx); + _mav_put_float(buf, 40, vy); + _mav_put_float(buf, 44, vz); + _mav_put_float(buf, 48, rollspeed); + _mav_put_float(buf, 52, pitchspeed); + _mav_put_float(buf, 56, yawspeed); + _mav_put_uint8_t(buf, 228, frame_id); + _mav_put_uint8_t(buf, 229, child_frame_id); + _mav_put_uint8_t(buf, 230, reset_counter); + _mav_put_uint8_t(buf, 231, estimator_type); + _mav_put_float_array(buf, 20, q, 4); + _mav_put_float_array(buf, 60, pose_covariance, 21); + _mav_put_float_array(buf, 144, velocity_covariance, 21); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, buf, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); +#else + mavlink_odometry_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.frame_id = frame_id; + packet.child_frame_id = child_frame_id; + packet.reset_counter = reset_counter; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.pose_covariance, pose_covariance, sizeof(float)*21); + mav_array_memcpy(packet.velocity_covariance, velocity_covariance, sizeof(float)*21); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, (const char *)&packet, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); +#endif +} + +/** + * @brief Send a odometry message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_odometry_send_struct(mavlink_channel_t chan, const mavlink_odometry_t* odometry) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_odometry_send(chan, odometry->time_usec, odometry->frame_id, odometry->child_frame_id, odometry->x, odometry->y, odometry->z, odometry->q, odometry->vx, odometry->vy, odometry->vz, odometry->rollspeed, odometry->pitchspeed, odometry->yawspeed, odometry->pose_covariance, odometry->velocity_covariance, odometry->reset_counter, odometry->estimator_type); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, (const char *)odometry, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); +#endif +} + +#if MAVLINK_MSG_ID_ODOMETRY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_odometry_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t frame_id, uint8_t child_frame_id, float x, float y, float z, const float *q, float vx, float vy, float vz, float rollspeed, float pitchspeed, float yawspeed, const float *pose_covariance, const float *velocity_covariance, uint8_t reset_counter, uint8_t estimator_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 36, vx); + _mav_put_float(buf, 40, vy); + _mav_put_float(buf, 44, vz); + _mav_put_float(buf, 48, rollspeed); + _mav_put_float(buf, 52, pitchspeed); + _mav_put_float(buf, 56, yawspeed); + _mav_put_uint8_t(buf, 228, frame_id); + _mav_put_uint8_t(buf, 229, child_frame_id); + _mav_put_uint8_t(buf, 230, reset_counter); + _mav_put_uint8_t(buf, 231, estimator_type); + _mav_put_float_array(buf, 20, q, 4); + _mav_put_float_array(buf, 60, pose_covariance, 21); + _mav_put_float_array(buf, 144, velocity_covariance, 21); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, buf, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); +#else + mavlink_odometry_t *packet = (mavlink_odometry_t *)msgbuf; + packet->time_usec = time_usec; + packet->x = x; + packet->y = y; + packet->z = z; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + packet->frame_id = frame_id; + packet->child_frame_id = child_frame_id; + packet->reset_counter = reset_counter; + packet->estimator_type = estimator_type; + mav_array_memcpy(packet->q, q, sizeof(float)*4); + mav_array_memcpy(packet->pose_covariance, pose_covariance, sizeof(float)*21); + mav_array_memcpy(packet->velocity_covariance, velocity_covariance, sizeof(float)*21); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, (const char *)packet, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); +#endif +} +#endif + +#endif + +// MESSAGE ODOMETRY UNPACKING + + +/** + * @brief Get field time_usec from odometry message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_odometry_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field frame_id from odometry message + * + * @return Coordinate frame of reference for the pose data. + */ +static inline uint8_t mavlink_msg_odometry_get_frame_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 228); +} + +/** + * @brief Get field child_frame_id from odometry message + * + * @return Coordinate frame of reference for the velocity in free space (twist) data. + */ +static inline uint8_t mavlink_msg_odometry_get_child_frame_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 229); +} + +/** + * @brief Get field x from odometry message + * + * @return [m] X Position + */ +static inline float mavlink_msg_odometry_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field y from odometry message + * + * @return [m] Y Position + */ +static inline float mavlink_msg_odometry_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field z from odometry message + * + * @return [m] Z Position + */ +static inline float mavlink_msg_odometry_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field q from odometry message + * + * @return Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + */ +static inline uint16_t mavlink_msg_odometry_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 20); +} + +/** + * @brief Get field vx from odometry message + * + * @return [m/s] X linear speed + */ +static inline float mavlink_msg_odometry_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field vy from odometry message + * + * @return [m/s] Y linear speed + */ +static inline float mavlink_msg_odometry_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field vz from odometry message + * + * @return [m/s] Z linear speed + */ +static inline float mavlink_msg_odometry_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field rollspeed from odometry message + * + * @return [rad/s] Roll angular speed + */ +static inline float mavlink_msg_odometry_get_rollspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 48); +} + +/** + * @brief Get field pitchspeed from odometry message + * + * @return [rad/s] Pitch angular speed + */ +static inline float mavlink_msg_odometry_get_pitchspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 52); +} + +/** + * @brief Get field yawspeed from odometry message + * + * @return [rad/s] Yaw angular speed + */ +static inline float mavlink_msg_odometry_get_yawspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 56); +} + +/** + * @brief Get field pose_covariance from odometry message + * + * @return Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + */ +static inline uint16_t mavlink_msg_odometry_get_pose_covariance(const mavlink_message_t* msg, float *pose_covariance) +{ + return _MAV_RETURN_float_array(msg, pose_covariance, 21, 60); +} + +/** + * @brief Get field velocity_covariance from odometry message + * + * @return Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + */ +static inline uint16_t mavlink_msg_odometry_get_velocity_covariance(const mavlink_message_t* msg, float *velocity_covariance) +{ + return _MAV_RETURN_float_array(msg, velocity_covariance, 21, 144); +} + +/** + * @brief Get field reset_counter from odometry message + * + * @return Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + */ +static inline uint8_t mavlink_msg_odometry_get_reset_counter(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 230); +} + +/** + * @brief Get field estimator_type from odometry message + * + * @return Type of estimator that is providing the odometry. + */ +static inline uint8_t mavlink_msg_odometry_get_estimator_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 231); +} + +/** + * @brief Decode a odometry message into a struct + * + * @param msg The message to decode + * @param odometry C-struct to decode the message contents into + */ +static inline void mavlink_msg_odometry_decode(const mavlink_message_t* msg, mavlink_odometry_t* odometry) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + odometry->time_usec = mavlink_msg_odometry_get_time_usec(msg); + odometry->x = mavlink_msg_odometry_get_x(msg); + odometry->y = mavlink_msg_odometry_get_y(msg); + odometry->z = mavlink_msg_odometry_get_z(msg); + mavlink_msg_odometry_get_q(msg, odometry->q); + odometry->vx = mavlink_msg_odometry_get_vx(msg); + odometry->vy = mavlink_msg_odometry_get_vy(msg); + odometry->vz = mavlink_msg_odometry_get_vz(msg); + odometry->rollspeed = mavlink_msg_odometry_get_rollspeed(msg); + odometry->pitchspeed = mavlink_msg_odometry_get_pitchspeed(msg); + odometry->yawspeed = mavlink_msg_odometry_get_yawspeed(msg); + mavlink_msg_odometry_get_pose_covariance(msg, odometry->pose_covariance); + mavlink_msg_odometry_get_velocity_covariance(msg, odometry->velocity_covariance); + odometry->frame_id = mavlink_msg_odometry_get_frame_id(msg); + odometry->child_frame_id = mavlink_msg_odometry_get_child_frame_id(msg); + odometry->reset_counter = mavlink_msg_odometry_get_reset_counter(msg); + odometry->estimator_type = mavlink_msg_odometry_get_estimator_type(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_ODOMETRY_LEN? msg->len : MAVLINK_MSG_ID_ODOMETRY_LEN; + memset(odometry, 0, MAVLINK_MSG_ID_ODOMETRY_LEN); + memcpy(odometry, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_onboard_computer_status.h b/lib/main/MAVLink/common/mavlink_msg_onboard_computer_status.h new file mode 100644 index 0000000000..6da2581423 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_onboard_computer_status.h @@ -0,0 +1,693 @@ +#pragma once +// MESSAGE ONBOARD_COMPUTER_STATUS PACKING + +#define MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS 390 + + +typedef struct __mavlink_onboard_computer_status_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + uint32_t uptime; /*< [ms] Time since system boot.*/ + uint32_t ram_usage; /*< [MiB] Amount of used RAM on the component system. A value of UINT32_MAX implies the field is unused.*/ + uint32_t ram_total; /*< [MiB] Total amount of RAM on the component system. A value of UINT32_MAX implies the field is unused.*/ + uint32_t storage_type[4]; /*< Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable). A value of UINT32_MAX implies the field is unused.*/ + uint32_t storage_usage[4]; /*< [MiB] Amount of used storage space on the component system. A value of UINT32_MAX implies the field is unused.*/ + uint32_t storage_total[4]; /*< [MiB] Total amount of storage space on the component system. A value of UINT32_MAX implies the field is unused.*/ + uint32_t link_type[6]; /*< Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary*/ + uint32_t link_tx_rate[6]; /*< [KiB/s] Network traffic from the component system. A value of UINT32_MAX implies the field is unused.*/ + uint32_t link_rx_rate[6]; /*< [KiB/s] Network traffic to the component system. A value of UINT32_MAX implies the field is unused.*/ + uint32_t link_tx_max[6]; /*< [KiB/s] Network capacity from the component system. A value of UINT32_MAX implies the field is unused.*/ + uint32_t link_rx_max[6]; /*< [KiB/s] Network capacity to the component system. A value of UINT32_MAX implies the field is unused.*/ + int16_t fan_speed[4]; /*< [rpm] Fan speeds. A value of INT16_MAX implies the field is unused.*/ + uint8_t type; /*< Type of the onboard computer: 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers.*/ + uint8_t cpu_cores[8]; /*< CPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused.*/ + uint8_t cpu_combined[10]; /*< Combined CPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused.*/ + uint8_t gpu_cores[4]; /*< GPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused.*/ + uint8_t gpu_combined[10]; /*< Combined GPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused.*/ + int8_t temperature_board; /*< [degC] Temperature of the board. A value of INT8_MAX implies the field is unused.*/ + int8_t temperature_core[8]; /*< [degC] Temperature of the CPU core. A value of INT8_MAX implies the field is unused.*/ +} mavlink_onboard_computer_status_t; + +#define MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN 238 +#define MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN 238 +#define MAVLINK_MSG_ID_390_LEN 238 +#define MAVLINK_MSG_ID_390_MIN_LEN 238 + +#define MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC 156 +#define MAVLINK_MSG_ID_390_CRC 156 + +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_STORAGE_TYPE_LEN 4 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_STORAGE_USAGE_LEN 4 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_STORAGE_TOTAL_LEN 4 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_LINK_TYPE_LEN 6 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_LINK_TX_RATE_LEN 6 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_LINK_RX_RATE_LEN 6 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_LINK_TX_MAX_LEN 6 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_LINK_RX_MAX_LEN 6 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_FAN_SPEED_LEN 4 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_CPU_CORES_LEN 8 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_CPU_COMBINED_LEN 10 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_GPU_CORES_LEN 4 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_GPU_COMBINED_LEN 10 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_TEMPERATURE_CORE_LEN 8 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_ONBOARD_COMPUTER_STATUS { \ + 390, \ + "ONBOARD_COMPUTER_STATUS", \ + 20, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_onboard_computer_status_t, time_usec) }, \ + { "uptime", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_onboard_computer_status_t, uptime) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 196, offsetof(mavlink_onboard_computer_status_t, type) }, \ + { "cpu_cores", NULL, MAVLINK_TYPE_UINT8_T, 8, 197, offsetof(mavlink_onboard_computer_status_t, cpu_cores) }, \ + { "cpu_combined", NULL, MAVLINK_TYPE_UINT8_T, 10, 205, offsetof(mavlink_onboard_computer_status_t, cpu_combined) }, \ + { "gpu_cores", NULL, MAVLINK_TYPE_UINT8_T, 4, 215, offsetof(mavlink_onboard_computer_status_t, gpu_cores) }, \ + { "gpu_combined", NULL, MAVLINK_TYPE_UINT8_T, 10, 219, offsetof(mavlink_onboard_computer_status_t, gpu_combined) }, \ + { "temperature_board", NULL, MAVLINK_TYPE_INT8_T, 0, 229, offsetof(mavlink_onboard_computer_status_t, temperature_board) }, \ + { "temperature_core", NULL, MAVLINK_TYPE_INT8_T, 8, 230, offsetof(mavlink_onboard_computer_status_t, temperature_core) }, \ + { "fan_speed", NULL, MAVLINK_TYPE_INT16_T, 4, 188, offsetof(mavlink_onboard_computer_status_t, fan_speed) }, \ + { "ram_usage", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_onboard_computer_status_t, ram_usage) }, \ + { "ram_total", NULL, MAVLINK_TYPE_UINT32_T, 0, 16, offsetof(mavlink_onboard_computer_status_t, ram_total) }, \ + { "storage_type", NULL, MAVLINK_TYPE_UINT32_T, 4, 20, offsetof(mavlink_onboard_computer_status_t, storage_type) }, \ + { "storage_usage", NULL, MAVLINK_TYPE_UINT32_T, 4, 36, offsetof(mavlink_onboard_computer_status_t, storage_usage) }, \ + { "storage_total", NULL, MAVLINK_TYPE_UINT32_T, 4, 52, offsetof(mavlink_onboard_computer_status_t, storage_total) }, \ + { "link_type", NULL, MAVLINK_TYPE_UINT32_T, 6, 68, offsetof(mavlink_onboard_computer_status_t, link_type) }, \ + { "link_tx_rate", NULL, MAVLINK_TYPE_UINT32_T, 6, 92, offsetof(mavlink_onboard_computer_status_t, link_tx_rate) }, \ + { "link_rx_rate", NULL, MAVLINK_TYPE_UINT32_T, 6, 116, offsetof(mavlink_onboard_computer_status_t, link_rx_rate) }, \ + { "link_tx_max", NULL, MAVLINK_TYPE_UINT32_T, 6, 140, offsetof(mavlink_onboard_computer_status_t, link_tx_max) }, \ + { "link_rx_max", NULL, MAVLINK_TYPE_UINT32_T, 6, 164, offsetof(mavlink_onboard_computer_status_t, link_rx_max) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_ONBOARD_COMPUTER_STATUS { \ + "ONBOARD_COMPUTER_STATUS", \ + 20, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_onboard_computer_status_t, time_usec) }, \ + { "uptime", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_onboard_computer_status_t, uptime) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 196, offsetof(mavlink_onboard_computer_status_t, type) }, \ + { "cpu_cores", NULL, MAVLINK_TYPE_UINT8_T, 8, 197, offsetof(mavlink_onboard_computer_status_t, cpu_cores) }, \ + { "cpu_combined", NULL, MAVLINK_TYPE_UINT8_T, 10, 205, offsetof(mavlink_onboard_computer_status_t, cpu_combined) }, \ + { "gpu_cores", NULL, MAVLINK_TYPE_UINT8_T, 4, 215, offsetof(mavlink_onboard_computer_status_t, gpu_cores) }, \ + { "gpu_combined", NULL, MAVLINK_TYPE_UINT8_T, 10, 219, offsetof(mavlink_onboard_computer_status_t, gpu_combined) }, \ + { "temperature_board", NULL, MAVLINK_TYPE_INT8_T, 0, 229, offsetof(mavlink_onboard_computer_status_t, temperature_board) }, \ + { "temperature_core", NULL, MAVLINK_TYPE_INT8_T, 8, 230, offsetof(mavlink_onboard_computer_status_t, temperature_core) }, \ + { "fan_speed", NULL, MAVLINK_TYPE_INT16_T, 4, 188, offsetof(mavlink_onboard_computer_status_t, fan_speed) }, \ + { "ram_usage", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_onboard_computer_status_t, ram_usage) }, \ + { "ram_total", NULL, MAVLINK_TYPE_UINT32_T, 0, 16, offsetof(mavlink_onboard_computer_status_t, ram_total) }, \ + { "storage_type", NULL, MAVLINK_TYPE_UINT32_T, 4, 20, offsetof(mavlink_onboard_computer_status_t, storage_type) }, \ + { "storage_usage", NULL, MAVLINK_TYPE_UINT32_T, 4, 36, offsetof(mavlink_onboard_computer_status_t, storage_usage) }, \ + { "storage_total", NULL, MAVLINK_TYPE_UINT32_T, 4, 52, offsetof(mavlink_onboard_computer_status_t, storage_total) }, \ + { "link_type", NULL, MAVLINK_TYPE_UINT32_T, 6, 68, offsetof(mavlink_onboard_computer_status_t, link_type) }, \ + { "link_tx_rate", NULL, MAVLINK_TYPE_UINT32_T, 6, 92, offsetof(mavlink_onboard_computer_status_t, link_tx_rate) }, \ + { "link_rx_rate", NULL, MAVLINK_TYPE_UINT32_T, 6, 116, offsetof(mavlink_onboard_computer_status_t, link_rx_rate) }, \ + { "link_tx_max", NULL, MAVLINK_TYPE_UINT32_T, 6, 140, offsetof(mavlink_onboard_computer_status_t, link_tx_max) }, \ + { "link_rx_max", NULL, MAVLINK_TYPE_UINT32_T, 6, 164, offsetof(mavlink_onboard_computer_status_t, link_rx_max) }, \ + } \ +} +#endif + +/** + * @brief Pack a onboard_computer_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime [ms] Time since system boot. + * @param type Type of the onboard computer: 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers. + * @param cpu_cores CPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + * @param cpu_combined Combined CPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + * @param gpu_cores GPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + * @param gpu_combined Combined GPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + * @param temperature_board [degC] Temperature of the board. A value of INT8_MAX implies the field is unused. + * @param temperature_core [degC] Temperature of the CPU core. A value of INT8_MAX implies the field is unused. + * @param fan_speed [rpm] Fan speeds. A value of INT16_MAX implies the field is unused. + * @param ram_usage [MiB] Amount of used RAM on the component system. A value of UINT32_MAX implies the field is unused. + * @param ram_total [MiB] Total amount of RAM on the component system. A value of UINT32_MAX implies the field is unused. + * @param storage_type Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable). A value of UINT32_MAX implies the field is unused. + * @param storage_usage [MiB] Amount of used storage space on the component system. A value of UINT32_MAX implies the field is unused. + * @param storage_total [MiB] Total amount of storage space on the component system. A value of UINT32_MAX implies the field is unused. + * @param link_type Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary + * @param link_tx_rate [KiB/s] Network traffic from the component system. A value of UINT32_MAX implies the field is unused. + * @param link_rx_rate [KiB/s] Network traffic to the component system. A value of UINT32_MAX implies the field is unused. + * @param link_tx_max [KiB/s] Network capacity from the component system. A value of UINT32_MAX implies the field is unused. + * @param link_rx_max [KiB/s] Network capacity to the component system. A value of UINT32_MAX implies the field is unused. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_onboard_computer_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint32_t uptime, uint8_t type, const uint8_t *cpu_cores, const uint8_t *cpu_combined, const uint8_t *gpu_cores, const uint8_t *gpu_combined, int8_t temperature_board, const int8_t *temperature_core, const int16_t *fan_speed, uint32_t ram_usage, uint32_t ram_total, const uint32_t *storage_type, const uint32_t *storage_usage, const uint32_t *storage_total, const uint32_t *link_type, const uint32_t *link_tx_rate, const uint32_t *link_rx_rate, const uint32_t *link_tx_max, const uint32_t *link_rx_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime); + _mav_put_uint32_t(buf, 12, ram_usage); + _mav_put_uint32_t(buf, 16, ram_total); + _mav_put_uint8_t(buf, 196, type); + _mav_put_int8_t(buf, 229, temperature_board); + _mav_put_uint32_t_array(buf, 20, storage_type, 4); + _mav_put_uint32_t_array(buf, 36, storage_usage, 4); + _mav_put_uint32_t_array(buf, 52, storage_total, 4); + _mav_put_uint32_t_array(buf, 68, link_type, 6); + _mav_put_uint32_t_array(buf, 92, link_tx_rate, 6); + _mav_put_uint32_t_array(buf, 116, link_rx_rate, 6); + _mav_put_uint32_t_array(buf, 140, link_tx_max, 6); + _mav_put_uint32_t_array(buf, 164, link_rx_max, 6); + _mav_put_int16_t_array(buf, 188, fan_speed, 4); + _mav_put_uint8_t_array(buf, 197, cpu_cores, 8); + _mav_put_uint8_t_array(buf, 205, cpu_combined, 10); + _mav_put_uint8_t_array(buf, 215, gpu_cores, 4); + _mav_put_uint8_t_array(buf, 219, gpu_combined, 10); + _mav_put_int8_t_array(buf, 230, temperature_core, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN); +#else + mavlink_onboard_computer_status_t packet; + packet.time_usec = time_usec; + packet.uptime = uptime; + packet.ram_usage = ram_usage; + packet.ram_total = ram_total; + packet.type = type; + packet.temperature_board = temperature_board; + mav_array_memcpy(packet.storage_type, storage_type, sizeof(uint32_t)*4); + mav_array_memcpy(packet.storage_usage, storage_usage, sizeof(uint32_t)*4); + mav_array_memcpy(packet.storage_total, storage_total, sizeof(uint32_t)*4); + mav_array_memcpy(packet.link_type, link_type, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_tx_rate, link_tx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_rx_rate, link_rx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_tx_max, link_tx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_rx_max, link_rx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet.fan_speed, fan_speed, sizeof(int16_t)*4); + mav_array_memcpy(packet.cpu_cores, cpu_cores, sizeof(uint8_t)*8); + mav_array_memcpy(packet.cpu_combined, cpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet.gpu_cores, gpu_cores, sizeof(uint8_t)*4); + mav_array_memcpy(packet.gpu_combined, gpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet.temperature_core, temperature_core, sizeof(int8_t)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); +} + +/** + * @brief Pack a onboard_computer_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime [ms] Time since system boot. + * @param type Type of the onboard computer: 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers. + * @param cpu_cores CPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + * @param cpu_combined Combined CPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + * @param gpu_cores GPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + * @param gpu_combined Combined GPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + * @param temperature_board [degC] Temperature of the board. A value of INT8_MAX implies the field is unused. + * @param temperature_core [degC] Temperature of the CPU core. A value of INT8_MAX implies the field is unused. + * @param fan_speed [rpm] Fan speeds. A value of INT16_MAX implies the field is unused. + * @param ram_usage [MiB] Amount of used RAM on the component system. A value of UINT32_MAX implies the field is unused. + * @param ram_total [MiB] Total amount of RAM on the component system. A value of UINT32_MAX implies the field is unused. + * @param storage_type Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable). A value of UINT32_MAX implies the field is unused. + * @param storage_usage [MiB] Amount of used storage space on the component system. A value of UINT32_MAX implies the field is unused. + * @param storage_total [MiB] Total amount of storage space on the component system. A value of UINT32_MAX implies the field is unused. + * @param link_type Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary + * @param link_tx_rate [KiB/s] Network traffic from the component system. A value of UINT32_MAX implies the field is unused. + * @param link_rx_rate [KiB/s] Network traffic to the component system. A value of UINT32_MAX implies the field is unused. + * @param link_tx_max [KiB/s] Network capacity from the component system. A value of UINT32_MAX implies the field is unused. + * @param link_rx_max [KiB/s] Network capacity to the component system. A value of UINT32_MAX implies the field is unused. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_onboard_computer_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint32_t uptime,uint8_t type,const uint8_t *cpu_cores,const uint8_t *cpu_combined,const uint8_t *gpu_cores,const uint8_t *gpu_combined,int8_t temperature_board,const int8_t *temperature_core,const int16_t *fan_speed,uint32_t ram_usage,uint32_t ram_total,const uint32_t *storage_type,const uint32_t *storage_usage,const uint32_t *storage_total,const uint32_t *link_type,const uint32_t *link_tx_rate,const uint32_t *link_rx_rate,const uint32_t *link_tx_max,const uint32_t *link_rx_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime); + _mav_put_uint32_t(buf, 12, ram_usage); + _mav_put_uint32_t(buf, 16, ram_total); + _mav_put_uint8_t(buf, 196, type); + _mav_put_int8_t(buf, 229, temperature_board); + _mav_put_uint32_t_array(buf, 20, storage_type, 4); + _mav_put_uint32_t_array(buf, 36, storage_usage, 4); + _mav_put_uint32_t_array(buf, 52, storage_total, 4); + _mav_put_uint32_t_array(buf, 68, link_type, 6); + _mav_put_uint32_t_array(buf, 92, link_tx_rate, 6); + _mav_put_uint32_t_array(buf, 116, link_rx_rate, 6); + _mav_put_uint32_t_array(buf, 140, link_tx_max, 6); + _mav_put_uint32_t_array(buf, 164, link_rx_max, 6); + _mav_put_int16_t_array(buf, 188, fan_speed, 4); + _mav_put_uint8_t_array(buf, 197, cpu_cores, 8); + _mav_put_uint8_t_array(buf, 205, cpu_combined, 10); + _mav_put_uint8_t_array(buf, 215, gpu_cores, 4); + _mav_put_uint8_t_array(buf, 219, gpu_combined, 10); + _mav_put_int8_t_array(buf, 230, temperature_core, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN); +#else + mavlink_onboard_computer_status_t packet; + packet.time_usec = time_usec; + packet.uptime = uptime; + packet.ram_usage = ram_usage; + packet.ram_total = ram_total; + packet.type = type; + packet.temperature_board = temperature_board; + mav_array_memcpy(packet.storage_type, storage_type, sizeof(uint32_t)*4); + mav_array_memcpy(packet.storage_usage, storage_usage, sizeof(uint32_t)*4); + mav_array_memcpy(packet.storage_total, storage_total, sizeof(uint32_t)*4); + mav_array_memcpy(packet.link_type, link_type, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_tx_rate, link_tx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_rx_rate, link_rx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_tx_max, link_tx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_rx_max, link_rx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet.fan_speed, fan_speed, sizeof(int16_t)*4); + mav_array_memcpy(packet.cpu_cores, cpu_cores, sizeof(uint8_t)*8); + mav_array_memcpy(packet.cpu_combined, cpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet.gpu_cores, gpu_cores, sizeof(uint8_t)*4); + mav_array_memcpy(packet.gpu_combined, gpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet.temperature_core, temperature_core, sizeof(int8_t)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); +} + +/** + * @brief Encode a onboard_computer_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param onboard_computer_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_onboard_computer_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_onboard_computer_status_t* onboard_computer_status) +{ + return mavlink_msg_onboard_computer_status_pack(system_id, component_id, msg, onboard_computer_status->time_usec, onboard_computer_status->uptime, onboard_computer_status->type, onboard_computer_status->cpu_cores, onboard_computer_status->cpu_combined, onboard_computer_status->gpu_cores, onboard_computer_status->gpu_combined, onboard_computer_status->temperature_board, onboard_computer_status->temperature_core, onboard_computer_status->fan_speed, onboard_computer_status->ram_usage, onboard_computer_status->ram_total, onboard_computer_status->storage_type, onboard_computer_status->storage_usage, onboard_computer_status->storage_total, onboard_computer_status->link_type, onboard_computer_status->link_tx_rate, onboard_computer_status->link_rx_rate, onboard_computer_status->link_tx_max, onboard_computer_status->link_rx_max); +} + +/** + * @brief Encode a onboard_computer_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param onboard_computer_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_onboard_computer_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_onboard_computer_status_t* onboard_computer_status) +{ + return mavlink_msg_onboard_computer_status_pack_chan(system_id, component_id, chan, msg, onboard_computer_status->time_usec, onboard_computer_status->uptime, onboard_computer_status->type, onboard_computer_status->cpu_cores, onboard_computer_status->cpu_combined, onboard_computer_status->gpu_cores, onboard_computer_status->gpu_combined, onboard_computer_status->temperature_board, onboard_computer_status->temperature_core, onboard_computer_status->fan_speed, onboard_computer_status->ram_usage, onboard_computer_status->ram_total, onboard_computer_status->storage_type, onboard_computer_status->storage_usage, onboard_computer_status->storage_total, onboard_computer_status->link_type, onboard_computer_status->link_tx_rate, onboard_computer_status->link_rx_rate, onboard_computer_status->link_tx_max, onboard_computer_status->link_rx_max); +} + +/** + * @brief Send a onboard_computer_status message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime [ms] Time since system boot. + * @param type Type of the onboard computer: 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers. + * @param cpu_cores CPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + * @param cpu_combined Combined CPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + * @param gpu_cores GPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + * @param gpu_combined Combined GPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + * @param temperature_board [degC] Temperature of the board. A value of INT8_MAX implies the field is unused. + * @param temperature_core [degC] Temperature of the CPU core. A value of INT8_MAX implies the field is unused. + * @param fan_speed [rpm] Fan speeds. A value of INT16_MAX implies the field is unused. + * @param ram_usage [MiB] Amount of used RAM on the component system. A value of UINT32_MAX implies the field is unused. + * @param ram_total [MiB] Total amount of RAM on the component system. A value of UINT32_MAX implies the field is unused. + * @param storage_type Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable). A value of UINT32_MAX implies the field is unused. + * @param storage_usage [MiB] Amount of used storage space on the component system. A value of UINT32_MAX implies the field is unused. + * @param storage_total [MiB] Total amount of storage space on the component system. A value of UINT32_MAX implies the field is unused. + * @param link_type Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary + * @param link_tx_rate [KiB/s] Network traffic from the component system. A value of UINT32_MAX implies the field is unused. + * @param link_rx_rate [KiB/s] Network traffic to the component system. A value of UINT32_MAX implies the field is unused. + * @param link_tx_max [KiB/s] Network capacity from the component system. A value of UINT32_MAX implies the field is unused. + * @param link_rx_max [KiB/s] Network capacity to the component system. A value of UINT32_MAX implies the field is unused. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_onboard_computer_status_send(mavlink_channel_t chan, uint64_t time_usec, uint32_t uptime, uint8_t type, const uint8_t *cpu_cores, const uint8_t *cpu_combined, const uint8_t *gpu_cores, const uint8_t *gpu_combined, int8_t temperature_board, const int8_t *temperature_core, const int16_t *fan_speed, uint32_t ram_usage, uint32_t ram_total, const uint32_t *storage_type, const uint32_t *storage_usage, const uint32_t *storage_total, const uint32_t *link_type, const uint32_t *link_tx_rate, const uint32_t *link_rx_rate, const uint32_t *link_tx_max, const uint32_t *link_rx_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime); + _mav_put_uint32_t(buf, 12, ram_usage); + _mav_put_uint32_t(buf, 16, ram_total); + _mav_put_uint8_t(buf, 196, type); + _mav_put_int8_t(buf, 229, temperature_board); + _mav_put_uint32_t_array(buf, 20, storage_type, 4); + _mav_put_uint32_t_array(buf, 36, storage_usage, 4); + _mav_put_uint32_t_array(buf, 52, storage_total, 4); + _mav_put_uint32_t_array(buf, 68, link_type, 6); + _mav_put_uint32_t_array(buf, 92, link_tx_rate, 6); + _mav_put_uint32_t_array(buf, 116, link_rx_rate, 6); + _mav_put_uint32_t_array(buf, 140, link_tx_max, 6); + _mav_put_uint32_t_array(buf, 164, link_rx_max, 6); + _mav_put_int16_t_array(buf, 188, fan_speed, 4); + _mav_put_uint8_t_array(buf, 197, cpu_cores, 8); + _mav_put_uint8_t_array(buf, 205, cpu_combined, 10); + _mav_put_uint8_t_array(buf, 215, gpu_cores, 4); + _mav_put_uint8_t_array(buf, 219, gpu_combined, 10); + _mav_put_int8_t_array(buf, 230, temperature_core, 8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS, buf, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); +#else + mavlink_onboard_computer_status_t packet; + packet.time_usec = time_usec; + packet.uptime = uptime; + packet.ram_usage = ram_usage; + packet.ram_total = ram_total; + packet.type = type; + packet.temperature_board = temperature_board; + mav_array_memcpy(packet.storage_type, storage_type, sizeof(uint32_t)*4); + mav_array_memcpy(packet.storage_usage, storage_usage, sizeof(uint32_t)*4); + mav_array_memcpy(packet.storage_total, storage_total, sizeof(uint32_t)*4); + mav_array_memcpy(packet.link_type, link_type, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_tx_rate, link_tx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_rx_rate, link_rx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_tx_max, link_tx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_rx_max, link_rx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet.fan_speed, fan_speed, sizeof(int16_t)*4); + mav_array_memcpy(packet.cpu_cores, cpu_cores, sizeof(uint8_t)*8); + mav_array_memcpy(packet.cpu_combined, cpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet.gpu_cores, gpu_cores, sizeof(uint8_t)*4); + mav_array_memcpy(packet.gpu_combined, gpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet.temperature_core, temperature_core, sizeof(int8_t)*8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); +#endif +} + +/** + * @brief Send a onboard_computer_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_onboard_computer_status_send_struct(mavlink_channel_t chan, const mavlink_onboard_computer_status_t* onboard_computer_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_onboard_computer_status_send(chan, onboard_computer_status->time_usec, onboard_computer_status->uptime, onboard_computer_status->type, onboard_computer_status->cpu_cores, onboard_computer_status->cpu_combined, onboard_computer_status->gpu_cores, onboard_computer_status->gpu_combined, onboard_computer_status->temperature_board, onboard_computer_status->temperature_core, onboard_computer_status->fan_speed, onboard_computer_status->ram_usage, onboard_computer_status->ram_total, onboard_computer_status->storage_type, onboard_computer_status->storage_usage, onboard_computer_status->storage_total, onboard_computer_status->link_type, onboard_computer_status->link_tx_rate, onboard_computer_status->link_rx_rate, onboard_computer_status->link_tx_max, onboard_computer_status->link_rx_max); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS, (const char *)onboard_computer_status, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_onboard_computer_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint32_t uptime, uint8_t type, const uint8_t *cpu_cores, const uint8_t *cpu_combined, const uint8_t *gpu_cores, const uint8_t *gpu_combined, int8_t temperature_board, const int8_t *temperature_core, const int16_t *fan_speed, uint32_t ram_usage, uint32_t ram_total, const uint32_t *storage_type, const uint32_t *storage_usage, const uint32_t *storage_total, const uint32_t *link_type, const uint32_t *link_tx_rate, const uint32_t *link_rx_rate, const uint32_t *link_tx_max, const uint32_t *link_rx_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime); + _mav_put_uint32_t(buf, 12, ram_usage); + _mav_put_uint32_t(buf, 16, ram_total); + _mav_put_uint8_t(buf, 196, type); + _mav_put_int8_t(buf, 229, temperature_board); + _mav_put_uint32_t_array(buf, 20, storage_type, 4); + _mav_put_uint32_t_array(buf, 36, storage_usage, 4); + _mav_put_uint32_t_array(buf, 52, storage_total, 4); + _mav_put_uint32_t_array(buf, 68, link_type, 6); + _mav_put_uint32_t_array(buf, 92, link_tx_rate, 6); + _mav_put_uint32_t_array(buf, 116, link_rx_rate, 6); + _mav_put_uint32_t_array(buf, 140, link_tx_max, 6); + _mav_put_uint32_t_array(buf, 164, link_rx_max, 6); + _mav_put_int16_t_array(buf, 188, fan_speed, 4); + _mav_put_uint8_t_array(buf, 197, cpu_cores, 8); + _mav_put_uint8_t_array(buf, 205, cpu_combined, 10); + _mav_put_uint8_t_array(buf, 215, gpu_cores, 4); + _mav_put_uint8_t_array(buf, 219, gpu_combined, 10); + _mav_put_int8_t_array(buf, 230, temperature_core, 8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS, buf, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); +#else + mavlink_onboard_computer_status_t *packet = (mavlink_onboard_computer_status_t *)msgbuf; + packet->time_usec = time_usec; + packet->uptime = uptime; + packet->ram_usage = ram_usage; + packet->ram_total = ram_total; + packet->type = type; + packet->temperature_board = temperature_board; + mav_array_memcpy(packet->storage_type, storage_type, sizeof(uint32_t)*4); + mav_array_memcpy(packet->storage_usage, storage_usage, sizeof(uint32_t)*4); + mav_array_memcpy(packet->storage_total, storage_total, sizeof(uint32_t)*4); + mav_array_memcpy(packet->link_type, link_type, sizeof(uint32_t)*6); + mav_array_memcpy(packet->link_tx_rate, link_tx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet->link_rx_rate, link_rx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet->link_tx_max, link_tx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet->link_rx_max, link_rx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet->fan_speed, fan_speed, sizeof(int16_t)*4); + mav_array_memcpy(packet->cpu_cores, cpu_cores, sizeof(uint8_t)*8); + mav_array_memcpy(packet->cpu_combined, cpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet->gpu_cores, gpu_cores, sizeof(uint8_t)*4); + mav_array_memcpy(packet->gpu_combined, gpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet->temperature_core, temperature_core, sizeof(int8_t)*8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS, (const char *)packet, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE ONBOARD_COMPUTER_STATUS UNPACKING + + +/** + * @brief Get field time_usec from onboard_computer_status message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_onboard_computer_status_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field uptime from onboard_computer_status message + * + * @return [ms] Time since system boot. + */ +static inline uint32_t mavlink_msg_onboard_computer_status_get_uptime(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field type from onboard_computer_status message + * + * @return Type of the onboard computer: 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers. + */ +static inline uint8_t mavlink_msg_onboard_computer_status_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 196); +} + +/** + * @brief Get field cpu_cores from onboard_computer_status message + * + * @return CPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_cpu_cores(const mavlink_message_t* msg, uint8_t *cpu_cores) +{ + return _MAV_RETURN_uint8_t_array(msg, cpu_cores, 8, 197); +} + +/** + * @brief Get field cpu_combined from onboard_computer_status message + * + * @return Combined CPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_cpu_combined(const mavlink_message_t* msg, uint8_t *cpu_combined) +{ + return _MAV_RETURN_uint8_t_array(msg, cpu_combined, 10, 205); +} + +/** + * @brief Get field gpu_cores from onboard_computer_status message + * + * @return GPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_gpu_cores(const mavlink_message_t* msg, uint8_t *gpu_cores) +{ + return _MAV_RETURN_uint8_t_array(msg, gpu_cores, 4, 215); +} + +/** + * @brief Get field gpu_combined from onboard_computer_status message + * + * @return Combined GPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_gpu_combined(const mavlink_message_t* msg, uint8_t *gpu_combined) +{ + return _MAV_RETURN_uint8_t_array(msg, gpu_combined, 10, 219); +} + +/** + * @brief Get field temperature_board from onboard_computer_status message + * + * @return [degC] Temperature of the board. A value of INT8_MAX implies the field is unused. + */ +static inline int8_t mavlink_msg_onboard_computer_status_get_temperature_board(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int8_t(msg, 229); +} + +/** + * @brief Get field temperature_core from onboard_computer_status message + * + * @return [degC] Temperature of the CPU core. A value of INT8_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_temperature_core(const mavlink_message_t* msg, int8_t *temperature_core) +{ + return _MAV_RETURN_int8_t_array(msg, temperature_core, 8, 230); +} + +/** + * @brief Get field fan_speed from onboard_computer_status message + * + * @return [rpm] Fan speeds. A value of INT16_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_fan_speed(const mavlink_message_t* msg, int16_t *fan_speed) +{ + return _MAV_RETURN_int16_t_array(msg, fan_speed, 4, 188); +} + +/** + * @brief Get field ram_usage from onboard_computer_status message + * + * @return [MiB] Amount of used RAM on the component system. A value of UINT32_MAX implies the field is unused. + */ +static inline uint32_t mavlink_msg_onboard_computer_status_get_ram_usage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 12); +} + +/** + * @brief Get field ram_total from onboard_computer_status message + * + * @return [MiB] Total amount of RAM on the component system. A value of UINT32_MAX implies the field is unused. + */ +static inline uint32_t mavlink_msg_onboard_computer_status_get_ram_total(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 16); +} + +/** + * @brief Get field storage_type from onboard_computer_status message + * + * @return Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable). A value of UINT32_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_storage_type(const mavlink_message_t* msg, uint32_t *storage_type) +{ + return _MAV_RETURN_uint32_t_array(msg, storage_type, 4, 20); +} + +/** + * @brief Get field storage_usage from onboard_computer_status message + * + * @return [MiB] Amount of used storage space on the component system. A value of UINT32_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_storage_usage(const mavlink_message_t* msg, uint32_t *storage_usage) +{ + return _MAV_RETURN_uint32_t_array(msg, storage_usage, 4, 36); +} + +/** + * @brief Get field storage_total from onboard_computer_status message + * + * @return [MiB] Total amount of storage space on the component system. A value of UINT32_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_storage_total(const mavlink_message_t* msg, uint32_t *storage_total) +{ + return _MAV_RETURN_uint32_t_array(msg, storage_total, 4, 52); +} + +/** + * @brief Get field link_type from onboard_computer_status message + * + * @return Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_link_type(const mavlink_message_t* msg, uint32_t *link_type) +{ + return _MAV_RETURN_uint32_t_array(msg, link_type, 6, 68); +} + +/** + * @brief Get field link_tx_rate from onboard_computer_status message + * + * @return [KiB/s] Network traffic from the component system. A value of UINT32_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_link_tx_rate(const mavlink_message_t* msg, uint32_t *link_tx_rate) +{ + return _MAV_RETURN_uint32_t_array(msg, link_tx_rate, 6, 92); +} + +/** + * @brief Get field link_rx_rate from onboard_computer_status message + * + * @return [KiB/s] Network traffic to the component system. A value of UINT32_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_link_rx_rate(const mavlink_message_t* msg, uint32_t *link_rx_rate) +{ + return _MAV_RETURN_uint32_t_array(msg, link_rx_rate, 6, 116); +} + +/** + * @brief Get field link_tx_max from onboard_computer_status message + * + * @return [KiB/s] Network capacity from the component system. A value of UINT32_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_link_tx_max(const mavlink_message_t* msg, uint32_t *link_tx_max) +{ + return _MAV_RETURN_uint32_t_array(msg, link_tx_max, 6, 140); +} + +/** + * @brief Get field link_rx_max from onboard_computer_status message + * + * @return [KiB/s] Network capacity to the component system. A value of UINT32_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_link_rx_max(const mavlink_message_t* msg, uint32_t *link_rx_max) +{ + return _MAV_RETURN_uint32_t_array(msg, link_rx_max, 6, 164); +} + +/** + * @brief Decode a onboard_computer_status message into a struct + * + * @param msg The message to decode + * @param onboard_computer_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_onboard_computer_status_decode(const mavlink_message_t* msg, mavlink_onboard_computer_status_t* onboard_computer_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + onboard_computer_status->time_usec = mavlink_msg_onboard_computer_status_get_time_usec(msg); + onboard_computer_status->uptime = mavlink_msg_onboard_computer_status_get_uptime(msg); + onboard_computer_status->ram_usage = mavlink_msg_onboard_computer_status_get_ram_usage(msg); + onboard_computer_status->ram_total = mavlink_msg_onboard_computer_status_get_ram_total(msg); + mavlink_msg_onboard_computer_status_get_storage_type(msg, onboard_computer_status->storage_type); + mavlink_msg_onboard_computer_status_get_storage_usage(msg, onboard_computer_status->storage_usage); + mavlink_msg_onboard_computer_status_get_storage_total(msg, onboard_computer_status->storage_total); + mavlink_msg_onboard_computer_status_get_link_type(msg, onboard_computer_status->link_type); + mavlink_msg_onboard_computer_status_get_link_tx_rate(msg, onboard_computer_status->link_tx_rate); + mavlink_msg_onboard_computer_status_get_link_rx_rate(msg, onboard_computer_status->link_rx_rate); + mavlink_msg_onboard_computer_status_get_link_tx_max(msg, onboard_computer_status->link_tx_max); + mavlink_msg_onboard_computer_status_get_link_rx_max(msg, onboard_computer_status->link_rx_max); + mavlink_msg_onboard_computer_status_get_fan_speed(msg, onboard_computer_status->fan_speed); + onboard_computer_status->type = mavlink_msg_onboard_computer_status_get_type(msg); + mavlink_msg_onboard_computer_status_get_cpu_cores(msg, onboard_computer_status->cpu_cores); + mavlink_msg_onboard_computer_status_get_cpu_combined(msg, onboard_computer_status->cpu_combined); + mavlink_msg_onboard_computer_status_get_gpu_cores(msg, onboard_computer_status->gpu_cores); + mavlink_msg_onboard_computer_status_get_gpu_combined(msg, onboard_computer_status->gpu_combined); + onboard_computer_status->temperature_board = mavlink_msg_onboard_computer_status_get_temperature_board(msg); + mavlink_msg_onboard_computer_status_get_temperature_core(msg, onboard_computer_status->temperature_core); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN? msg->len : MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN; + memset(onboard_computer_status, 0, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN); + memcpy(onboard_computer_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_authentication.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_authentication.h new file mode 100644 index 0000000000..cd7b68e026 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_authentication.h @@ -0,0 +1,406 @@ +#pragma once +// MESSAGE OPEN_DRONE_ID_AUTHENTICATION PACKING + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION 12902 + + +typedef struct __mavlink_open_drone_id_authentication_t { + uint32_t timestamp; /*< [s] This field is only present for page 0. 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019.*/ + uint8_t target_system; /*< System ID (0 for broadcast).*/ + uint8_t target_component; /*< Component ID (0 for broadcast).*/ + uint8_t id_or_mac[20]; /*< Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */ + uint8_t authentication_type; /*< Indicates the type of authentication.*/ + uint8_t data_page; /*< Allowed range is 0 - 4.*/ + uint8_t page_count; /*< This field is only present for page 0. Allowed range is 0 - 5.*/ + uint8_t length; /*< [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. Allowed range is 0 - 109 (17 + 23*4).*/ + uint8_t authentication_data[23]; /*< Opaque authentication data. For page 0, the size is only 17 bytes. For other pages, the size is 23 bytes. Shall be filled with nulls in the unused portion of the field.*/ +} mavlink_open_drone_id_authentication_t; + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN 53 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN 53 +#define MAVLINK_MSG_ID_12902_LEN 53 +#define MAVLINK_MSG_ID_12902_MIN_LEN 53 + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC 49 +#define MAVLINK_MSG_ID_12902_CRC 49 + +#define MAVLINK_MSG_OPEN_DRONE_ID_AUTHENTICATION_FIELD_ID_OR_MAC_LEN 20 +#define MAVLINK_MSG_OPEN_DRONE_ID_AUTHENTICATION_FIELD_AUTHENTICATION_DATA_LEN 23 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_AUTHENTICATION { \ + 12902, \ + "OPEN_DRONE_ID_AUTHENTICATION", \ + 9, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_open_drone_id_authentication_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_open_drone_id_authentication_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 6, offsetof(mavlink_open_drone_id_authentication_t, id_or_mac) }, \ + { "authentication_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_open_drone_id_authentication_t, authentication_type) }, \ + { "data_page", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_open_drone_id_authentication_t, data_page) }, \ + { "page_count", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_open_drone_id_authentication_t, page_count) }, \ + { "length", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_open_drone_id_authentication_t, length) }, \ + { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_open_drone_id_authentication_t, timestamp) }, \ + { "authentication_data", NULL, MAVLINK_TYPE_UINT8_T, 23, 30, offsetof(mavlink_open_drone_id_authentication_t, authentication_data) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_AUTHENTICATION { \ + "OPEN_DRONE_ID_AUTHENTICATION", \ + 9, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_open_drone_id_authentication_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_open_drone_id_authentication_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 6, offsetof(mavlink_open_drone_id_authentication_t, id_or_mac) }, \ + { "authentication_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_open_drone_id_authentication_t, authentication_type) }, \ + { "data_page", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_open_drone_id_authentication_t, data_page) }, \ + { "page_count", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_open_drone_id_authentication_t, page_count) }, \ + { "length", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_open_drone_id_authentication_t, length) }, \ + { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_open_drone_id_authentication_t, timestamp) }, \ + { "authentication_data", NULL, MAVLINK_TYPE_UINT8_T, 23, 30, offsetof(mavlink_open_drone_id_authentication_t, authentication_data) }, \ + } \ +} +#endif + +/** + * @brief Pack a open_drone_id_authentication message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param authentication_type Indicates the type of authentication. + * @param data_page Allowed range is 0 - 4. + * @param page_count This field is only present for page 0. Allowed range is 0 - 5. + * @param length [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. Allowed range is 0 - 109 (17 + 23*4). + * @param timestamp [s] This field is only present for page 0. 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + * @param authentication_data Opaque authentication data. For page 0, the size is only 17 bytes. For other pages, the size is 23 bytes. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_authentication_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t authentication_type, uint8_t data_page, uint8_t page_count, uint8_t length, uint32_t timestamp, const uint8_t *authentication_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN]; + _mav_put_uint32_t(buf, 0, timestamp); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 26, authentication_type); + _mav_put_uint8_t(buf, 27, data_page); + _mav_put_uint8_t(buf, 28, page_count); + _mav_put_uint8_t(buf, 29, length); + _mav_put_uint8_t_array(buf, 6, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 30, authentication_data, 23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN); +#else + mavlink_open_drone_id_authentication_t packet; + packet.timestamp = timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + packet.authentication_type = authentication_type; + packet.data_page = data_page; + packet.page_count = page_count; + packet.length = length; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.authentication_data, authentication_data, sizeof(uint8_t)*23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); +} + +/** + * @brief Pack a open_drone_id_authentication message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param authentication_type Indicates the type of authentication. + * @param data_page Allowed range is 0 - 4. + * @param page_count This field is only present for page 0. Allowed range is 0 - 5. + * @param length [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. Allowed range is 0 - 109 (17 + 23*4). + * @param timestamp [s] This field is only present for page 0. 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + * @param authentication_data Opaque authentication data. For page 0, the size is only 17 bytes. For other pages, the size is 23 bytes. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_authentication_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const uint8_t *id_or_mac,uint8_t authentication_type,uint8_t data_page,uint8_t page_count,uint8_t length,uint32_t timestamp,const uint8_t *authentication_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN]; + _mav_put_uint32_t(buf, 0, timestamp); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 26, authentication_type); + _mav_put_uint8_t(buf, 27, data_page); + _mav_put_uint8_t(buf, 28, page_count); + _mav_put_uint8_t(buf, 29, length); + _mav_put_uint8_t_array(buf, 6, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 30, authentication_data, 23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN); +#else + mavlink_open_drone_id_authentication_t packet; + packet.timestamp = timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + packet.authentication_type = authentication_type; + packet.data_page = data_page; + packet.page_count = page_count; + packet.length = length; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.authentication_data, authentication_data, sizeof(uint8_t)*23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); +} + +/** + * @brief Encode a open_drone_id_authentication struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_authentication C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_authentication_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_authentication_t* open_drone_id_authentication) +{ + return mavlink_msg_open_drone_id_authentication_pack(system_id, component_id, msg, open_drone_id_authentication->target_system, open_drone_id_authentication->target_component, open_drone_id_authentication->id_or_mac, open_drone_id_authentication->authentication_type, open_drone_id_authentication->data_page, open_drone_id_authentication->page_count, open_drone_id_authentication->length, open_drone_id_authentication->timestamp, open_drone_id_authentication->authentication_data); +} + +/** + * @brief Encode a open_drone_id_authentication struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_authentication C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_authentication_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_authentication_t* open_drone_id_authentication) +{ + return mavlink_msg_open_drone_id_authentication_pack_chan(system_id, component_id, chan, msg, open_drone_id_authentication->target_system, open_drone_id_authentication->target_component, open_drone_id_authentication->id_or_mac, open_drone_id_authentication->authentication_type, open_drone_id_authentication->data_page, open_drone_id_authentication->page_count, open_drone_id_authentication->length, open_drone_id_authentication->timestamp, open_drone_id_authentication->authentication_data); +} + +/** + * @brief Send a open_drone_id_authentication message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param authentication_type Indicates the type of authentication. + * @param data_page Allowed range is 0 - 4. + * @param page_count This field is only present for page 0. Allowed range is 0 - 5. + * @param length [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. Allowed range is 0 - 109 (17 + 23*4). + * @param timestamp [s] This field is only present for page 0. 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + * @param authentication_data Opaque authentication data. For page 0, the size is only 17 bytes. For other pages, the size is 23 bytes. Shall be filled with nulls in the unused portion of the field. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_open_drone_id_authentication_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t authentication_type, uint8_t data_page, uint8_t page_count, uint8_t length, uint32_t timestamp, const uint8_t *authentication_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN]; + _mav_put_uint32_t(buf, 0, timestamp); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 26, authentication_type); + _mav_put_uint8_t(buf, 27, data_page); + _mav_put_uint8_t(buf, 28, page_count); + _mav_put_uint8_t(buf, 29, length); + _mav_put_uint8_t_array(buf, 6, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 30, authentication_data, 23); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); +#else + mavlink_open_drone_id_authentication_t packet; + packet.timestamp = timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + packet.authentication_type = authentication_type; + packet.data_page = data_page; + packet.page_count = page_count; + packet.length = length; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.authentication_data, authentication_data, sizeof(uint8_t)*23); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); +#endif +} + +/** + * @brief Send a open_drone_id_authentication message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_open_drone_id_authentication_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_authentication_t* open_drone_id_authentication) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_open_drone_id_authentication_send(chan, open_drone_id_authentication->target_system, open_drone_id_authentication->target_component, open_drone_id_authentication->id_or_mac, open_drone_id_authentication->authentication_type, open_drone_id_authentication->data_page, open_drone_id_authentication->page_count, open_drone_id_authentication->length, open_drone_id_authentication->timestamp, open_drone_id_authentication->authentication_data); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION, (const char *)open_drone_id_authentication, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_open_drone_id_authentication_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t authentication_type, uint8_t data_page, uint8_t page_count, uint8_t length, uint32_t timestamp, const uint8_t *authentication_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, timestamp); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 26, authentication_type); + _mav_put_uint8_t(buf, 27, data_page); + _mav_put_uint8_t(buf, 28, page_count); + _mav_put_uint8_t(buf, 29, length); + _mav_put_uint8_t_array(buf, 6, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 30, authentication_data, 23); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); +#else + mavlink_open_drone_id_authentication_t *packet = (mavlink_open_drone_id_authentication_t *)msgbuf; + packet->timestamp = timestamp; + packet->target_system = target_system; + packet->target_component = target_component; + packet->authentication_type = authentication_type; + packet->data_page = data_page; + packet->page_count = page_count; + packet->length = length; + mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet->authentication_data, authentication_data, sizeof(uint8_t)*23); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OPEN_DRONE_ID_AUTHENTICATION UNPACKING + + +/** + * @brief Get field target_system from open_drone_id_authentication message + * + * @return System ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_authentication_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from open_drone_id_authentication message + * + * @return Component ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_authentication_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field id_or_mac from open_drone_id_authentication message + * + * @return Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + */ +static inline uint16_t mavlink_msg_open_drone_id_authentication_get_id_or_mac(const mavlink_message_t* msg, uint8_t *id_or_mac) +{ + return _MAV_RETURN_uint8_t_array(msg, id_or_mac, 20, 6); +} + +/** + * @brief Get field authentication_type from open_drone_id_authentication message + * + * @return Indicates the type of authentication. + */ +static inline uint8_t mavlink_msg_open_drone_id_authentication_get_authentication_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 26); +} + +/** + * @brief Get field data_page from open_drone_id_authentication message + * + * @return Allowed range is 0 - 4. + */ +static inline uint8_t mavlink_msg_open_drone_id_authentication_get_data_page(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 27); +} + +/** + * @brief Get field page_count from open_drone_id_authentication message + * + * @return This field is only present for page 0. Allowed range is 0 - 5. + */ +static inline uint8_t mavlink_msg_open_drone_id_authentication_get_page_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 28); +} + +/** + * @brief Get field length from open_drone_id_authentication message + * + * @return [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. Allowed range is 0 - 109 (17 + 23*4). + */ +static inline uint8_t mavlink_msg_open_drone_id_authentication_get_length(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 29); +} + +/** + * @brief Get field timestamp from open_drone_id_authentication message + * + * @return [s] This field is only present for page 0. 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + */ +static inline uint32_t mavlink_msg_open_drone_id_authentication_get_timestamp(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field authentication_data from open_drone_id_authentication message + * + * @return Opaque authentication data. For page 0, the size is only 17 bytes. For other pages, the size is 23 bytes. Shall be filled with nulls in the unused portion of the field. + */ +static inline uint16_t mavlink_msg_open_drone_id_authentication_get_authentication_data(const mavlink_message_t* msg, uint8_t *authentication_data) +{ + return _MAV_RETURN_uint8_t_array(msg, authentication_data, 23, 30); +} + +/** + * @brief Decode a open_drone_id_authentication message into a struct + * + * @param msg The message to decode + * @param open_drone_id_authentication C-struct to decode the message contents into + */ +static inline void mavlink_msg_open_drone_id_authentication_decode(const mavlink_message_t* msg, mavlink_open_drone_id_authentication_t* open_drone_id_authentication) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + open_drone_id_authentication->timestamp = mavlink_msg_open_drone_id_authentication_get_timestamp(msg); + open_drone_id_authentication->target_system = mavlink_msg_open_drone_id_authentication_get_target_system(msg); + open_drone_id_authentication->target_component = mavlink_msg_open_drone_id_authentication_get_target_component(msg); + mavlink_msg_open_drone_id_authentication_get_id_or_mac(msg, open_drone_id_authentication->id_or_mac); + open_drone_id_authentication->authentication_type = mavlink_msg_open_drone_id_authentication_get_authentication_type(msg); + open_drone_id_authentication->data_page = mavlink_msg_open_drone_id_authentication_get_data_page(msg); + open_drone_id_authentication->page_count = mavlink_msg_open_drone_id_authentication_get_page_count(msg); + open_drone_id_authentication->length = mavlink_msg_open_drone_id_authentication_get_length(msg); + mavlink_msg_open_drone_id_authentication_get_authentication_data(msg, open_drone_id_authentication->authentication_data); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN? msg->len : MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN; + memset(open_drone_id_authentication, 0, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN); + memcpy(open_drone_id_authentication, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_basic_id.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_basic_id.h new file mode 100644 index 0000000000..1102960b71 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_basic_id.h @@ -0,0 +1,331 @@ +#pragma once +// MESSAGE OPEN_DRONE_ID_BASIC_ID PACKING + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID 12900 + + +typedef struct __mavlink_open_drone_id_basic_id_t { + uint8_t target_system; /*< System ID (0 for broadcast).*/ + uint8_t target_component; /*< Component ID (0 for broadcast).*/ + uint8_t id_or_mac[20]; /*< Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */ + uint8_t id_type; /*< Indicates the format for the uas_id field of this message.*/ + uint8_t ua_type; /*< Indicates the type of UA (Unmanned Aircraft).*/ + uint8_t uas_id[20]; /*< UAS (Unmanned Aircraft System) ID following the format specified by id_type. Shall be filled with nulls in the unused portion of the field.*/ +} mavlink_open_drone_id_basic_id_t; + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN 44 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN 44 +#define MAVLINK_MSG_ID_12900_LEN 44 +#define MAVLINK_MSG_ID_12900_MIN_LEN 44 + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC 114 +#define MAVLINK_MSG_ID_12900_CRC 114 + +#define MAVLINK_MSG_OPEN_DRONE_ID_BASIC_ID_FIELD_ID_OR_MAC_LEN 20 +#define MAVLINK_MSG_OPEN_DRONE_ID_BASIC_ID_FIELD_UAS_ID_LEN 20 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_BASIC_ID { \ + 12900, \ + "OPEN_DRONE_ID_BASIC_ID", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_basic_id_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_basic_id_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 2, offsetof(mavlink_open_drone_id_basic_id_t, id_or_mac) }, \ + { "id_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_open_drone_id_basic_id_t, id_type) }, \ + { "ua_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 23, offsetof(mavlink_open_drone_id_basic_id_t, ua_type) }, \ + { "uas_id", NULL, MAVLINK_TYPE_UINT8_T, 20, 24, offsetof(mavlink_open_drone_id_basic_id_t, uas_id) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_BASIC_ID { \ + "OPEN_DRONE_ID_BASIC_ID", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_basic_id_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_basic_id_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 2, offsetof(mavlink_open_drone_id_basic_id_t, id_or_mac) }, \ + { "id_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_open_drone_id_basic_id_t, id_type) }, \ + { "ua_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 23, offsetof(mavlink_open_drone_id_basic_id_t, ua_type) }, \ + { "uas_id", NULL, MAVLINK_TYPE_UINT8_T, 20, 24, offsetof(mavlink_open_drone_id_basic_id_t, uas_id) }, \ + } \ +} +#endif + +/** + * @brief Pack a open_drone_id_basic_id message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param id_type Indicates the format for the uas_id field of this message. + * @param ua_type Indicates the type of UA (Unmanned Aircraft). + * @param uas_id UAS (Unmanned Aircraft System) ID following the format specified by id_type. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_basic_id_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t id_type, uint8_t ua_type, const uint8_t *uas_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, id_type); + _mav_put_uint8_t(buf, 23, ua_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 24, uas_id, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN); +#else + mavlink_open_drone_id_basic_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.id_type = id_type; + packet.ua_type = ua_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); +} + +/** + * @brief Pack a open_drone_id_basic_id message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param id_type Indicates the format for the uas_id field of this message. + * @param ua_type Indicates the type of UA (Unmanned Aircraft). + * @param uas_id UAS (Unmanned Aircraft System) ID following the format specified by id_type. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_basic_id_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const uint8_t *id_or_mac,uint8_t id_type,uint8_t ua_type,const uint8_t *uas_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, id_type); + _mav_put_uint8_t(buf, 23, ua_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 24, uas_id, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN); +#else + mavlink_open_drone_id_basic_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.id_type = id_type; + packet.ua_type = ua_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); +} + +/** + * @brief Encode a open_drone_id_basic_id struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_basic_id C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_basic_id_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_basic_id_t* open_drone_id_basic_id) +{ + return mavlink_msg_open_drone_id_basic_id_pack(system_id, component_id, msg, open_drone_id_basic_id->target_system, open_drone_id_basic_id->target_component, open_drone_id_basic_id->id_or_mac, open_drone_id_basic_id->id_type, open_drone_id_basic_id->ua_type, open_drone_id_basic_id->uas_id); +} + +/** + * @brief Encode a open_drone_id_basic_id struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_basic_id C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_basic_id_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_basic_id_t* open_drone_id_basic_id) +{ + return mavlink_msg_open_drone_id_basic_id_pack_chan(system_id, component_id, chan, msg, open_drone_id_basic_id->target_system, open_drone_id_basic_id->target_component, open_drone_id_basic_id->id_or_mac, open_drone_id_basic_id->id_type, open_drone_id_basic_id->ua_type, open_drone_id_basic_id->uas_id); +} + +/** + * @brief Send a open_drone_id_basic_id message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param id_type Indicates the format for the uas_id field of this message. + * @param ua_type Indicates the type of UA (Unmanned Aircraft). + * @param uas_id UAS (Unmanned Aircraft System) ID following the format specified by id_type. Shall be filled with nulls in the unused portion of the field. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_open_drone_id_basic_id_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t id_type, uint8_t ua_type, const uint8_t *uas_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, id_type); + _mav_put_uint8_t(buf, 23, ua_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 24, uas_id, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); +#else + mavlink_open_drone_id_basic_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.id_type = id_type; + packet.ua_type = ua_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); +#endif +} + +/** + * @brief Send a open_drone_id_basic_id message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_open_drone_id_basic_id_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_basic_id_t* open_drone_id_basic_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_open_drone_id_basic_id_send(chan, open_drone_id_basic_id->target_system, open_drone_id_basic_id->target_component, open_drone_id_basic_id->id_or_mac, open_drone_id_basic_id->id_type, open_drone_id_basic_id->ua_type, open_drone_id_basic_id->uas_id); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID, (const char *)open_drone_id_basic_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_open_drone_id_basic_id_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t id_type, uint8_t ua_type, const uint8_t *uas_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, id_type); + _mav_put_uint8_t(buf, 23, ua_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 24, uas_id, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); +#else + mavlink_open_drone_id_basic_id_t *packet = (mavlink_open_drone_id_basic_id_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->id_type = id_type; + packet->ua_type = ua_type; + mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet->uas_id, uas_id, sizeof(uint8_t)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OPEN_DRONE_ID_BASIC_ID UNPACKING + + +/** + * @brief Get field target_system from open_drone_id_basic_id message + * + * @return System ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_basic_id_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from open_drone_id_basic_id message + * + * @return Component ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_basic_id_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field id_or_mac from open_drone_id_basic_id message + * + * @return Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + */ +static inline uint16_t mavlink_msg_open_drone_id_basic_id_get_id_or_mac(const mavlink_message_t* msg, uint8_t *id_or_mac) +{ + return _MAV_RETURN_uint8_t_array(msg, id_or_mac, 20, 2); +} + +/** + * @brief Get field id_type from open_drone_id_basic_id message + * + * @return Indicates the format for the uas_id field of this message. + */ +static inline uint8_t mavlink_msg_open_drone_id_basic_id_get_id_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 22); +} + +/** + * @brief Get field ua_type from open_drone_id_basic_id message + * + * @return Indicates the type of UA (Unmanned Aircraft). + */ +static inline uint8_t mavlink_msg_open_drone_id_basic_id_get_ua_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 23); +} + +/** + * @brief Get field uas_id from open_drone_id_basic_id message + * + * @return UAS (Unmanned Aircraft System) ID following the format specified by id_type. Shall be filled with nulls in the unused portion of the field. + */ +static inline uint16_t mavlink_msg_open_drone_id_basic_id_get_uas_id(const mavlink_message_t* msg, uint8_t *uas_id) +{ + return _MAV_RETURN_uint8_t_array(msg, uas_id, 20, 24); +} + +/** + * @brief Decode a open_drone_id_basic_id message into a struct + * + * @param msg The message to decode + * @param open_drone_id_basic_id C-struct to decode the message contents into + */ +static inline void mavlink_msg_open_drone_id_basic_id_decode(const mavlink_message_t* msg, mavlink_open_drone_id_basic_id_t* open_drone_id_basic_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + open_drone_id_basic_id->target_system = mavlink_msg_open_drone_id_basic_id_get_target_system(msg); + open_drone_id_basic_id->target_component = mavlink_msg_open_drone_id_basic_id_get_target_component(msg); + mavlink_msg_open_drone_id_basic_id_get_id_or_mac(msg, open_drone_id_basic_id->id_or_mac); + open_drone_id_basic_id->id_type = mavlink_msg_open_drone_id_basic_id_get_id_type(msg); + open_drone_id_basic_id->ua_type = mavlink_msg_open_drone_id_basic_id_get_ua_type(msg); + mavlink_msg_open_drone_id_basic_id_get_uas_id(msg, open_drone_id_basic_id->uas_id); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN? msg->len : MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN; + memset(open_drone_id_basic_id, 0, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN); + memcpy(open_drone_id_basic_id, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_location.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_location.h new file mode 100644 index 0000000000..0c28343933 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_location.h @@ -0,0 +1,655 @@ +#pragma once +// MESSAGE OPEN_DRONE_ID_LOCATION PACKING + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION 12901 + + +typedef struct __mavlink_open_drone_id_location_t { + int32_t latitude; /*< [degE7] Current latitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon).*/ + int32_t longitude; /*< [degE7] Current longitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon).*/ + float altitude_barometric; /*< [m] The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m.*/ + float altitude_geodetic; /*< [m] The geodetic altitude as defined by WGS84. If unknown: -1000 m.*/ + float height; /*< [m] The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m.*/ + float timestamp; /*< [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000.*/ + uint16_t direction; /*< [cdeg] Direction over ground (not heading, but direction of movement) measured clockwise from true North: 0 - 35999 centi-degrees. If unknown: 36100 centi-degrees.*/ + uint16_t speed_horizontal; /*< [cm/s] Ground speed. Positive only. If unknown: 25500 cm/s. If speed is larger than 25425 cm/s, use 25425 cm/s.*/ + int16_t speed_vertical; /*< [cm/s] The vertical speed. Up is positive. If unknown: 6300 cm/s. If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s.*/ + uint8_t target_system; /*< System ID (0 for broadcast).*/ + uint8_t target_component; /*< Component ID (0 for broadcast).*/ + uint8_t id_or_mac[20]; /*< Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */ + uint8_t status; /*< Indicates whether the unmanned aircraft is on the ground or in the air.*/ + uint8_t height_reference; /*< Indicates the reference point for the height field.*/ + uint8_t horizontal_accuracy; /*< The accuracy of the horizontal position.*/ + uint8_t vertical_accuracy; /*< The accuracy of the vertical position.*/ + uint8_t barometer_accuracy; /*< The accuracy of the barometric altitude.*/ + uint8_t speed_accuracy; /*< The accuracy of the horizontal and vertical speed.*/ + uint8_t timestamp_accuracy; /*< The accuracy of the timestamps.*/ +} mavlink_open_drone_id_location_t; + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN 59 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN 59 +#define MAVLINK_MSG_ID_12901_LEN 59 +#define MAVLINK_MSG_ID_12901_MIN_LEN 59 + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC 254 +#define MAVLINK_MSG_ID_12901_CRC 254 + +#define MAVLINK_MSG_OPEN_DRONE_ID_LOCATION_FIELD_ID_OR_MAC_LEN 20 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_LOCATION { \ + 12901, \ + "OPEN_DRONE_ID_LOCATION", \ + 19, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_open_drone_id_location_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_open_drone_id_location_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 32, offsetof(mavlink_open_drone_id_location_t, id_or_mac) }, \ + { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_open_drone_id_location_t, status) }, \ + { "direction", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_open_drone_id_location_t, direction) }, \ + { "speed_horizontal", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_open_drone_id_location_t, speed_horizontal) }, \ + { "speed_vertical", NULL, MAVLINK_TYPE_INT16_T, 0, 28, offsetof(mavlink_open_drone_id_location_t, speed_vertical) }, \ + { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_open_drone_id_location_t, latitude) }, \ + { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_open_drone_id_location_t, longitude) }, \ + { "altitude_barometric", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_open_drone_id_location_t, altitude_barometric) }, \ + { "altitude_geodetic", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_open_drone_id_location_t, altitude_geodetic) }, \ + { "height_reference", NULL, MAVLINK_TYPE_UINT8_T, 0, 53, offsetof(mavlink_open_drone_id_location_t, height_reference) }, \ + { "height", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_open_drone_id_location_t, height) }, \ + { "horizontal_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 54, offsetof(mavlink_open_drone_id_location_t, horizontal_accuracy) }, \ + { "vertical_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 55, offsetof(mavlink_open_drone_id_location_t, vertical_accuracy) }, \ + { "barometer_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 56, offsetof(mavlink_open_drone_id_location_t, barometer_accuracy) }, \ + { "speed_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 57, offsetof(mavlink_open_drone_id_location_t, speed_accuracy) }, \ + { "timestamp", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_open_drone_id_location_t, timestamp) }, \ + { "timestamp_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 58, offsetof(mavlink_open_drone_id_location_t, timestamp_accuracy) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_LOCATION { \ + "OPEN_DRONE_ID_LOCATION", \ + 19, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_open_drone_id_location_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_open_drone_id_location_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 32, offsetof(mavlink_open_drone_id_location_t, id_or_mac) }, \ + { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_open_drone_id_location_t, status) }, \ + { "direction", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_open_drone_id_location_t, direction) }, \ + { "speed_horizontal", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_open_drone_id_location_t, speed_horizontal) }, \ + { "speed_vertical", NULL, MAVLINK_TYPE_INT16_T, 0, 28, offsetof(mavlink_open_drone_id_location_t, speed_vertical) }, \ + { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_open_drone_id_location_t, latitude) }, \ + { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_open_drone_id_location_t, longitude) }, \ + { "altitude_barometric", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_open_drone_id_location_t, altitude_barometric) }, \ + { "altitude_geodetic", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_open_drone_id_location_t, altitude_geodetic) }, \ + { "height_reference", NULL, MAVLINK_TYPE_UINT8_T, 0, 53, offsetof(mavlink_open_drone_id_location_t, height_reference) }, \ + { "height", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_open_drone_id_location_t, height) }, \ + { "horizontal_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 54, offsetof(mavlink_open_drone_id_location_t, horizontal_accuracy) }, \ + { "vertical_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 55, offsetof(mavlink_open_drone_id_location_t, vertical_accuracy) }, \ + { "barometer_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 56, offsetof(mavlink_open_drone_id_location_t, barometer_accuracy) }, \ + { "speed_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 57, offsetof(mavlink_open_drone_id_location_t, speed_accuracy) }, \ + { "timestamp", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_open_drone_id_location_t, timestamp) }, \ + { "timestamp_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 58, offsetof(mavlink_open_drone_id_location_t, timestamp_accuracy) }, \ + } \ +} +#endif + +/** + * @brief Pack a open_drone_id_location message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param status Indicates whether the unmanned aircraft is on the ground or in the air. + * @param direction [cdeg] Direction over ground (not heading, but direction of movement) measured clockwise from true North: 0 - 35999 centi-degrees. If unknown: 36100 centi-degrees. + * @param speed_horizontal [cm/s] Ground speed. Positive only. If unknown: 25500 cm/s. If speed is larger than 25425 cm/s, use 25425 cm/s. + * @param speed_vertical [cm/s] The vertical speed. Up is positive. If unknown: 6300 cm/s. If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s. + * @param latitude [degE7] Current latitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + * @param longitude [degE7] Current longitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + * @param altitude_barometric [m] The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. + * @param altitude_geodetic [m] The geodetic altitude as defined by WGS84. If unknown: -1000 m. + * @param height_reference Indicates the reference point for the height field. + * @param height [m] The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m. + * @param horizontal_accuracy The accuracy of the horizontal position. + * @param vertical_accuracy The accuracy of the vertical position. + * @param barometer_accuracy The accuracy of the barometric altitude. + * @param speed_accuracy The accuracy of the horizontal and vertical speed. + * @param timestamp [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. + * @param timestamp_accuracy The accuracy of the timestamps. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_location_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t status, uint16_t direction, uint16_t speed_horizontal, int16_t speed_vertical, int32_t latitude, int32_t longitude, float altitude_barometric, float altitude_geodetic, uint8_t height_reference, float height, uint8_t horizontal_accuracy, uint8_t vertical_accuracy, uint8_t barometer_accuracy, uint8_t speed_accuracy, float timestamp, uint8_t timestamp_accuracy) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_float(buf, 8, altitude_barometric); + _mav_put_float(buf, 12, altitude_geodetic); + _mav_put_float(buf, 16, height); + _mav_put_float(buf, 20, timestamp); + _mav_put_uint16_t(buf, 24, direction); + _mav_put_uint16_t(buf, 26, speed_horizontal); + _mav_put_int16_t(buf, 28, speed_vertical); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 52, status); + _mav_put_uint8_t(buf, 53, height_reference); + _mav_put_uint8_t(buf, 54, horizontal_accuracy); + _mav_put_uint8_t(buf, 55, vertical_accuracy); + _mav_put_uint8_t(buf, 56, barometer_accuracy); + _mav_put_uint8_t(buf, 57, speed_accuracy); + _mav_put_uint8_t(buf, 58, timestamp_accuracy); + _mav_put_uint8_t_array(buf, 32, id_or_mac, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN); +#else + mavlink_open_drone_id_location_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude_barometric = altitude_barometric; + packet.altitude_geodetic = altitude_geodetic; + packet.height = height; + packet.timestamp = timestamp; + packet.direction = direction; + packet.speed_horizontal = speed_horizontal; + packet.speed_vertical = speed_vertical; + packet.target_system = target_system; + packet.target_component = target_component; + packet.status = status; + packet.height_reference = height_reference; + packet.horizontal_accuracy = horizontal_accuracy; + packet.vertical_accuracy = vertical_accuracy; + packet.barometer_accuracy = barometer_accuracy; + packet.speed_accuracy = speed_accuracy; + packet.timestamp_accuracy = timestamp_accuracy; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); +} + +/** + * @brief Pack a open_drone_id_location message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param status Indicates whether the unmanned aircraft is on the ground or in the air. + * @param direction [cdeg] Direction over ground (not heading, but direction of movement) measured clockwise from true North: 0 - 35999 centi-degrees. If unknown: 36100 centi-degrees. + * @param speed_horizontal [cm/s] Ground speed. Positive only. If unknown: 25500 cm/s. If speed is larger than 25425 cm/s, use 25425 cm/s. + * @param speed_vertical [cm/s] The vertical speed. Up is positive. If unknown: 6300 cm/s. If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s. + * @param latitude [degE7] Current latitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + * @param longitude [degE7] Current longitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + * @param altitude_barometric [m] The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. + * @param altitude_geodetic [m] The geodetic altitude as defined by WGS84. If unknown: -1000 m. + * @param height_reference Indicates the reference point for the height field. + * @param height [m] The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m. + * @param horizontal_accuracy The accuracy of the horizontal position. + * @param vertical_accuracy The accuracy of the vertical position. + * @param barometer_accuracy The accuracy of the barometric altitude. + * @param speed_accuracy The accuracy of the horizontal and vertical speed. + * @param timestamp [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. + * @param timestamp_accuracy The accuracy of the timestamps. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_location_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const uint8_t *id_or_mac,uint8_t status,uint16_t direction,uint16_t speed_horizontal,int16_t speed_vertical,int32_t latitude,int32_t longitude,float altitude_barometric,float altitude_geodetic,uint8_t height_reference,float height,uint8_t horizontal_accuracy,uint8_t vertical_accuracy,uint8_t barometer_accuracy,uint8_t speed_accuracy,float timestamp,uint8_t timestamp_accuracy) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_float(buf, 8, altitude_barometric); + _mav_put_float(buf, 12, altitude_geodetic); + _mav_put_float(buf, 16, height); + _mav_put_float(buf, 20, timestamp); + _mav_put_uint16_t(buf, 24, direction); + _mav_put_uint16_t(buf, 26, speed_horizontal); + _mav_put_int16_t(buf, 28, speed_vertical); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 52, status); + _mav_put_uint8_t(buf, 53, height_reference); + _mav_put_uint8_t(buf, 54, horizontal_accuracy); + _mav_put_uint8_t(buf, 55, vertical_accuracy); + _mav_put_uint8_t(buf, 56, barometer_accuracy); + _mav_put_uint8_t(buf, 57, speed_accuracy); + _mav_put_uint8_t(buf, 58, timestamp_accuracy); + _mav_put_uint8_t_array(buf, 32, id_or_mac, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN); +#else + mavlink_open_drone_id_location_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude_barometric = altitude_barometric; + packet.altitude_geodetic = altitude_geodetic; + packet.height = height; + packet.timestamp = timestamp; + packet.direction = direction; + packet.speed_horizontal = speed_horizontal; + packet.speed_vertical = speed_vertical; + packet.target_system = target_system; + packet.target_component = target_component; + packet.status = status; + packet.height_reference = height_reference; + packet.horizontal_accuracy = horizontal_accuracy; + packet.vertical_accuracy = vertical_accuracy; + packet.barometer_accuracy = barometer_accuracy; + packet.speed_accuracy = speed_accuracy; + packet.timestamp_accuracy = timestamp_accuracy; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); +} + +/** + * @brief Encode a open_drone_id_location struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_location C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_location_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_location_t* open_drone_id_location) +{ + return mavlink_msg_open_drone_id_location_pack(system_id, component_id, msg, open_drone_id_location->target_system, open_drone_id_location->target_component, open_drone_id_location->id_or_mac, open_drone_id_location->status, open_drone_id_location->direction, open_drone_id_location->speed_horizontal, open_drone_id_location->speed_vertical, open_drone_id_location->latitude, open_drone_id_location->longitude, open_drone_id_location->altitude_barometric, open_drone_id_location->altitude_geodetic, open_drone_id_location->height_reference, open_drone_id_location->height, open_drone_id_location->horizontal_accuracy, open_drone_id_location->vertical_accuracy, open_drone_id_location->barometer_accuracy, open_drone_id_location->speed_accuracy, open_drone_id_location->timestamp, open_drone_id_location->timestamp_accuracy); +} + +/** + * @brief Encode a open_drone_id_location struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_location C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_location_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_location_t* open_drone_id_location) +{ + return mavlink_msg_open_drone_id_location_pack_chan(system_id, component_id, chan, msg, open_drone_id_location->target_system, open_drone_id_location->target_component, open_drone_id_location->id_or_mac, open_drone_id_location->status, open_drone_id_location->direction, open_drone_id_location->speed_horizontal, open_drone_id_location->speed_vertical, open_drone_id_location->latitude, open_drone_id_location->longitude, open_drone_id_location->altitude_barometric, open_drone_id_location->altitude_geodetic, open_drone_id_location->height_reference, open_drone_id_location->height, open_drone_id_location->horizontal_accuracy, open_drone_id_location->vertical_accuracy, open_drone_id_location->barometer_accuracy, open_drone_id_location->speed_accuracy, open_drone_id_location->timestamp, open_drone_id_location->timestamp_accuracy); +} + +/** + * @brief Send a open_drone_id_location message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param status Indicates whether the unmanned aircraft is on the ground or in the air. + * @param direction [cdeg] Direction over ground (not heading, but direction of movement) measured clockwise from true North: 0 - 35999 centi-degrees. If unknown: 36100 centi-degrees. + * @param speed_horizontal [cm/s] Ground speed. Positive only. If unknown: 25500 cm/s. If speed is larger than 25425 cm/s, use 25425 cm/s. + * @param speed_vertical [cm/s] The vertical speed. Up is positive. If unknown: 6300 cm/s. If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s. + * @param latitude [degE7] Current latitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + * @param longitude [degE7] Current longitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + * @param altitude_barometric [m] The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. + * @param altitude_geodetic [m] The geodetic altitude as defined by WGS84. If unknown: -1000 m. + * @param height_reference Indicates the reference point for the height field. + * @param height [m] The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m. + * @param horizontal_accuracy The accuracy of the horizontal position. + * @param vertical_accuracy The accuracy of the vertical position. + * @param barometer_accuracy The accuracy of the barometric altitude. + * @param speed_accuracy The accuracy of the horizontal and vertical speed. + * @param timestamp [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. + * @param timestamp_accuracy The accuracy of the timestamps. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_open_drone_id_location_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t status, uint16_t direction, uint16_t speed_horizontal, int16_t speed_vertical, int32_t latitude, int32_t longitude, float altitude_barometric, float altitude_geodetic, uint8_t height_reference, float height, uint8_t horizontal_accuracy, uint8_t vertical_accuracy, uint8_t barometer_accuracy, uint8_t speed_accuracy, float timestamp, uint8_t timestamp_accuracy) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_float(buf, 8, altitude_barometric); + _mav_put_float(buf, 12, altitude_geodetic); + _mav_put_float(buf, 16, height); + _mav_put_float(buf, 20, timestamp); + _mav_put_uint16_t(buf, 24, direction); + _mav_put_uint16_t(buf, 26, speed_horizontal); + _mav_put_int16_t(buf, 28, speed_vertical); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 52, status); + _mav_put_uint8_t(buf, 53, height_reference); + _mav_put_uint8_t(buf, 54, horizontal_accuracy); + _mav_put_uint8_t(buf, 55, vertical_accuracy); + _mav_put_uint8_t(buf, 56, barometer_accuracy); + _mav_put_uint8_t(buf, 57, speed_accuracy); + _mav_put_uint8_t(buf, 58, timestamp_accuracy); + _mav_put_uint8_t_array(buf, 32, id_or_mac, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); +#else + mavlink_open_drone_id_location_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude_barometric = altitude_barometric; + packet.altitude_geodetic = altitude_geodetic; + packet.height = height; + packet.timestamp = timestamp; + packet.direction = direction; + packet.speed_horizontal = speed_horizontal; + packet.speed_vertical = speed_vertical; + packet.target_system = target_system; + packet.target_component = target_component; + packet.status = status; + packet.height_reference = height_reference; + packet.horizontal_accuracy = horizontal_accuracy; + packet.vertical_accuracy = vertical_accuracy; + packet.barometer_accuracy = barometer_accuracy; + packet.speed_accuracy = speed_accuracy; + packet.timestamp_accuracy = timestamp_accuracy; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); +#endif +} + +/** + * @brief Send a open_drone_id_location message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_open_drone_id_location_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_location_t* open_drone_id_location) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_open_drone_id_location_send(chan, open_drone_id_location->target_system, open_drone_id_location->target_component, open_drone_id_location->id_or_mac, open_drone_id_location->status, open_drone_id_location->direction, open_drone_id_location->speed_horizontal, open_drone_id_location->speed_vertical, open_drone_id_location->latitude, open_drone_id_location->longitude, open_drone_id_location->altitude_barometric, open_drone_id_location->altitude_geodetic, open_drone_id_location->height_reference, open_drone_id_location->height, open_drone_id_location->horizontal_accuracy, open_drone_id_location->vertical_accuracy, open_drone_id_location->barometer_accuracy, open_drone_id_location->speed_accuracy, open_drone_id_location->timestamp, open_drone_id_location->timestamp_accuracy); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION, (const char *)open_drone_id_location, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_open_drone_id_location_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t status, uint16_t direction, uint16_t speed_horizontal, int16_t speed_vertical, int32_t latitude, int32_t longitude, float altitude_barometric, float altitude_geodetic, uint8_t height_reference, float height, uint8_t horizontal_accuracy, uint8_t vertical_accuracy, uint8_t barometer_accuracy, uint8_t speed_accuracy, float timestamp, uint8_t timestamp_accuracy) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_float(buf, 8, altitude_barometric); + _mav_put_float(buf, 12, altitude_geodetic); + _mav_put_float(buf, 16, height); + _mav_put_float(buf, 20, timestamp); + _mav_put_uint16_t(buf, 24, direction); + _mav_put_uint16_t(buf, 26, speed_horizontal); + _mav_put_int16_t(buf, 28, speed_vertical); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 52, status); + _mav_put_uint8_t(buf, 53, height_reference); + _mav_put_uint8_t(buf, 54, horizontal_accuracy); + _mav_put_uint8_t(buf, 55, vertical_accuracy); + _mav_put_uint8_t(buf, 56, barometer_accuracy); + _mav_put_uint8_t(buf, 57, speed_accuracy); + _mav_put_uint8_t(buf, 58, timestamp_accuracy); + _mav_put_uint8_t_array(buf, 32, id_or_mac, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); +#else + mavlink_open_drone_id_location_t *packet = (mavlink_open_drone_id_location_t *)msgbuf; + packet->latitude = latitude; + packet->longitude = longitude; + packet->altitude_barometric = altitude_barometric; + packet->altitude_geodetic = altitude_geodetic; + packet->height = height; + packet->timestamp = timestamp; + packet->direction = direction; + packet->speed_horizontal = speed_horizontal; + packet->speed_vertical = speed_vertical; + packet->target_system = target_system; + packet->target_component = target_component; + packet->status = status; + packet->height_reference = height_reference; + packet->horizontal_accuracy = horizontal_accuracy; + packet->vertical_accuracy = vertical_accuracy; + packet->barometer_accuracy = barometer_accuracy; + packet->speed_accuracy = speed_accuracy; + packet->timestamp_accuracy = timestamp_accuracy; + mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OPEN_DRONE_ID_LOCATION UNPACKING + + +/** + * @brief Get field target_system from open_drone_id_location message + * + * @return System ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 30); +} + +/** + * @brief Get field target_component from open_drone_id_location message + * + * @return Component ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 31); +} + +/** + * @brief Get field id_or_mac from open_drone_id_location message + * + * @return Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + */ +static inline uint16_t mavlink_msg_open_drone_id_location_get_id_or_mac(const mavlink_message_t* msg, uint8_t *id_or_mac) +{ + return _MAV_RETURN_uint8_t_array(msg, id_or_mac, 20, 32); +} + +/** + * @brief Get field status from open_drone_id_location message + * + * @return Indicates whether the unmanned aircraft is on the ground or in the air. + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 52); +} + +/** + * @brief Get field direction from open_drone_id_location message + * + * @return [cdeg] Direction over ground (not heading, but direction of movement) measured clockwise from true North: 0 - 35999 centi-degrees. If unknown: 36100 centi-degrees. + */ +static inline uint16_t mavlink_msg_open_drone_id_location_get_direction(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field speed_horizontal from open_drone_id_location message + * + * @return [cm/s] Ground speed. Positive only. If unknown: 25500 cm/s. If speed is larger than 25425 cm/s, use 25425 cm/s. + */ +static inline uint16_t mavlink_msg_open_drone_id_location_get_speed_horizontal(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 26); +} + +/** + * @brief Get field speed_vertical from open_drone_id_location message + * + * @return [cm/s] The vertical speed. Up is positive. If unknown: 6300 cm/s. If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s. + */ +static inline int16_t mavlink_msg_open_drone_id_location_get_speed_vertical(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 28); +} + +/** + * @brief Get field latitude from open_drone_id_location message + * + * @return [degE7] Current latitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + */ +static inline int32_t mavlink_msg_open_drone_id_location_get_latitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field longitude from open_drone_id_location message + * + * @return [degE7] Current longitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + */ +static inline int32_t mavlink_msg_open_drone_id_location_get_longitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field altitude_barometric from open_drone_id_location message + * + * @return [m] The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. + */ +static inline float mavlink_msg_open_drone_id_location_get_altitude_barometric(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field altitude_geodetic from open_drone_id_location message + * + * @return [m] The geodetic altitude as defined by WGS84. If unknown: -1000 m. + */ +static inline float mavlink_msg_open_drone_id_location_get_altitude_geodetic(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field height_reference from open_drone_id_location message + * + * @return Indicates the reference point for the height field. + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_height_reference(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 53); +} + +/** + * @brief Get field height from open_drone_id_location message + * + * @return [m] The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m. + */ +static inline float mavlink_msg_open_drone_id_location_get_height(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field horizontal_accuracy from open_drone_id_location message + * + * @return The accuracy of the horizontal position. + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_horizontal_accuracy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 54); +} + +/** + * @brief Get field vertical_accuracy from open_drone_id_location message + * + * @return The accuracy of the vertical position. + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_vertical_accuracy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 55); +} + +/** + * @brief Get field barometer_accuracy from open_drone_id_location message + * + * @return The accuracy of the barometric altitude. + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_barometer_accuracy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 56); +} + +/** + * @brief Get field speed_accuracy from open_drone_id_location message + * + * @return The accuracy of the horizontal and vertical speed. + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_speed_accuracy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 57); +} + +/** + * @brief Get field timestamp from open_drone_id_location message + * + * @return [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. + */ +static inline float mavlink_msg_open_drone_id_location_get_timestamp(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field timestamp_accuracy from open_drone_id_location message + * + * @return The accuracy of the timestamps. + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_timestamp_accuracy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 58); +} + +/** + * @brief Decode a open_drone_id_location message into a struct + * + * @param msg The message to decode + * @param open_drone_id_location C-struct to decode the message contents into + */ +static inline void mavlink_msg_open_drone_id_location_decode(const mavlink_message_t* msg, mavlink_open_drone_id_location_t* open_drone_id_location) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + open_drone_id_location->latitude = mavlink_msg_open_drone_id_location_get_latitude(msg); + open_drone_id_location->longitude = mavlink_msg_open_drone_id_location_get_longitude(msg); + open_drone_id_location->altitude_barometric = mavlink_msg_open_drone_id_location_get_altitude_barometric(msg); + open_drone_id_location->altitude_geodetic = mavlink_msg_open_drone_id_location_get_altitude_geodetic(msg); + open_drone_id_location->height = mavlink_msg_open_drone_id_location_get_height(msg); + open_drone_id_location->timestamp = mavlink_msg_open_drone_id_location_get_timestamp(msg); + open_drone_id_location->direction = mavlink_msg_open_drone_id_location_get_direction(msg); + open_drone_id_location->speed_horizontal = mavlink_msg_open_drone_id_location_get_speed_horizontal(msg); + open_drone_id_location->speed_vertical = mavlink_msg_open_drone_id_location_get_speed_vertical(msg); + open_drone_id_location->target_system = mavlink_msg_open_drone_id_location_get_target_system(msg); + open_drone_id_location->target_component = mavlink_msg_open_drone_id_location_get_target_component(msg); + mavlink_msg_open_drone_id_location_get_id_or_mac(msg, open_drone_id_location->id_or_mac); + open_drone_id_location->status = mavlink_msg_open_drone_id_location_get_status(msg); + open_drone_id_location->height_reference = mavlink_msg_open_drone_id_location_get_height_reference(msg); + open_drone_id_location->horizontal_accuracy = mavlink_msg_open_drone_id_location_get_horizontal_accuracy(msg); + open_drone_id_location->vertical_accuracy = mavlink_msg_open_drone_id_location_get_vertical_accuracy(msg); + open_drone_id_location->barometer_accuracy = mavlink_msg_open_drone_id_location_get_barometer_accuracy(msg); + open_drone_id_location->speed_accuracy = mavlink_msg_open_drone_id_location_get_speed_accuracy(msg); + open_drone_id_location->timestamp_accuracy = mavlink_msg_open_drone_id_location_get_timestamp_accuracy(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN? msg->len : MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN; + memset(open_drone_id_location, 0, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN); + memcpy(open_drone_id_location, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_message_pack.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_message_pack.h new file mode 100644 index 0000000000..16407cfb35 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_message_pack.h @@ -0,0 +1,305 @@ +#pragma once +// MESSAGE OPEN_DRONE_ID_MESSAGE_PACK PACKING + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK 12915 + + +typedef struct __mavlink_open_drone_id_message_pack_t { + uint8_t target_system; /*< System ID (0 for broadcast).*/ + uint8_t target_component; /*< Component ID (0 for broadcast).*/ + uint8_t single_message_size; /*< [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specificed to have this length.*/ + uint8_t msg_pack_size; /*< Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 10.*/ + uint8_t messages[250]; /*< Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field.*/ +} mavlink_open_drone_id_message_pack_t; + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN 254 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN 254 +#define MAVLINK_MSG_ID_12915_LEN 254 +#define MAVLINK_MSG_ID_12915_MIN_LEN 254 + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC 62 +#define MAVLINK_MSG_ID_12915_CRC 62 + +#define MAVLINK_MSG_OPEN_DRONE_ID_MESSAGE_PACK_FIELD_MESSAGES_LEN 250 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_MESSAGE_PACK { \ + 12915, \ + "OPEN_DRONE_ID_MESSAGE_PACK", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_message_pack_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_message_pack_t, target_component) }, \ + { "single_message_size", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_open_drone_id_message_pack_t, single_message_size) }, \ + { "msg_pack_size", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_open_drone_id_message_pack_t, msg_pack_size) }, \ + { "messages", NULL, MAVLINK_TYPE_UINT8_T, 250, 4, offsetof(mavlink_open_drone_id_message_pack_t, messages) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_MESSAGE_PACK { \ + "OPEN_DRONE_ID_MESSAGE_PACK", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_message_pack_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_message_pack_t, target_component) }, \ + { "single_message_size", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_open_drone_id_message_pack_t, single_message_size) }, \ + { "msg_pack_size", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_open_drone_id_message_pack_t, msg_pack_size) }, \ + { "messages", NULL, MAVLINK_TYPE_UINT8_T, 250, 4, offsetof(mavlink_open_drone_id_message_pack_t, messages) }, \ + } \ +} +#endif + +/** + * @brief Pack a open_drone_id_message_pack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param single_message_size [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specificed to have this length. + * @param msg_pack_size Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 10. + * @param messages Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_message_pack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t single_message_size, uint8_t msg_pack_size, const uint8_t *messages) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, single_message_size); + _mav_put_uint8_t(buf, 3, msg_pack_size); + _mav_put_uint8_t_array(buf, 4, messages, 250); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN); +#else + mavlink_open_drone_id_message_pack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.single_message_size = single_message_size; + packet.msg_pack_size = msg_pack_size; + mav_array_memcpy(packet.messages, messages, sizeof(uint8_t)*250); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); +} + +/** + * @brief Pack a open_drone_id_message_pack message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param single_message_size [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specificed to have this length. + * @param msg_pack_size Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 10. + * @param messages Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_message_pack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t single_message_size,uint8_t msg_pack_size,const uint8_t *messages) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, single_message_size); + _mav_put_uint8_t(buf, 3, msg_pack_size); + _mav_put_uint8_t_array(buf, 4, messages, 250); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN); +#else + mavlink_open_drone_id_message_pack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.single_message_size = single_message_size; + packet.msg_pack_size = msg_pack_size; + mav_array_memcpy(packet.messages, messages, sizeof(uint8_t)*250); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); +} + +/** + * @brief Encode a open_drone_id_message_pack struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_message_pack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_message_pack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_message_pack_t* open_drone_id_message_pack) +{ + return mavlink_msg_open_drone_id_message_pack_pack(system_id, component_id, msg, open_drone_id_message_pack->target_system, open_drone_id_message_pack->target_component, open_drone_id_message_pack->single_message_size, open_drone_id_message_pack->msg_pack_size, open_drone_id_message_pack->messages); +} + +/** + * @brief Encode a open_drone_id_message_pack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_message_pack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_message_pack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_message_pack_t* open_drone_id_message_pack) +{ + return mavlink_msg_open_drone_id_message_pack_pack_chan(system_id, component_id, chan, msg, open_drone_id_message_pack->target_system, open_drone_id_message_pack->target_component, open_drone_id_message_pack->single_message_size, open_drone_id_message_pack->msg_pack_size, open_drone_id_message_pack->messages); +} + +/** + * @brief Send a open_drone_id_message_pack message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param single_message_size [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specificed to have this length. + * @param msg_pack_size Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 10. + * @param messages Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_open_drone_id_message_pack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t single_message_size, uint8_t msg_pack_size, const uint8_t *messages) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, single_message_size); + _mav_put_uint8_t(buf, 3, msg_pack_size); + _mav_put_uint8_t_array(buf, 4, messages, 250); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); +#else + mavlink_open_drone_id_message_pack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.single_message_size = single_message_size; + packet.msg_pack_size = msg_pack_size; + mav_array_memcpy(packet.messages, messages, sizeof(uint8_t)*250); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); +#endif +} + +/** + * @brief Send a open_drone_id_message_pack message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_open_drone_id_message_pack_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_message_pack_t* open_drone_id_message_pack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_open_drone_id_message_pack_send(chan, open_drone_id_message_pack->target_system, open_drone_id_message_pack->target_component, open_drone_id_message_pack->single_message_size, open_drone_id_message_pack->msg_pack_size, open_drone_id_message_pack->messages); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK, (const char *)open_drone_id_message_pack, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_open_drone_id_message_pack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t single_message_size, uint8_t msg_pack_size, const uint8_t *messages) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, single_message_size); + _mav_put_uint8_t(buf, 3, msg_pack_size); + _mav_put_uint8_t_array(buf, 4, messages, 250); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); +#else + mavlink_open_drone_id_message_pack_t *packet = (mavlink_open_drone_id_message_pack_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->single_message_size = single_message_size; + packet->msg_pack_size = msg_pack_size; + mav_array_memcpy(packet->messages, messages, sizeof(uint8_t)*250); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OPEN_DRONE_ID_MESSAGE_PACK UNPACKING + + +/** + * @brief Get field target_system from open_drone_id_message_pack message + * + * @return System ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_message_pack_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from open_drone_id_message_pack message + * + * @return Component ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_message_pack_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field single_message_size from open_drone_id_message_pack message + * + * @return [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specificed to have this length. + */ +static inline uint8_t mavlink_msg_open_drone_id_message_pack_get_single_message_size(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field msg_pack_size from open_drone_id_message_pack message + * + * @return Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 10. + */ +static inline uint8_t mavlink_msg_open_drone_id_message_pack_get_msg_pack_size(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field messages from open_drone_id_message_pack message + * + * @return Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field. + */ +static inline uint16_t mavlink_msg_open_drone_id_message_pack_get_messages(const mavlink_message_t* msg, uint8_t *messages) +{ + return _MAV_RETURN_uint8_t_array(msg, messages, 250, 4); +} + +/** + * @brief Decode a open_drone_id_message_pack message into a struct + * + * @param msg The message to decode + * @param open_drone_id_message_pack C-struct to decode the message contents into + */ +static inline void mavlink_msg_open_drone_id_message_pack_decode(const mavlink_message_t* msg, mavlink_open_drone_id_message_pack_t* open_drone_id_message_pack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + open_drone_id_message_pack->target_system = mavlink_msg_open_drone_id_message_pack_get_target_system(msg); + open_drone_id_message_pack->target_component = mavlink_msg_open_drone_id_message_pack_get_target_component(msg); + open_drone_id_message_pack->single_message_size = mavlink_msg_open_drone_id_message_pack_get_single_message_size(msg); + open_drone_id_message_pack->msg_pack_size = mavlink_msg_open_drone_id_message_pack_get_msg_pack_size(msg); + mavlink_msg_open_drone_id_message_pack_get_messages(msg, open_drone_id_message_pack->messages); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN? msg->len : MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN; + memset(open_drone_id_message_pack, 0, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN); + memcpy(open_drone_id_message_pack, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_operator_id.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_operator_id.h new file mode 100644 index 0000000000..f3bf5aff29 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_operator_id.h @@ -0,0 +1,306 @@ +#pragma once +// MESSAGE OPEN_DRONE_ID_OPERATOR_ID PACKING + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID 12905 + + +typedef struct __mavlink_open_drone_id_operator_id_t { + uint8_t target_system; /*< System ID (0 for broadcast).*/ + uint8_t target_component; /*< Component ID (0 for broadcast).*/ + uint8_t id_or_mac[20]; /*< Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */ + uint8_t operator_id_type; /*< Indicates the type of the operator_id field.*/ + char operator_id[20]; /*< Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field.*/ +} mavlink_open_drone_id_operator_id_t; + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN 43 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN 43 +#define MAVLINK_MSG_ID_12905_LEN 43 +#define MAVLINK_MSG_ID_12905_MIN_LEN 43 + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC 49 +#define MAVLINK_MSG_ID_12905_CRC 49 + +#define MAVLINK_MSG_OPEN_DRONE_ID_OPERATOR_ID_FIELD_ID_OR_MAC_LEN 20 +#define MAVLINK_MSG_OPEN_DRONE_ID_OPERATOR_ID_FIELD_OPERATOR_ID_LEN 20 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_OPERATOR_ID { \ + 12905, \ + "OPEN_DRONE_ID_OPERATOR_ID", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_operator_id_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_operator_id_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 2, offsetof(mavlink_open_drone_id_operator_id_t, id_or_mac) }, \ + { "operator_id_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_open_drone_id_operator_id_t, operator_id_type) }, \ + { "operator_id", NULL, MAVLINK_TYPE_CHAR, 20, 23, offsetof(mavlink_open_drone_id_operator_id_t, operator_id) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_OPERATOR_ID { \ + "OPEN_DRONE_ID_OPERATOR_ID", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_operator_id_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_operator_id_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 2, offsetof(mavlink_open_drone_id_operator_id_t, id_or_mac) }, \ + { "operator_id_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_open_drone_id_operator_id_t, operator_id_type) }, \ + { "operator_id", NULL, MAVLINK_TYPE_CHAR, 20, 23, offsetof(mavlink_open_drone_id_operator_id_t, operator_id) }, \ + } \ +} +#endif + +/** + * @brief Pack a open_drone_id_operator_id message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param operator_id_type Indicates the type of the operator_id field. + * @param operator_id Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_operator_id_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_id_type, const char *operator_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, operator_id_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, operator_id, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN); +#else + mavlink_open_drone_id_operator_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.operator_id_type = operator_id_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.operator_id, operator_id, sizeof(char)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); +} + +/** + * @brief Pack a open_drone_id_operator_id message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param operator_id_type Indicates the type of the operator_id field. + * @param operator_id Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_operator_id_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const uint8_t *id_or_mac,uint8_t operator_id_type,const char *operator_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, operator_id_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, operator_id, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN); +#else + mavlink_open_drone_id_operator_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.operator_id_type = operator_id_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.operator_id, operator_id, sizeof(char)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); +} + +/** + * @brief Encode a open_drone_id_operator_id struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_operator_id C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_operator_id_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_operator_id_t* open_drone_id_operator_id) +{ + return mavlink_msg_open_drone_id_operator_id_pack(system_id, component_id, msg, open_drone_id_operator_id->target_system, open_drone_id_operator_id->target_component, open_drone_id_operator_id->id_or_mac, open_drone_id_operator_id->operator_id_type, open_drone_id_operator_id->operator_id); +} + +/** + * @brief Encode a open_drone_id_operator_id struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_operator_id C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_operator_id_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_operator_id_t* open_drone_id_operator_id) +{ + return mavlink_msg_open_drone_id_operator_id_pack_chan(system_id, component_id, chan, msg, open_drone_id_operator_id->target_system, open_drone_id_operator_id->target_component, open_drone_id_operator_id->id_or_mac, open_drone_id_operator_id->operator_id_type, open_drone_id_operator_id->operator_id); +} + +/** + * @brief Send a open_drone_id_operator_id message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param operator_id_type Indicates the type of the operator_id field. + * @param operator_id Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_open_drone_id_operator_id_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_id_type, const char *operator_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, operator_id_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, operator_id, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); +#else + mavlink_open_drone_id_operator_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.operator_id_type = operator_id_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.operator_id, operator_id, sizeof(char)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); +#endif +} + +/** + * @brief Send a open_drone_id_operator_id message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_open_drone_id_operator_id_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_operator_id_t* open_drone_id_operator_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_open_drone_id_operator_id_send(chan, open_drone_id_operator_id->target_system, open_drone_id_operator_id->target_component, open_drone_id_operator_id->id_or_mac, open_drone_id_operator_id->operator_id_type, open_drone_id_operator_id->operator_id); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID, (const char *)open_drone_id_operator_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_open_drone_id_operator_id_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_id_type, const char *operator_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, operator_id_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, operator_id, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); +#else + mavlink_open_drone_id_operator_id_t *packet = (mavlink_open_drone_id_operator_id_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->operator_id_type = operator_id_type; + mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet->operator_id, operator_id, sizeof(char)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OPEN_DRONE_ID_OPERATOR_ID UNPACKING + + +/** + * @brief Get field target_system from open_drone_id_operator_id message + * + * @return System ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_operator_id_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from open_drone_id_operator_id message + * + * @return Component ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_operator_id_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field id_or_mac from open_drone_id_operator_id message + * + * @return Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + */ +static inline uint16_t mavlink_msg_open_drone_id_operator_id_get_id_or_mac(const mavlink_message_t* msg, uint8_t *id_or_mac) +{ + return _MAV_RETURN_uint8_t_array(msg, id_or_mac, 20, 2); +} + +/** + * @brief Get field operator_id_type from open_drone_id_operator_id message + * + * @return Indicates the type of the operator_id field. + */ +static inline uint8_t mavlink_msg_open_drone_id_operator_id_get_operator_id_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 22); +} + +/** + * @brief Get field operator_id from open_drone_id_operator_id message + * + * @return Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + */ +static inline uint16_t mavlink_msg_open_drone_id_operator_id_get_operator_id(const mavlink_message_t* msg, char *operator_id) +{ + return _MAV_RETURN_char_array(msg, operator_id, 20, 23); +} + +/** + * @brief Decode a open_drone_id_operator_id message into a struct + * + * @param msg The message to decode + * @param open_drone_id_operator_id C-struct to decode the message contents into + */ +static inline void mavlink_msg_open_drone_id_operator_id_decode(const mavlink_message_t* msg, mavlink_open_drone_id_operator_id_t* open_drone_id_operator_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + open_drone_id_operator_id->target_system = mavlink_msg_open_drone_id_operator_id_get_target_system(msg); + open_drone_id_operator_id->target_component = mavlink_msg_open_drone_id_operator_id_get_target_component(msg); + mavlink_msg_open_drone_id_operator_id_get_id_or_mac(msg, open_drone_id_operator_id->id_or_mac); + open_drone_id_operator_id->operator_id_type = mavlink_msg_open_drone_id_operator_id_get_operator_id_type(msg); + mavlink_msg_open_drone_id_operator_id_get_operator_id(msg, open_drone_id_operator_id->operator_id); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN? msg->len : MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN; + memset(open_drone_id_operator_id, 0, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN); + memcpy(open_drone_id_operator_id, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_self_id.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_self_id.h new file mode 100644 index 0000000000..0750472ace --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_self_id.h @@ -0,0 +1,306 @@ +#pragma once +// MESSAGE OPEN_DRONE_ID_SELF_ID PACKING + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID 12903 + + +typedef struct __mavlink_open_drone_id_self_id_t { + uint8_t target_system; /*< System ID (0 for broadcast).*/ + uint8_t target_component; /*< Component ID (0 for broadcast).*/ + uint8_t id_or_mac[20]; /*< Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */ + uint8_t description_type; /*< Indicates the type of the description field.*/ + char description[23]; /*< Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field.*/ +} mavlink_open_drone_id_self_id_t; + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN 46 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN 46 +#define MAVLINK_MSG_ID_12903_LEN 46 +#define MAVLINK_MSG_ID_12903_MIN_LEN 46 + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC 249 +#define MAVLINK_MSG_ID_12903_CRC 249 + +#define MAVLINK_MSG_OPEN_DRONE_ID_SELF_ID_FIELD_ID_OR_MAC_LEN 20 +#define MAVLINK_MSG_OPEN_DRONE_ID_SELF_ID_FIELD_DESCRIPTION_LEN 23 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SELF_ID { \ + 12903, \ + "OPEN_DRONE_ID_SELF_ID", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_self_id_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_self_id_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 2, offsetof(mavlink_open_drone_id_self_id_t, id_or_mac) }, \ + { "description_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_open_drone_id_self_id_t, description_type) }, \ + { "description", NULL, MAVLINK_TYPE_CHAR, 23, 23, offsetof(mavlink_open_drone_id_self_id_t, description) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SELF_ID { \ + "OPEN_DRONE_ID_SELF_ID", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_self_id_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_self_id_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 2, offsetof(mavlink_open_drone_id_self_id_t, id_or_mac) }, \ + { "description_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_open_drone_id_self_id_t, description_type) }, \ + { "description", NULL, MAVLINK_TYPE_CHAR, 23, 23, offsetof(mavlink_open_drone_id_self_id_t, description) }, \ + } \ +} +#endif + +/** + * @brief Pack a open_drone_id_self_id message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param description_type Indicates the type of the description field. + * @param description Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_self_id_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t description_type, const char *description) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, description_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, description, 23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN); +#else + mavlink_open_drone_id_self_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.description_type = description_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.description, description, sizeof(char)*23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); +} + +/** + * @brief Pack a open_drone_id_self_id message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param description_type Indicates the type of the description field. + * @param description Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_self_id_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const uint8_t *id_or_mac,uint8_t description_type,const char *description) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, description_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, description, 23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN); +#else + mavlink_open_drone_id_self_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.description_type = description_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.description, description, sizeof(char)*23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); +} + +/** + * @brief Encode a open_drone_id_self_id struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_self_id C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_self_id_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_self_id_t* open_drone_id_self_id) +{ + return mavlink_msg_open_drone_id_self_id_pack(system_id, component_id, msg, open_drone_id_self_id->target_system, open_drone_id_self_id->target_component, open_drone_id_self_id->id_or_mac, open_drone_id_self_id->description_type, open_drone_id_self_id->description); +} + +/** + * @brief Encode a open_drone_id_self_id struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_self_id C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_self_id_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_self_id_t* open_drone_id_self_id) +{ + return mavlink_msg_open_drone_id_self_id_pack_chan(system_id, component_id, chan, msg, open_drone_id_self_id->target_system, open_drone_id_self_id->target_component, open_drone_id_self_id->id_or_mac, open_drone_id_self_id->description_type, open_drone_id_self_id->description); +} + +/** + * @brief Send a open_drone_id_self_id message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param description_type Indicates the type of the description field. + * @param description Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_open_drone_id_self_id_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t description_type, const char *description) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, description_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, description, 23); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); +#else + mavlink_open_drone_id_self_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.description_type = description_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.description, description, sizeof(char)*23); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); +#endif +} + +/** + * @brief Send a open_drone_id_self_id message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_open_drone_id_self_id_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_self_id_t* open_drone_id_self_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_open_drone_id_self_id_send(chan, open_drone_id_self_id->target_system, open_drone_id_self_id->target_component, open_drone_id_self_id->id_or_mac, open_drone_id_self_id->description_type, open_drone_id_self_id->description); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID, (const char *)open_drone_id_self_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_open_drone_id_self_id_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t description_type, const char *description) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, description_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, description, 23); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); +#else + mavlink_open_drone_id_self_id_t *packet = (mavlink_open_drone_id_self_id_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->description_type = description_type; + mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet->description, description, sizeof(char)*23); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OPEN_DRONE_ID_SELF_ID UNPACKING + + +/** + * @brief Get field target_system from open_drone_id_self_id message + * + * @return System ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_self_id_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from open_drone_id_self_id message + * + * @return Component ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_self_id_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field id_or_mac from open_drone_id_self_id message + * + * @return Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + */ +static inline uint16_t mavlink_msg_open_drone_id_self_id_get_id_or_mac(const mavlink_message_t* msg, uint8_t *id_or_mac) +{ + return _MAV_RETURN_uint8_t_array(msg, id_or_mac, 20, 2); +} + +/** + * @brief Get field description_type from open_drone_id_self_id message + * + * @return Indicates the type of the description field. + */ +static inline uint8_t mavlink_msg_open_drone_id_self_id_get_description_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 22); +} + +/** + * @brief Get field description from open_drone_id_self_id message + * + * @return Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + */ +static inline uint16_t mavlink_msg_open_drone_id_self_id_get_description(const mavlink_message_t* msg, char *description) +{ + return _MAV_RETURN_char_array(msg, description, 23, 23); +} + +/** + * @brief Decode a open_drone_id_self_id message into a struct + * + * @param msg The message to decode + * @param open_drone_id_self_id C-struct to decode the message contents into + */ +static inline void mavlink_msg_open_drone_id_self_id_decode(const mavlink_message_t* msg, mavlink_open_drone_id_self_id_t* open_drone_id_self_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + open_drone_id_self_id->target_system = mavlink_msg_open_drone_id_self_id_get_target_system(msg); + open_drone_id_self_id->target_component = mavlink_msg_open_drone_id_self_id_get_target_component(msg); + mavlink_msg_open_drone_id_self_id_get_id_or_mac(msg, open_drone_id_self_id->id_or_mac); + open_drone_id_self_id->description_type = mavlink_msg_open_drone_id_self_id_get_description_type(msg); + mavlink_msg_open_drone_id_self_id_get_description(msg, open_drone_id_self_id->description); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN? msg->len : MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN; + memset(open_drone_id_self_id, 0, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN); + memcpy(open_drone_id_self_id, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_system.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_system.h new file mode 100644 index 0000000000..2af999649f --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_system.h @@ -0,0 +1,505 @@ +#pragma once +// MESSAGE OPEN_DRONE_ID_SYSTEM PACKING + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM 12904 + + +typedef struct __mavlink_open_drone_id_system_t { + int32_t operator_latitude; /*< [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon).*/ + int32_t operator_longitude; /*< [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon).*/ + float area_ceiling; /*< [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m.*/ + float area_floor; /*< [m] Area Operations Floor relative to WGS84. If unknown: -1000 m.*/ + uint16_t area_count; /*< Number of aircraft in the area, group or formation (default 1).*/ + uint16_t area_radius; /*< [m] Radius of the cylindrical area of the group or formation (default 0).*/ + uint8_t target_system; /*< System ID (0 for broadcast).*/ + uint8_t target_component; /*< Component ID (0 for broadcast).*/ + uint8_t id_or_mac[20]; /*< Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */ + uint8_t operator_location_type; /*< Specifies the operator location type.*/ + uint8_t classification_type; /*< Specifies the classification type of the UA.*/ + uint8_t category_eu; /*< When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the category of the UA.*/ + uint8_t class_eu; /*< When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the class of the UA.*/ +} mavlink_open_drone_id_system_t; + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN 46 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN 46 +#define MAVLINK_MSG_ID_12904_LEN 46 +#define MAVLINK_MSG_ID_12904_MIN_LEN 46 + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC 203 +#define MAVLINK_MSG_ID_12904_CRC 203 + +#define MAVLINK_MSG_OPEN_DRONE_ID_SYSTEM_FIELD_ID_OR_MAC_LEN 20 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SYSTEM { \ + 12904, \ + "OPEN_DRONE_ID_SYSTEM", \ + 13, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_open_drone_id_system_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_open_drone_id_system_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 22, offsetof(mavlink_open_drone_id_system_t, id_or_mac) }, \ + { "operator_location_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_open_drone_id_system_t, operator_location_type) }, \ + { "classification_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_open_drone_id_system_t, classification_type) }, \ + { "operator_latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_open_drone_id_system_t, operator_latitude) }, \ + { "operator_longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_open_drone_id_system_t, operator_longitude) }, \ + { "area_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_open_drone_id_system_t, area_count) }, \ + { "area_radius", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_open_drone_id_system_t, area_radius) }, \ + { "area_ceiling", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_open_drone_id_system_t, area_ceiling) }, \ + { "area_floor", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_open_drone_id_system_t, area_floor) }, \ + { "category_eu", NULL, MAVLINK_TYPE_UINT8_T, 0, 44, offsetof(mavlink_open_drone_id_system_t, category_eu) }, \ + { "class_eu", NULL, MAVLINK_TYPE_UINT8_T, 0, 45, offsetof(mavlink_open_drone_id_system_t, class_eu) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SYSTEM { \ + "OPEN_DRONE_ID_SYSTEM", \ + 13, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_open_drone_id_system_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_open_drone_id_system_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 22, offsetof(mavlink_open_drone_id_system_t, id_or_mac) }, \ + { "operator_location_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_open_drone_id_system_t, operator_location_type) }, \ + { "classification_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_open_drone_id_system_t, classification_type) }, \ + { "operator_latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_open_drone_id_system_t, operator_latitude) }, \ + { "operator_longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_open_drone_id_system_t, operator_longitude) }, \ + { "area_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_open_drone_id_system_t, area_count) }, \ + { "area_radius", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_open_drone_id_system_t, area_radius) }, \ + { "area_ceiling", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_open_drone_id_system_t, area_ceiling) }, \ + { "area_floor", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_open_drone_id_system_t, area_floor) }, \ + { "category_eu", NULL, MAVLINK_TYPE_UINT8_T, 0, 44, offsetof(mavlink_open_drone_id_system_t, category_eu) }, \ + { "class_eu", NULL, MAVLINK_TYPE_UINT8_T, 0, 45, offsetof(mavlink_open_drone_id_system_t, class_eu) }, \ + } \ +} +#endif + +/** + * @brief Pack a open_drone_id_system message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param operator_location_type Specifies the operator location type. + * @param classification_type Specifies the classification type of the UA. + * @param operator_latitude [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon). + * @param operator_longitude [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon). + * @param area_count Number of aircraft in the area, group or formation (default 1). + * @param area_radius [m] Radius of the cylindrical area of the group or formation (default 0). + * @param area_ceiling [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. + * @param area_floor [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. + * @param category_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the category of the UA. + * @param class_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the class of the UA. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_system_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_location_type, uint8_t classification_type, int32_t operator_latitude, int32_t operator_longitude, uint16_t area_count, uint16_t area_radius, float area_ceiling, float area_floor, uint8_t category_eu, uint8_t class_eu) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN]; + _mav_put_int32_t(buf, 0, operator_latitude); + _mav_put_int32_t(buf, 4, operator_longitude); + _mav_put_float(buf, 8, area_ceiling); + _mav_put_float(buf, 12, area_floor); + _mav_put_uint16_t(buf, 16, area_count); + _mav_put_uint16_t(buf, 18, area_radius); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 42, operator_location_type); + _mav_put_uint8_t(buf, 43, classification_type); + _mav_put_uint8_t(buf, 44, category_eu); + _mav_put_uint8_t(buf, 45, class_eu); + _mav_put_uint8_t_array(buf, 22, id_or_mac, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN); +#else + mavlink_open_drone_id_system_t packet; + packet.operator_latitude = operator_latitude; + packet.operator_longitude = operator_longitude; + packet.area_ceiling = area_ceiling; + packet.area_floor = area_floor; + packet.area_count = area_count; + packet.area_radius = area_radius; + packet.target_system = target_system; + packet.target_component = target_component; + packet.operator_location_type = operator_location_type; + packet.classification_type = classification_type; + packet.category_eu = category_eu; + packet.class_eu = class_eu; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); +} + +/** + * @brief Pack a open_drone_id_system message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param operator_location_type Specifies the operator location type. + * @param classification_type Specifies the classification type of the UA. + * @param operator_latitude [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon). + * @param operator_longitude [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon). + * @param area_count Number of aircraft in the area, group or formation (default 1). + * @param area_radius [m] Radius of the cylindrical area of the group or formation (default 0). + * @param area_ceiling [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. + * @param area_floor [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. + * @param category_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the category of the UA. + * @param class_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the class of the UA. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_system_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const uint8_t *id_or_mac,uint8_t operator_location_type,uint8_t classification_type,int32_t operator_latitude,int32_t operator_longitude,uint16_t area_count,uint16_t area_radius,float area_ceiling,float area_floor,uint8_t category_eu,uint8_t class_eu) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN]; + _mav_put_int32_t(buf, 0, operator_latitude); + _mav_put_int32_t(buf, 4, operator_longitude); + _mav_put_float(buf, 8, area_ceiling); + _mav_put_float(buf, 12, area_floor); + _mav_put_uint16_t(buf, 16, area_count); + _mav_put_uint16_t(buf, 18, area_radius); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 42, operator_location_type); + _mav_put_uint8_t(buf, 43, classification_type); + _mav_put_uint8_t(buf, 44, category_eu); + _mav_put_uint8_t(buf, 45, class_eu); + _mav_put_uint8_t_array(buf, 22, id_or_mac, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN); +#else + mavlink_open_drone_id_system_t packet; + packet.operator_latitude = operator_latitude; + packet.operator_longitude = operator_longitude; + packet.area_ceiling = area_ceiling; + packet.area_floor = area_floor; + packet.area_count = area_count; + packet.area_radius = area_radius; + packet.target_system = target_system; + packet.target_component = target_component; + packet.operator_location_type = operator_location_type; + packet.classification_type = classification_type; + packet.category_eu = category_eu; + packet.class_eu = class_eu; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); +} + +/** + * @brief Encode a open_drone_id_system struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_system C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_system_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_system_t* open_drone_id_system) +{ + return mavlink_msg_open_drone_id_system_pack(system_id, component_id, msg, open_drone_id_system->target_system, open_drone_id_system->target_component, open_drone_id_system->id_or_mac, open_drone_id_system->operator_location_type, open_drone_id_system->classification_type, open_drone_id_system->operator_latitude, open_drone_id_system->operator_longitude, open_drone_id_system->area_count, open_drone_id_system->area_radius, open_drone_id_system->area_ceiling, open_drone_id_system->area_floor, open_drone_id_system->category_eu, open_drone_id_system->class_eu); +} + +/** + * @brief Encode a open_drone_id_system struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_system C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_system_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_system_t* open_drone_id_system) +{ + return mavlink_msg_open_drone_id_system_pack_chan(system_id, component_id, chan, msg, open_drone_id_system->target_system, open_drone_id_system->target_component, open_drone_id_system->id_or_mac, open_drone_id_system->operator_location_type, open_drone_id_system->classification_type, open_drone_id_system->operator_latitude, open_drone_id_system->operator_longitude, open_drone_id_system->area_count, open_drone_id_system->area_radius, open_drone_id_system->area_ceiling, open_drone_id_system->area_floor, open_drone_id_system->category_eu, open_drone_id_system->class_eu); +} + +/** + * @brief Send a open_drone_id_system message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param operator_location_type Specifies the operator location type. + * @param classification_type Specifies the classification type of the UA. + * @param operator_latitude [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon). + * @param operator_longitude [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon). + * @param area_count Number of aircraft in the area, group or formation (default 1). + * @param area_radius [m] Radius of the cylindrical area of the group or formation (default 0). + * @param area_ceiling [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. + * @param area_floor [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. + * @param category_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the category of the UA. + * @param class_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the class of the UA. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_open_drone_id_system_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_location_type, uint8_t classification_type, int32_t operator_latitude, int32_t operator_longitude, uint16_t area_count, uint16_t area_radius, float area_ceiling, float area_floor, uint8_t category_eu, uint8_t class_eu) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN]; + _mav_put_int32_t(buf, 0, operator_latitude); + _mav_put_int32_t(buf, 4, operator_longitude); + _mav_put_float(buf, 8, area_ceiling); + _mav_put_float(buf, 12, area_floor); + _mav_put_uint16_t(buf, 16, area_count); + _mav_put_uint16_t(buf, 18, area_radius); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 42, operator_location_type); + _mav_put_uint8_t(buf, 43, classification_type); + _mav_put_uint8_t(buf, 44, category_eu); + _mav_put_uint8_t(buf, 45, class_eu); + _mav_put_uint8_t_array(buf, 22, id_or_mac, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); +#else + mavlink_open_drone_id_system_t packet; + packet.operator_latitude = operator_latitude; + packet.operator_longitude = operator_longitude; + packet.area_ceiling = area_ceiling; + packet.area_floor = area_floor; + packet.area_count = area_count; + packet.area_radius = area_radius; + packet.target_system = target_system; + packet.target_component = target_component; + packet.operator_location_type = operator_location_type; + packet.classification_type = classification_type; + packet.category_eu = category_eu; + packet.class_eu = class_eu; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); +#endif +} + +/** + * @brief Send a open_drone_id_system message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_open_drone_id_system_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_system_t* open_drone_id_system) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_open_drone_id_system_send(chan, open_drone_id_system->target_system, open_drone_id_system->target_component, open_drone_id_system->id_or_mac, open_drone_id_system->operator_location_type, open_drone_id_system->classification_type, open_drone_id_system->operator_latitude, open_drone_id_system->operator_longitude, open_drone_id_system->area_count, open_drone_id_system->area_radius, open_drone_id_system->area_ceiling, open_drone_id_system->area_floor, open_drone_id_system->category_eu, open_drone_id_system->class_eu); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM, (const char *)open_drone_id_system, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_open_drone_id_system_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_location_type, uint8_t classification_type, int32_t operator_latitude, int32_t operator_longitude, uint16_t area_count, uint16_t area_radius, float area_ceiling, float area_floor, uint8_t category_eu, uint8_t class_eu) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, operator_latitude); + _mav_put_int32_t(buf, 4, operator_longitude); + _mav_put_float(buf, 8, area_ceiling); + _mav_put_float(buf, 12, area_floor); + _mav_put_uint16_t(buf, 16, area_count); + _mav_put_uint16_t(buf, 18, area_radius); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 42, operator_location_type); + _mav_put_uint8_t(buf, 43, classification_type); + _mav_put_uint8_t(buf, 44, category_eu); + _mav_put_uint8_t(buf, 45, class_eu); + _mav_put_uint8_t_array(buf, 22, id_or_mac, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); +#else + mavlink_open_drone_id_system_t *packet = (mavlink_open_drone_id_system_t *)msgbuf; + packet->operator_latitude = operator_latitude; + packet->operator_longitude = operator_longitude; + packet->area_ceiling = area_ceiling; + packet->area_floor = area_floor; + packet->area_count = area_count; + packet->area_radius = area_radius; + packet->target_system = target_system; + packet->target_component = target_component; + packet->operator_location_type = operator_location_type; + packet->classification_type = classification_type; + packet->category_eu = category_eu; + packet->class_eu = class_eu; + mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OPEN_DRONE_ID_SYSTEM UNPACKING + + +/** + * @brief Get field target_system from open_drone_id_system message + * + * @return System ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_system_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field target_component from open_drone_id_system message + * + * @return Component ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_system_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 21); +} + +/** + * @brief Get field id_or_mac from open_drone_id_system message + * + * @return Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + */ +static inline uint16_t mavlink_msg_open_drone_id_system_get_id_or_mac(const mavlink_message_t* msg, uint8_t *id_or_mac) +{ + return _MAV_RETURN_uint8_t_array(msg, id_or_mac, 20, 22); +} + +/** + * @brief Get field operator_location_type from open_drone_id_system message + * + * @return Specifies the operator location type. + */ +static inline uint8_t mavlink_msg_open_drone_id_system_get_operator_location_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 42); +} + +/** + * @brief Get field classification_type from open_drone_id_system message + * + * @return Specifies the classification type of the UA. + */ +static inline uint8_t mavlink_msg_open_drone_id_system_get_classification_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 43); +} + +/** + * @brief Get field operator_latitude from open_drone_id_system message + * + * @return [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon). + */ +static inline int32_t mavlink_msg_open_drone_id_system_get_operator_latitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field operator_longitude from open_drone_id_system message + * + * @return [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon). + */ +static inline int32_t mavlink_msg_open_drone_id_system_get_operator_longitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field area_count from open_drone_id_system message + * + * @return Number of aircraft in the area, group or formation (default 1). + */ +static inline uint16_t mavlink_msg_open_drone_id_system_get_area_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field area_radius from open_drone_id_system message + * + * @return [m] Radius of the cylindrical area of the group or formation (default 0). + */ +static inline uint16_t mavlink_msg_open_drone_id_system_get_area_radius(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field area_ceiling from open_drone_id_system message + * + * @return [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. + */ +static inline float mavlink_msg_open_drone_id_system_get_area_ceiling(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field area_floor from open_drone_id_system message + * + * @return [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. + */ +static inline float mavlink_msg_open_drone_id_system_get_area_floor(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field category_eu from open_drone_id_system message + * + * @return When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the category of the UA. + */ +static inline uint8_t mavlink_msg_open_drone_id_system_get_category_eu(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 44); +} + +/** + * @brief Get field class_eu from open_drone_id_system message + * + * @return When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the class of the UA. + */ +static inline uint8_t mavlink_msg_open_drone_id_system_get_class_eu(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 45); +} + +/** + * @brief Decode a open_drone_id_system message into a struct + * + * @param msg The message to decode + * @param open_drone_id_system C-struct to decode the message contents into + */ +static inline void mavlink_msg_open_drone_id_system_decode(const mavlink_message_t* msg, mavlink_open_drone_id_system_t* open_drone_id_system) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + open_drone_id_system->operator_latitude = mavlink_msg_open_drone_id_system_get_operator_latitude(msg); + open_drone_id_system->operator_longitude = mavlink_msg_open_drone_id_system_get_operator_longitude(msg); + open_drone_id_system->area_ceiling = mavlink_msg_open_drone_id_system_get_area_ceiling(msg); + open_drone_id_system->area_floor = mavlink_msg_open_drone_id_system_get_area_floor(msg); + open_drone_id_system->area_count = mavlink_msg_open_drone_id_system_get_area_count(msg); + open_drone_id_system->area_radius = mavlink_msg_open_drone_id_system_get_area_radius(msg); + open_drone_id_system->target_system = mavlink_msg_open_drone_id_system_get_target_system(msg); + open_drone_id_system->target_component = mavlink_msg_open_drone_id_system_get_target_component(msg); + mavlink_msg_open_drone_id_system_get_id_or_mac(msg, open_drone_id_system->id_or_mac); + open_drone_id_system->operator_location_type = mavlink_msg_open_drone_id_system_get_operator_location_type(msg); + open_drone_id_system->classification_type = mavlink_msg_open_drone_id_system_get_classification_type(msg); + open_drone_id_system->category_eu = mavlink_msg_open_drone_id_system_get_category_eu(msg); + open_drone_id_system->class_eu = mavlink_msg_open_drone_id_system_get_class_eu(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN? msg->len : MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN; + memset(open_drone_id_system, 0, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN); + memcpy(open_drone_id_system, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_optical_flow.h b/lib/main/MAVLink/common/mavlink_msg_optical_flow.h index 56082216f9..25c8886421 100755 --- a/lib/main/MAVLink/common/mavlink_msg_optical_flow.h +++ b/lib/main/MAVLink/common/mavlink_msg_optical_flow.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_OPTICAL_FLOW 100 - +MAVPACKED( typedef struct __mavlink_optical_flow_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ float flow_comp_m_x; /*< [m/s] Flow in x-sensor direction, angular-speed compensated*/ @@ -13,11 +13,13 @@ typedef struct __mavlink_optical_flow_t { int16_t flow_y; /*< [dpix] Flow in y-sensor direction*/ uint8_t sensor_id; /*< Sensor ID*/ uint8_t quality; /*< Optical flow quality / confidence. 0: bad, 255: maximum quality*/ -} mavlink_optical_flow_t; + float flow_rate_x; /*< [rad/s] Flow rate about X axis*/ + float flow_rate_y; /*< [rad/s] Flow rate about Y axis*/ +}) mavlink_optical_flow_t; -#define MAVLINK_MSG_ID_OPTICAL_FLOW_LEN 26 +#define MAVLINK_MSG_ID_OPTICAL_FLOW_LEN 34 #define MAVLINK_MSG_ID_OPTICAL_FLOW_MIN_LEN 26 -#define MAVLINK_MSG_ID_100_LEN 26 +#define MAVLINK_MSG_ID_100_LEN 34 #define MAVLINK_MSG_ID_100_MIN_LEN 26 #define MAVLINK_MSG_ID_OPTICAL_FLOW_CRC 175 @@ -29,7 +31,7 @@ typedef struct __mavlink_optical_flow_t { #define MAVLINK_MESSAGE_INFO_OPTICAL_FLOW { \ 100, \ "OPTICAL_FLOW", \ - 8, \ + 10, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_optical_flow_t, time_usec) }, \ { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_optical_flow_t, sensor_id) }, \ { "flow_x", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_optical_flow_t, flow_x) }, \ @@ -38,12 +40,14 @@ typedef struct __mavlink_optical_flow_t { { "flow_comp_m_y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_optical_flow_t, flow_comp_m_y) }, \ { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_optical_flow_t, quality) }, \ { "ground_distance", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_optical_flow_t, ground_distance) }, \ + { "flow_rate_x", NULL, MAVLINK_TYPE_FLOAT, 0, 26, offsetof(mavlink_optical_flow_t, flow_rate_x) }, \ + { "flow_rate_y", NULL, MAVLINK_TYPE_FLOAT, 0, 30, offsetof(mavlink_optical_flow_t, flow_rate_y) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_OPTICAL_FLOW { \ "OPTICAL_FLOW", \ - 8, \ + 10, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_optical_flow_t, time_usec) }, \ { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_optical_flow_t, sensor_id) }, \ { "flow_x", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_optical_flow_t, flow_x) }, \ @@ -52,6 +56,8 @@ typedef struct __mavlink_optical_flow_t { { "flow_comp_m_y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_optical_flow_t, flow_comp_m_y) }, \ { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_optical_flow_t, quality) }, \ { "ground_distance", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_optical_flow_t, ground_distance) }, \ + { "flow_rate_x", NULL, MAVLINK_TYPE_FLOAT, 0, 26, offsetof(mavlink_optical_flow_t, flow_rate_x) }, \ + { "flow_rate_y", NULL, MAVLINK_TYPE_FLOAT, 0, 30, offsetof(mavlink_optical_flow_t, flow_rate_y) }, \ } \ } #endif @@ -70,10 +76,12 @@ typedef struct __mavlink_optical_flow_t { * @param flow_comp_m_y [m/s] Flow in y-sensor direction, angular-speed compensated * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality * @param ground_distance [m] Ground distance. Positive value: distance known. Negative value: Unknown distance + * @param flow_rate_x [rad/s] Flow rate about X axis + * @param flow_rate_y [rad/s] Flow rate about Y axis * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) + uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance, float flow_rate_x, float flow_rate_y) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_LEN]; @@ -85,6 +93,8 @@ static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t _mav_put_int16_t(buf, 22, flow_y); _mav_put_uint8_t(buf, 24, sensor_id); _mav_put_uint8_t(buf, 25, quality); + _mav_put_float(buf, 26, flow_rate_x); + _mav_put_float(buf, 30, flow_rate_y); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); #else @@ -97,6 +107,8 @@ static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t packet.flow_y = flow_y; packet.sensor_id = sensor_id; packet.quality = quality; + packet.flow_rate_x = flow_rate_x; + packet.flow_rate_y = flow_rate_y; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); #endif @@ -119,11 +131,13 @@ static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t * @param flow_comp_m_y [m/s] Flow in y-sensor direction, angular-speed compensated * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality * @param ground_distance [m] Ground distance. Positive value: distance known. Negative value: Unknown distance + * @param flow_rate_x [rad/s] Flow rate about X axis + * @param flow_rate_y [rad/s] Flow rate about Y axis * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,uint8_t sensor_id,int16_t flow_x,int16_t flow_y,float flow_comp_m_x,float flow_comp_m_y,uint8_t quality,float ground_distance) + uint64_t time_usec,uint8_t sensor_id,int16_t flow_x,int16_t flow_y,float flow_comp_m_x,float flow_comp_m_y,uint8_t quality,float ground_distance,float flow_rate_x,float flow_rate_y) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_LEN]; @@ -135,6 +149,8 @@ static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uin _mav_put_int16_t(buf, 22, flow_y); _mav_put_uint8_t(buf, 24, sensor_id); _mav_put_uint8_t(buf, 25, quality); + _mav_put_float(buf, 26, flow_rate_x); + _mav_put_float(buf, 30, flow_rate_y); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); #else @@ -147,6 +163,8 @@ static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uin packet.flow_y = flow_y; packet.sensor_id = sensor_id; packet.quality = quality; + packet.flow_rate_x = flow_rate_x; + packet.flow_rate_y = flow_rate_y; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); #endif @@ -165,7 +183,7 @@ static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow) { - return mavlink_msg_optical_flow_pack(system_id, component_id, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance); + return mavlink_msg_optical_flow_pack(system_id, component_id, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance, optical_flow->flow_rate_x, optical_flow->flow_rate_y); } /** @@ -179,7 +197,7 @@ static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_optical_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow) { - return mavlink_msg_optical_flow_pack_chan(system_id, component_id, chan, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance); + return mavlink_msg_optical_flow_pack_chan(system_id, component_id, chan, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance, optical_flow->flow_rate_x, optical_flow->flow_rate_y); } /** @@ -194,10 +212,12 @@ static inline uint16_t mavlink_msg_optical_flow_encode_chan(uint8_t system_id, u * @param flow_comp_m_y [m/s] Flow in y-sensor direction, angular-speed compensated * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality * @param ground_distance [m] Ground distance. Positive value: distance known. Negative value: Unknown distance + * @param flow_rate_x [rad/s] Flow rate about X axis + * @param flow_rate_y [rad/s] Flow rate about Y axis */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) +static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance, float flow_rate_x, float flow_rate_y) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_LEN]; @@ -209,6 +229,8 @@ static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_ _mav_put_int16_t(buf, 22, flow_y); _mav_put_uint8_t(buf, 24, sensor_id); _mav_put_uint8_t(buf, 25, quality); + _mav_put_float(buf, 26, flow_rate_x); + _mav_put_float(buf, 30, flow_rate_y); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_MIN_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); #else @@ -221,6 +243,8 @@ static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_ packet.flow_y = flow_y; packet.sensor_id = sensor_id; packet.quality = quality; + packet.flow_rate_x = flow_rate_x; + packet.flow_rate_y = flow_rate_y; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_OPTICAL_FLOW_MIN_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); #endif @@ -234,7 +258,7 @@ static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_ static inline void mavlink_msg_optical_flow_send_struct(mavlink_channel_t chan, const mavlink_optical_flow_t* optical_flow) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_optical_flow_send(chan, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance); + mavlink_msg_optical_flow_send(chan, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance, optical_flow->flow_rate_x, optical_flow->flow_rate_y); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)optical_flow, MAVLINK_MSG_ID_OPTICAL_FLOW_MIN_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); #endif @@ -248,7 +272,7 @@ static inline void mavlink_msg_optical_flow_send_struct(mavlink_channel_t chan, is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) +static inline void mavlink_msg_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance, float flow_rate_x, float flow_rate_y) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -260,6 +284,8 @@ static inline void mavlink_msg_optical_flow_send_buf(mavlink_message_t *msgbuf, _mav_put_int16_t(buf, 22, flow_y); _mav_put_uint8_t(buf, 24, sensor_id); _mav_put_uint8_t(buf, 25, quality); + _mav_put_float(buf, 26, flow_rate_x); + _mav_put_float(buf, 30, flow_rate_y); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_MIN_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); #else @@ -272,6 +298,8 @@ static inline void mavlink_msg_optical_flow_send_buf(mavlink_message_t *msgbuf, packet->flow_y = flow_y; packet->sensor_id = sensor_id; packet->quality = quality; + packet->flow_rate_x = flow_rate_x; + packet->flow_rate_y = flow_rate_y; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_MIN_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); #endif @@ -363,6 +391,26 @@ static inline float mavlink_msg_optical_flow_get_ground_distance(const mavlink_m return _MAV_RETURN_float(msg, 16); } +/** + * @brief Get field flow_rate_x from optical_flow message + * + * @return [rad/s] Flow rate about X axis + */ +static inline float mavlink_msg_optical_flow_get_flow_rate_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 26); +} + +/** + * @brief Get field flow_rate_y from optical_flow message + * + * @return [rad/s] Flow rate about Y axis + */ +static inline float mavlink_msg_optical_flow_get_flow_rate_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 30); +} + /** * @brief Decode a optical_flow message into a struct * @@ -380,6 +428,8 @@ static inline void mavlink_msg_optical_flow_decode(const mavlink_message_t* msg, optical_flow->flow_y = mavlink_msg_optical_flow_get_flow_y(msg); optical_flow->sensor_id = mavlink_msg_optical_flow_get_sensor_id(msg); optical_flow->quality = mavlink_msg_optical_flow_get_quality(msg); + optical_flow->flow_rate_x = mavlink_msg_optical_flow_get_flow_rate_x(msg); + optical_flow->flow_rate_y = mavlink_msg_optical_flow_get_flow_rate_y(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_OPTICAL_FLOW_LEN? msg->len : MAVLINK_MSG_ID_OPTICAL_FLOW_LEN; memset(optical_flow, 0, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_orbit_execution_status.h b/lib/main/MAVLink/common/mavlink_msg_orbit_execution_status.h new file mode 100644 index 0000000000..29e2915a74 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_orbit_execution_status.h @@ -0,0 +1,338 @@ +#pragma once +// MESSAGE ORBIT_EXECUTION_STATUS PACKING + +#define MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS 360 + + +typedef struct __mavlink_orbit_execution_status_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + float radius; /*< [m] Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise.*/ + int32_t x; /*< X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7.*/ + int32_t y; /*< Y coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7.*/ + float z; /*< [m] Altitude of center point. Coordinate system depends on frame field.*/ + uint8_t frame; /*< The coordinate system of the fields: x, y, z.*/ +} mavlink_orbit_execution_status_t; + +#define MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN 25 +#define MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN 25 +#define MAVLINK_MSG_ID_360_LEN 25 +#define MAVLINK_MSG_ID_360_MIN_LEN 25 + +#define MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC 11 +#define MAVLINK_MSG_ID_360_CRC 11 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_ORBIT_EXECUTION_STATUS { \ + 360, \ + "ORBIT_EXECUTION_STATUS", \ + 6, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_orbit_execution_status_t, time_usec) }, \ + { "radius", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_orbit_execution_status_t, radius) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_orbit_execution_status_t, frame) }, \ + { "x", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_orbit_execution_status_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_orbit_execution_status_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_orbit_execution_status_t, z) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_ORBIT_EXECUTION_STATUS { \ + "ORBIT_EXECUTION_STATUS", \ + 6, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_orbit_execution_status_t, time_usec) }, \ + { "radius", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_orbit_execution_status_t, radius) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_orbit_execution_status_t, frame) }, \ + { "x", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_orbit_execution_status_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_orbit_execution_status_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_orbit_execution_status_t, z) }, \ + } \ +} +#endif + +/** + * @brief Pack a orbit_execution_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param radius [m] Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. + * @param frame The coordinate system of the fields: x, y, z. + * @param x X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + * @param y Y coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + * @param z [m] Altitude of center point. Coordinate system depends on frame field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_orbit_execution_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, float radius, uint8_t frame, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, radius); + _mav_put_int32_t(buf, 12, x); + _mav_put_int32_t(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_uint8_t(buf, 24, frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN); +#else + mavlink_orbit_execution_status_t packet; + packet.time_usec = time_usec; + packet.radius = radius; + packet.x = x; + packet.y = y; + packet.z = z; + packet.frame = frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC); +} + +/** + * @brief Pack a orbit_execution_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param radius [m] Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. + * @param frame The coordinate system of the fields: x, y, z. + * @param x X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + * @param y Y coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + * @param z [m] Altitude of center point. Coordinate system depends on frame field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_orbit_execution_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,float radius,uint8_t frame,int32_t x,int32_t y,float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, radius); + _mav_put_int32_t(buf, 12, x); + _mav_put_int32_t(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_uint8_t(buf, 24, frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN); +#else + mavlink_orbit_execution_status_t packet; + packet.time_usec = time_usec; + packet.radius = radius; + packet.x = x; + packet.y = y; + packet.z = z; + packet.frame = frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC); +} + +/** + * @brief Encode a orbit_execution_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param orbit_execution_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_orbit_execution_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_orbit_execution_status_t* orbit_execution_status) +{ + return mavlink_msg_orbit_execution_status_pack(system_id, component_id, msg, orbit_execution_status->time_usec, orbit_execution_status->radius, orbit_execution_status->frame, orbit_execution_status->x, orbit_execution_status->y, orbit_execution_status->z); +} + +/** + * @brief Encode a orbit_execution_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param orbit_execution_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_orbit_execution_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_orbit_execution_status_t* orbit_execution_status) +{ + return mavlink_msg_orbit_execution_status_pack_chan(system_id, component_id, chan, msg, orbit_execution_status->time_usec, orbit_execution_status->radius, orbit_execution_status->frame, orbit_execution_status->x, orbit_execution_status->y, orbit_execution_status->z); +} + +/** + * @brief Send a orbit_execution_status message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param radius [m] Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. + * @param frame The coordinate system of the fields: x, y, z. + * @param x X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + * @param y Y coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + * @param z [m] Altitude of center point. Coordinate system depends on frame field. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_orbit_execution_status_send(mavlink_channel_t chan, uint64_t time_usec, float radius, uint8_t frame, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, radius); + _mav_put_int32_t(buf, 12, x); + _mav_put_int32_t(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_uint8_t(buf, 24, frame); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS, buf, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC); +#else + mavlink_orbit_execution_status_t packet; + packet.time_usec = time_usec; + packet.radius = radius; + packet.x = x; + packet.y = y; + packet.z = z; + packet.frame = frame; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC); +#endif +} + +/** + * @brief Send a orbit_execution_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_orbit_execution_status_send_struct(mavlink_channel_t chan, const mavlink_orbit_execution_status_t* orbit_execution_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_orbit_execution_status_send(chan, orbit_execution_status->time_usec, orbit_execution_status->radius, orbit_execution_status->frame, orbit_execution_status->x, orbit_execution_status->y, orbit_execution_status->z); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS, (const char *)orbit_execution_status, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_orbit_execution_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float radius, uint8_t frame, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, radius); + _mav_put_int32_t(buf, 12, x); + _mav_put_int32_t(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_uint8_t(buf, 24, frame); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS, buf, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC); +#else + mavlink_orbit_execution_status_t *packet = (mavlink_orbit_execution_status_t *)msgbuf; + packet->time_usec = time_usec; + packet->radius = radius; + packet->x = x; + packet->y = y; + packet->z = z; + packet->frame = frame; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS, (const char *)packet, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE ORBIT_EXECUTION_STATUS UNPACKING + + +/** + * @brief Get field time_usec from orbit_execution_status message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_orbit_execution_status_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field radius from orbit_execution_status message + * + * @return [m] Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. + */ +static inline float mavlink_msg_orbit_execution_status_get_radius(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field frame from orbit_execution_status message + * + * @return The coordinate system of the fields: x, y, z. + */ +static inline uint8_t mavlink_msg_orbit_execution_status_get_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 24); +} + +/** + * @brief Get field x from orbit_execution_status message + * + * @return X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + */ +static inline int32_t mavlink_msg_orbit_execution_status_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field y from orbit_execution_status message + * + * @return Y coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + */ +static inline int32_t mavlink_msg_orbit_execution_status_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field z from orbit_execution_status message + * + * @return [m] Altitude of center point. Coordinate system depends on frame field. + */ +static inline float mavlink_msg_orbit_execution_status_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Decode a orbit_execution_status message into a struct + * + * @param msg The message to decode + * @param orbit_execution_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_orbit_execution_status_decode(const mavlink_message_t* msg, mavlink_orbit_execution_status_t* orbit_execution_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + orbit_execution_status->time_usec = mavlink_msg_orbit_execution_status_get_time_usec(msg); + orbit_execution_status->radius = mavlink_msg_orbit_execution_status_get_radius(msg); + orbit_execution_status->x = mavlink_msg_orbit_execution_status_get_x(msg); + orbit_execution_status->y = mavlink_msg_orbit_execution_status_get_y(msg); + orbit_execution_status->z = mavlink_msg_orbit_execution_status_get_z(msg); + orbit_execution_status->frame = mavlink_msg_orbit_execution_status_get_frame(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN? msg->len : MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN; + memset(orbit_execution_status, 0, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN); + memcpy(orbit_execution_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_param_ext_ack.h b/lib/main/MAVLink/common/mavlink_msg_param_ext_ack.h new file mode 100644 index 0000000000..3a560d2b8f --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_param_ext_ack.h @@ -0,0 +1,281 @@ +#pragma once +// MESSAGE PARAM_EXT_ACK PACKING + +#define MAVLINK_MSG_ID_PARAM_EXT_ACK 324 + + +typedef struct __mavlink_param_ext_ack_t { + char param_id[16]; /*< Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string*/ + char param_value[128]; /*< Parameter value (new value if PARAM_ACK_ACCEPTED, current value otherwise)*/ + uint8_t param_type; /*< Parameter type.*/ + uint8_t param_result; /*< Result code.*/ +} mavlink_param_ext_ack_t; + +#define MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN 146 +#define MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN 146 +#define MAVLINK_MSG_ID_324_LEN 146 +#define MAVLINK_MSG_ID_324_MIN_LEN 146 + +#define MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC 132 +#define MAVLINK_MSG_ID_324_CRC 132 + +#define MAVLINK_MSG_PARAM_EXT_ACK_FIELD_PARAM_ID_LEN 16 +#define MAVLINK_MSG_PARAM_EXT_ACK_FIELD_PARAM_VALUE_LEN 128 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_ACK { \ + 324, \ + "PARAM_EXT_ACK", \ + 4, \ + { { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 0, offsetof(mavlink_param_ext_ack_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_CHAR, 128, 16, offsetof(mavlink_param_ext_ack_t, param_value) }, \ + { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 144, offsetof(mavlink_param_ext_ack_t, param_type) }, \ + { "param_result", NULL, MAVLINK_TYPE_UINT8_T, 0, 145, offsetof(mavlink_param_ext_ack_t, param_result) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_ACK { \ + "PARAM_EXT_ACK", \ + 4, \ + { { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 0, offsetof(mavlink_param_ext_ack_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_CHAR, 128, 16, offsetof(mavlink_param_ext_ack_t, param_value) }, \ + { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 144, offsetof(mavlink_param_ext_ack_t, param_type) }, \ + { "param_result", NULL, MAVLINK_TYPE_UINT8_T, 0, 145, offsetof(mavlink_param_ext_ack_t, param_result) }, \ + } \ +} +#endif + +/** + * @brief Pack a param_ext_ack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value (new value if PARAM_ACK_ACCEPTED, current value otherwise) + * @param param_type Parameter type. + * @param param_result Result code. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const char *param_id, const char *param_value, uint8_t param_type, uint8_t param_result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN]; + _mav_put_uint8_t(buf, 144, param_type); + _mav_put_uint8_t(buf, 145, param_result); + _mav_put_char_array(buf, 0, param_id, 16); + _mav_put_char_array(buf, 16, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN); +#else + mavlink_param_ext_ack_t packet; + packet.param_type = param_type; + packet.param_result = param_result; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_ACK; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); +} + +/** + * @brief Pack a param_ext_ack message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value (new value if PARAM_ACK_ACCEPTED, current value otherwise) + * @param param_type Parameter type. + * @param param_result Result code. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + const char *param_id,const char *param_value,uint8_t param_type,uint8_t param_result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN]; + _mav_put_uint8_t(buf, 144, param_type); + _mav_put_uint8_t(buf, 145, param_result); + _mav_put_char_array(buf, 0, param_id, 16); + _mav_put_char_array(buf, 16, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN); +#else + mavlink_param_ext_ack_t packet; + packet.param_type = param_type; + packet.param_result = param_result; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_ACK; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); +} + +/** + * @brief Encode a param_ext_ack struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param param_ext_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_ext_ack_t* param_ext_ack) +{ + return mavlink_msg_param_ext_ack_pack(system_id, component_id, msg, param_ext_ack->param_id, param_ext_ack->param_value, param_ext_ack->param_type, param_ext_ack->param_result); +} + +/** + * @brief Encode a param_ext_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_ext_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_ext_ack_t* param_ext_ack) +{ + return mavlink_msg_param_ext_ack_pack_chan(system_id, component_id, chan, msg, param_ext_ack->param_id, param_ext_ack->param_value, param_ext_ack->param_type, param_ext_ack->param_result); +} + +/** + * @brief Send a param_ext_ack message + * @param chan MAVLink channel to send the message + * + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value (new value if PARAM_ACK_ACCEPTED, current value otherwise) + * @param param_type Parameter type. + * @param param_result Result code. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_ext_ack_send(mavlink_channel_t chan, const char *param_id, const char *param_value, uint8_t param_type, uint8_t param_result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN]; + _mav_put_uint8_t(buf, 144, param_type); + _mav_put_uint8_t(buf, 145, param_result); + _mav_put_char_array(buf, 0, param_id, 16); + _mav_put_char_array(buf, 16, param_value, 128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_ACK, buf, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); +#else + mavlink_param_ext_ack_t packet; + packet.param_type = param_type; + packet.param_result = param_result; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_ACK, (const char *)&packet, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); +#endif +} + +/** + * @brief Send a param_ext_ack message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_param_ext_ack_send_struct(mavlink_channel_t chan, const mavlink_param_ext_ack_t* param_ext_ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_param_ext_ack_send(chan, param_ext_ack->param_id, param_ext_ack->param_value, param_ext_ack->param_type, param_ext_ack->param_result); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_ACK, (const char *)param_ext_ack, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); +#endif +} + +#if MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_ext_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *param_id, const char *param_value, uint8_t param_type, uint8_t param_result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 144, param_type); + _mav_put_uint8_t(buf, 145, param_result); + _mav_put_char_array(buf, 0, param_id, 16); + _mav_put_char_array(buf, 16, param_value, 128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_ACK, buf, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); +#else + mavlink_param_ext_ack_t *packet = (mavlink_param_ext_ack_t *)msgbuf; + packet->param_type = param_type; + packet->param_result = param_result; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet->param_value, param_value, sizeof(char)*128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_ACK, (const char *)packet, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); +#endif +} +#endif + +#endif + +// MESSAGE PARAM_EXT_ACK UNPACKING + + +/** + * @brief Get field param_id from param_ext_ack message + * + * @return Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + */ +static inline uint16_t mavlink_msg_param_ext_ack_get_param_id(const mavlink_message_t* msg, char *param_id) +{ + return _MAV_RETURN_char_array(msg, param_id, 16, 0); +} + +/** + * @brief Get field param_value from param_ext_ack message + * + * @return Parameter value (new value if PARAM_ACK_ACCEPTED, current value otherwise) + */ +static inline uint16_t mavlink_msg_param_ext_ack_get_param_value(const mavlink_message_t* msg, char *param_value) +{ + return _MAV_RETURN_char_array(msg, param_value, 128, 16); +} + +/** + * @brief Get field param_type from param_ext_ack message + * + * @return Parameter type. + */ +static inline uint8_t mavlink_msg_param_ext_ack_get_param_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 144); +} + +/** + * @brief Get field param_result from param_ext_ack message + * + * @return Result code. + */ +static inline uint8_t mavlink_msg_param_ext_ack_get_param_result(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 145); +} + +/** + * @brief Decode a param_ext_ack message into a struct + * + * @param msg The message to decode + * @param param_ext_ack C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_ext_ack_decode(const mavlink_message_t* msg, mavlink_param_ext_ack_t* param_ext_ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_param_ext_ack_get_param_id(msg, param_ext_ack->param_id); + mavlink_msg_param_ext_ack_get_param_value(msg, param_ext_ack->param_value); + param_ext_ack->param_type = mavlink_msg_param_ext_ack_get_param_type(msg); + param_ext_ack->param_result = mavlink_msg_param_ext_ack_get_param_result(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN? msg->len : MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN; + memset(param_ext_ack, 0, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN); + memcpy(param_ext_ack, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_param_ext_request_list.h b/lib/main/MAVLink/common/mavlink_msg_param_ext_request_list.h new file mode 100644 index 0000000000..a3983dfb85 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_param_ext_request_list.h @@ -0,0 +1,238 @@ +#pragma once +// MESSAGE PARAM_EXT_REQUEST_LIST PACKING + +#define MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST 321 + + +typedef struct __mavlink_param_ext_request_list_t { + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ +} mavlink_param_ext_request_list_t; + +#define MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN 2 +#define MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN 2 +#define MAVLINK_MSG_ID_321_LEN 2 +#define MAVLINK_MSG_ID_321_MIN_LEN 2 + +#define MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC 88 +#define MAVLINK_MSG_ID_321_CRC 88 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_LIST { \ + 321, \ + "PARAM_EXT_REQUEST_LIST", \ + 2, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_ext_request_list_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_ext_request_list_t, target_component) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_LIST { \ + "PARAM_EXT_REQUEST_LIST", \ + 2, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_ext_request_list_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_ext_request_list_t, target_component) }, \ + } \ +} +#endif + +/** + * @brief Pack a param_ext_request_list message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN); +#else + mavlink_param_ext_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC); +} + +/** + * @brief Pack a param_ext_request_list message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN); +#else + mavlink_param_ext_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC); +} + +/** + * @brief Encode a param_ext_request_list struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param param_ext_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_ext_request_list_t* param_ext_request_list) +{ + return mavlink_msg_param_ext_request_list_pack(system_id, component_id, msg, param_ext_request_list->target_system, param_ext_request_list->target_component); +} + +/** + * @brief Encode a param_ext_request_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_ext_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_ext_request_list_t* param_ext_request_list) +{ + return mavlink_msg_param_ext_request_list_pack_chan(system_id, component_id, chan, msg, param_ext_request_list->target_system, param_ext_request_list->target_component); +} + +/** + * @brief Send a param_ext_request_list message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_ext_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC); +#else + mavlink_param_ext_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC); +#endif +} + +/** + * @brief Send a param_ext_request_list message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_param_ext_request_list_send_struct(mavlink_channel_t chan, const mavlink_param_ext_request_list_t* param_ext_request_list) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_param_ext_request_list_send(chan, param_ext_request_list->target_system, param_ext_request_list->target_component); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST, (const char *)param_ext_request_list, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC); +#endif +} + +#if MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_ext_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC); +#else + mavlink_param_ext_request_list_t *packet = (mavlink_param_ext_request_list_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC); +#endif +} +#endif + +#endif + +// MESSAGE PARAM_EXT_REQUEST_LIST UNPACKING + + +/** + * @brief Get field target_system from param_ext_request_list message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_param_ext_request_list_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from param_ext_request_list message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_param_ext_request_list_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Decode a param_ext_request_list message into a struct + * + * @param msg The message to decode + * @param param_ext_request_list C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_ext_request_list_decode(const mavlink_message_t* msg, mavlink_param_ext_request_list_t* param_ext_request_list) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + param_ext_request_list->target_system = mavlink_msg_param_ext_request_list_get_target_system(msg); + param_ext_request_list->target_component = mavlink_msg_param_ext_request_list_get_target_component(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN? msg->len : MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN; + memset(param_ext_request_list, 0, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN); + memcpy(param_ext_request_list, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_param_ext_request_read.h b/lib/main/MAVLink/common/mavlink_msg_param_ext_request_read.h new file mode 100644 index 0000000000..974f68566e --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_param_ext_request_read.h @@ -0,0 +1,280 @@ +#pragma once +// MESSAGE PARAM_EXT_REQUEST_READ PACKING + +#define MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ 320 + + +typedef struct __mavlink_param_ext_request_read_t { + int16_t param_index; /*< Parameter index. Set to -1 to use the Parameter ID field as identifier (else param_id will be ignored)*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + char param_id[16]; /*< Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string*/ +} mavlink_param_ext_request_read_t; + +#define MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN 20 +#define MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN 20 +#define MAVLINK_MSG_ID_320_LEN 20 +#define MAVLINK_MSG_ID_320_MIN_LEN 20 + +#define MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC 243 +#define MAVLINK_MSG_ID_320_CRC 243 + +#define MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN 16 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_READ { \ + 320, \ + "PARAM_EXT_REQUEST_READ", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_param_ext_request_read_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_param_ext_request_read_t, target_component) }, \ + { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 4, offsetof(mavlink_param_ext_request_read_t, param_id) }, \ + { "param_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_param_ext_request_read_t, param_index) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_READ { \ + "PARAM_EXT_REQUEST_READ", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_param_ext_request_read_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_param_ext_request_read_t, target_component) }, \ + { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 4, offsetof(mavlink_param_ext_request_read_t, param_id) }, \ + { "param_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_param_ext_request_read_t, param_index) }, \ + } \ +} +#endif + +/** + * @brief Pack a param_ext_request_read message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Set to -1 to use the Parameter ID field as identifier (else param_id will be ignored) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_request_read_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN]; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN); +#else + mavlink_param_ext_request_read_t packet; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); +} + +/** + * @brief Pack a param_ext_request_read message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Set to -1 to use the Parameter ID field as identifier (else param_id will be ignored) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_request_read_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const char *param_id,int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN]; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN); +#else + mavlink_param_ext_request_read_t packet; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); +} + +/** + * @brief Encode a param_ext_request_read struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param param_ext_request_read C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_request_read_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_ext_request_read_t* param_ext_request_read) +{ + return mavlink_msg_param_ext_request_read_pack(system_id, component_id, msg, param_ext_request_read->target_system, param_ext_request_read->target_component, param_ext_request_read->param_id, param_ext_request_read->param_index); +} + +/** + * @brief Encode a param_ext_request_read struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_ext_request_read C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_request_read_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_ext_request_read_t* param_ext_request_read) +{ + return mavlink_msg_param_ext_request_read_pack_chan(system_id, component_id, chan, msg, param_ext_request_read->target_system, param_ext_request_read->target_component, param_ext_request_read->param_id, param_ext_request_read->param_index); +} + +/** + * @brief Send a param_ext_request_read message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Set to -1 to use the Parameter ID field as identifier (else param_id will be ignored) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_ext_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN]; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); +#else + mavlink_param_ext_request_read_t packet; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ, (const char *)&packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); +#endif +} + +/** + * @brief Send a param_ext_request_read message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_param_ext_request_read_send_struct(mavlink_channel_t chan, const mavlink_param_ext_request_read_t* param_ext_request_read) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_param_ext_request_read_send(chan, param_ext_request_read->target_system, param_ext_request_read->target_component, param_ext_request_read->param_id, param_ext_request_read->param_index); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ, (const char *)param_ext_request_read, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); +#endif +} + +#if MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_ext_request_read_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); +#else + mavlink_param_ext_request_read_t *packet = (mavlink_param_ext_request_read_t *)msgbuf; + packet->param_index = param_index; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ, (const char *)packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); +#endif +} +#endif + +#endif + +// MESSAGE PARAM_EXT_REQUEST_READ UNPACKING + + +/** + * @brief Get field target_system from param_ext_request_read message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_param_ext_request_read_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field target_component from param_ext_request_read message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_param_ext_request_read_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field param_id from param_ext_request_read message + * + * @return Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + */ +static inline uint16_t mavlink_msg_param_ext_request_read_get_param_id(const mavlink_message_t* msg, char *param_id) +{ + return _MAV_RETURN_char_array(msg, param_id, 16, 4); +} + +/** + * @brief Get field param_index from param_ext_request_read message + * + * @return Parameter index. Set to -1 to use the Parameter ID field as identifier (else param_id will be ignored) + */ +static inline int16_t mavlink_msg_param_ext_request_read_get_param_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 0); +} + +/** + * @brief Decode a param_ext_request_read message into a struct + * + * @param msg The message to decode + * @param param_ext_request_read C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_ext_request_read_decode(const mavlink_message_t* msg, mavlink_param_ext_request_read_t* param_ext_request_read) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + param_ext_request_read->param_index = mavlink_msg_param_ext_request_read_get_param_index(msg); + param_ext_request_read->target_system = mavlink_msg_param_ext_request_read_get_target_system(msg); + param_ext_request_read->target_component = mavlink_msg_param_ext_request_read_get_target_component(msg); + mavlink_msg_param_ext_request_read_get_param_id(msg, param_ext_request_read->param_id); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN? msg->len : MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN; + memset(param_ext_request_read, 0, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN); + memcpy(param_ext_request_read, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_param_ext_set.h b/lib/main/MAVLink/common/mavlink_msg_param_ext_set.h new file mode 100644 index 0000000000..bb4c7ccf04 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_param_ext_set.h @@ -0,0 +1,306 @@ +#pragma once +// MESSAGE PARAM_EXT_SET PACKING + +#define MAVLINK_MSG_ID_PARAM_EXT_SET 323 + + +typedef struct __mavlink_param_ext_set_t { + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + char param_id[16]; /*< Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string*/ + char param_value[128]; /*< Parameter value*/ + uint8_t param_type; /*< Parameter type.*/ +} mavlink_param_ext_set_t; + +#define MAVLINK_MSG_ID_PARAM_EXT_SET_LEN 147 +#define MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN 147 +#define MAVLINK_MSG_ID_323_LEN 147 +#define MAVLINK_MSG_ID_323_MIN_LEN 147 + +#define MAVLINK_MSG_ID_PARAM_EXT_SET_CRC 78 +#define MAVLINK_MSG_ID_323_CRC 78 + +#define MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_ID_LEN 16 +#define MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN 128 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_SET { \ + 323, \ + "PARAM_EXT_SET", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_ext_set_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_ext_set_t, target_component) }, \ + { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 2, offsetof(mavlink_param_ext_set_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_CHAR, 128, 18, offsetof(mavlink_param_ext_set_t, param_value) }, \ + { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 146, offsetof(mavlink_param_ext_set_t, param_type) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_SET { \ + "PARAM_EXT_SET", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_ext_set_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_ext_set_t, target_component) }, \ + { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 2, offsetof(mavlink_param_ext_set_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_CHAR, 128, 18, offsetof(mavlink_param_ext_set_t, param_value) }, \ + { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 146, offsetof(mavlink_param_ext_set_t, param_type) }, \ + } \ +} +#endif + +/** + * @brief Pack a param_ext_set message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value + * @param param_type Parameter type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const char *param_id, const char *param_value, uint8_t param_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_SET_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 146, param_type); + _mav_put_char_array(buf, 2, param_id, 16); + _mav_put_char_array(buf, 18, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN); +#else + mavlink_param_ext_set_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_SET; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); +} + +/** + * @brief Pack a param_ext_set message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value + * @param param_type Parameter type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const char *param_id,const char *param_value,uint8_t param_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_SET_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 146, param_type); + _mav_put_char_array(buf, 2, param_id, 16); + _mav_put_char_array(buf, 18, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN); +#else + mavlink_param_ext_set_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_SET; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); +} + +/** + * @brief Encode a param_ext_set struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param param_ext_set C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_set_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_ext_set_t* param_ext_set) +{ + return mavlink_msg_param_ext_set_pack(system_id, component_id, msg, param_ext_set->target_system, param_ext_set->target_component, param_ext_set->param_id, param_ext_set->param_value, param_ext_set->param_type); +} + +/** + * @brief Encode a param_ext_set struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_ext_set C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_set_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_ext_set_t* param_ext_set) +{ + return mavlink_msg_param_ext_set_pack_chan(system_id, component_id, chan, msg, param_ext_set->target_system, param_ext_set->target_component, param_ext_set->param_id, param_ext_set->param_value, param_ext_set->param_type); +} + +/** + * @brief Send a param_ext_set message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value + * @param param_type Parameter type. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_ext_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, const char *param_value, uint8_t param_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_SET_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 146, param_type); + _mav_put_char_array(buf, 2, param_id, 16); + _mav_put_char_array(buf, 18, param_value, 128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_SET, buf, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); +#else + mavlink_param_ext_set_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_SET, (const char *)&packet, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); +#endif +} + +/** + * @brief Send a param_ext_set message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_param_ext_set_send_struct(mavlink_channel_t chan, const mavlink_param_ext_set_t* param_ext_set) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_param_ext_set_send(chan, param_ext_set->target_system, param_ext_set->target_component, param_ext_set->param_id, param_ext_set->param_value, param_ext_set->param_type); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_SET, (const char *)param_ext_set, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); +#endif +} + +#if MAVLINK_MSG_ID_PARAM_EXT_SET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_ext_set_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, const char *param_value, uint8_t param_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 146, param_type); + _mav_put_char_array(buf, 2, param_id, 16); + _mav_put_char_array(buf, 18, param_value, 128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_SET, buf, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); +#else + mavlink_param_ext_set_t *packet = (mavlink_param_ext_set_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->param_type = param_type; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet->param_value, param_value, sizeof(char)*128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_SET, (const char *)packet, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); +#endif +} +#endif + +#endif + +// MESSAGE PARAM_EXT_SET UNPACKING + + +/** + * @brief Get field target_system from param_ext_set message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_param_ext_set_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from param_ext_set message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_param_ext_set_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field param_id from param_ext_set message + * + * @return Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + */ +static inline uint16_t mavlink_msg_param_ext_set_get_param_id(const mavlink_message_t* msg, char *param_id) +{ + return _MAV_RETURN_char_array(msg, param_id, 16, 2); +} + +/** + * @brief Get field param_value from param_ext_set message + * + * @return Parameter value + */ +static inline uint16_t mavlink_msg_param_ext_set_get_param_value(const mavlink_message_t* msg, char *param_value) +{ + return _MAV_RETURN_char_array(msg, param_value, 128, 18); +} + +/** + * @brief Get field param_type from param_ext_set message + * + * @return Parameter type. + */ +static inline uint8_t mavlink_msg_param_ext_set_get_param_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 146); +} + +/** + * @brief Decode a param_ext_set message into a struct + * + * @param msg The message to decode + * @param param_ext_set C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_ext_set_decode(const mavlink_message_t* msg, mavlink_param_ext_set_t* param_ext_set) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + param_ext_set->target_system = mavlink_msg_param_ext_set_get_target_system(msg); + param_ext_set->target_component = mavlink_msg_param_ext_set_get_target_component(msg); + mavlink_msg_param_ext_set_get_param_id(msg, param_ext_set->param_id); + mavlink_msg_param_ext_set_get_param_value(msg, param_ext_set->param_value); + param_ext_set->param_type = mavlink_msg_param_ext_set_get_param_type(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_PARAM_EXT_SET_LEN? msg->len : MAVLINK_MSG_ID_PARAM_EXT_SET_LEN; + memset(param_ext_set, 0, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN); + memcpy(param_ext_set, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_param_ext_value.h b/lib/main/MAVLink/common/mavlink_msg_param_ext_value.h new file mode 100644 index 0000000000..1bf131c390 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_param_ext_value.h @@ -0,0 +1,306 @@ +#pragma once +// MESSAGE PARAM_EXT_VALUE PACKING + +#define MAVLINK_MSG_ID_PARAM_EXT_VALUE 322 + + +typedef struct __mavlink_param_ext_value_t { + uint16_t param_count; /*< Total number of parameters*/ + uint16_t param_index; /*< Index of this parameter*/ + char param_id[16]; /*< Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string*/ + char param_value[128]; /*< Parameter value*/ + uint8_t param_type; /*< Parameter type.*/ +} mavlink_param_ext_value_t; + +#define MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN 149 +#define MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN 149 +#define MAVLINK_MSG_ID_322_LEN 149 +#define MAVLINK_MSG_ID_322_MIN_LEN 149 + +#define MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC 243 +#define MAVLINK_MSG_ID_322_CRC 243 + +#define MAVLINK_MSG_PARAM_EXT_VALUE_FIELD_PARAM_ID_LEN 16 +#define MAVLINK_MSG_PARAM_EXT_VALUE_FIELD_PARAM_VALUE_LEN 128 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_VALUE { \ + 322, \ + "PARAM_EXT_VALUE", \ + 5, \ + { { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 4, offsetof(mavlink_param_ext_value_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_CHAR, 128, 20, offsetof(mavlink_param_ext_value_t, param_value) }, \ + { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 148, offsetof(mavlink_param_ext_value_t, param_type) }, \ + { "param_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_param_ext_value_t, param_count) }, \ + { "param_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_param_ext_value_t, param_index) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_VALUE { \ + "PARAM_EXT_VALUE", \ + 5, \ + { { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 4, offsetof(mavlink_param_ext_value_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_CHAR, 128, 20, offsetof(mavlink_param_ext_value_t, param_value) }, \ + { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 148, offsetof(mavlink_param_ext_value_t, param_type) }, \ + { "param_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_param_ext_value_t, param_count) }, \ + { "param_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_param_ext_value_t, param_index) }, \ + } \ +} +#endif + +/** + * @brief Pack a param_ext_value message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value + * @param param_type Parameter type. + * @param param_count Total number of parameters + * @param param_index Index of this parameter + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_value_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const char *param_id, const char *param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN]; + _mav_put_uint16_t(buf, 0, param_count); + _mav_put_uint16_t(buf, 2, param_index); + _mav_put_uint8_t(buf, 148, param_type); + _mav_put_char_array(buf, 4, param_id, 16); + _mav_put_char_array(buf, 20, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN); +#else + mavlink_param_ext_value_t packet; + packet.param_count = param_count; + packet.param_index = param_index; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_VALUE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); +} + +/** + * @brief Pack a param_ext_value message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value + * @param param_type Parameter type. + * @param param_count Total number of parameters + * @param param_index Index of this parameter + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_value_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + const char *param_id,const char *param_value,uint8_t param_type,uint16_t param_count,uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN]; + _mav_put_uint16_t(buf, 0, param_count); + _mav_put_uint16_t(buf, 2, param_index); + _mav_put_uint8_t(buf, 148, param_type); + _mav_put_char_array(buf, 4, param_id, 16); + _mav_put_char_array(buf, 20, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN); +#else + mavlink_param_ext_value_t packet; + packet.param_count = param_count; + packet.param_index = param_index; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_VALUE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); +} + +/** + * @brief Encode a param_ext_value struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param param_ext_value C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_value_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_ext_value_t* param_ext_value) +{ + return mavlink_msg_param_ext_value_pack(system_id, component_id, msg, param_ext_value->param_id, param_ext_value->param_value, param_ext_value->param_type, param_ext_value->param_count, param_ext_value->param_index); +} + +/** + * @brief Encode a param_ext_value struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_ext_value C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_value_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_ext_value_t* param_ext_value) +{ + return mavlink_msg_param_ext_value_pack_chan(system_id, component_id, chan, msg, param_ext_value->param_id, param_ext_value->param_value, param_ext_value->param_type, param_ext_value->param_count, param_ext_value->param_index); +} + +/** + * @brief Send a param_ext_value message + * @param chan MAVLink channel to send the message + * + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value + * @param param_type Parameter type. + * @param param_count Total number of parameters + * @param param_index Index of this parameter + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_ext_value_send(mavlink_channel_t chan, const char *param_id, const char *param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN]; + _mav_put_uint16_t(buf, 0, param_count); + _mav_put_uint16_t(buf, 2, param_index); + _mav_put_uint8_t(buf, 148, param_type); + _mav_put_char_array(buf, 4, param_id, 16); + _mav_put_char_array(buf, 20, param_value, 128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_VALUE, buf, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); +#else + mavlink_param_ext_value_t packet; + packet.param_count = param_count; + packet.param_index = param_index; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_VALUE, (const char *)&packet, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); +#endif +} + +/** + * @brief Send a param_ext_value message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_param_ext_value_send_struct(mavlink_channel_t chan, const mavlink_param_ext_value_t* param_ext_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_param_ext_value_send(chan, param_ext_value->param_id, param_ext_value->param_value, param_ext_value->param_type, param_ext_value->param_count, param_ext_value->param_index); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_VALUE, (const char *)param_ext_value, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_ext_value_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *param_id, const char *param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, param_count); + _mav_put_uint16_t(buf, 2, param_index); + _mav_put_uint8_t(buf, 148, param_type); + _mav_put_char_array(buf, 4, param_id, 16); + _mav_put_char_array(buf, 20, param_value, 128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_VALUE, buf, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); +#else + mavlink_param_ext_value_t *packet = (mavlink_param_ext_value_t *)msgbuf; + packet->param_count = param_count; + packet->param_index = param_index; + packet->param_type = param_type; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet->param_value, param_value, sizeof(char)*128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_VALUE, (const char *)packet, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE PARAM_EXT_VALUE UNPACKING + + +/** + * @brief Get field param_id from param_ext_value message + * + * @return Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + */ +static inline uint16_t mavlink_msg_param_ext_value_get_param_id(const mavlink_message_t* msg, char *param_id) +{ + return _MAV_RETURN_char_array(msg, param_id, 16, 4); +} + +/** + * @brief Get field param_value from param_ext_value message + * + * @return Parameter value + */ +static inline uint16_t mavlink_msg_param_ext_value_get_param_value(const mavlink_message_t* msg, char *param_value) +{ + return _MAV_RETURN_char_array(msg, param_value, 128, 20); +} + +/** + * @brief Get field param_type from param_ext_value message + * + * @return Parameter type. + */ +static inline uint8_t mavlink_msg_param_ext_value_get_param_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 148); +} + +/** + * @brief Get field param_count from param_ext_value message + * + * @return Total number of parameters + */ +static inline uint16_t mavlink_msg_param_ext_value_get_param_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field param_index from param_ext_value message + * + * @return Index of this parameter + */ +static inline uint16_t mavlink_msg_param_ext_value_get_param_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Decode a param_ext_value message into a struct + * + * @param msg The message to decode + * @param param_ext_value C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_ext_value_decode(const mavlink_message_t* msg, mavlink_param_ext_value_t* param_ext_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + param_ext_value->param_count = mavlink_msg_param_ext_value_get_param_count(msg); + param_ext_value->param_index = mavlink_msg_param_ext_value_get_param_index(msg); + mavlink_msg_param_ext_value_get_param_id(msg, param_ext_value->param_id); + mavlink_msg_param_ext_value_get_param_value(msg, param_ext_value->param_value); + param_ext_value->param_type = mavlink_msg_param_ext_value_get_param_type(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN? msg->len : MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN; + memset(param_ext_value, 0, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN); + memcpy(param_ext_value, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_play_tune.h b/lib/main/MAVLink/common/mavlink_msg_play_tune.h new file mode 100644 index 0000000000..c318448e24 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_play_tune.h @@ -0,0 +1,281 @@ +#pragma once +// MESSAGE PLAY_TUNE PACKING + +#define MAVLINK_MSG_ID_PLAY_TUNE 258 + + +typedef struct __mavlink_play_tune_t { + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + char tune[30]; /*< tune in board specific format*/ + char tune2[200]; /*< tune extension (appended to tune)*/ +} mavlink_play_tune_t; + +#define MAVLINK_MSG_ID_PLAY_TUNE_LEN 232 +#define MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN 32 +#define MAVLINK_MSG_ID_258_LEN 232 +#define MAVLINK_MSG_ID_258_MIN_LEN 32 + +#define MAVLINK_MSG_ID_PLAY_TUNE_CRC 187 +#define MAVLINK_MSG_ID_258_CRC 187 + +#define MAVLINK_MSG_PLAY_TUNE_FIELD_TUNE_LEN 30 +#define MAVLINK_MSG_PLAY_TUNE_FIELD_TUNE2_LEN 200 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_PLAY_TUNE { \ + 258, \ + "PLAY_TUNE", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_play_tune_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_play_tune_t, target_component) }, \ + { "tune", NULL, MAVLINK_TYPE_CHAR, 30, 2, offsetof(mavlink_play_tune_t, tune) }, \ + { "tune2", NULL, MAVLINK_TYPE_CHAR, 200, 32, offsetof(mavlink_play_tune_t, tune2) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_PLAY_TUNE { \ + "PLAY_TUNE", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_play_tune_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_play_tune_t, target_component) }, \ + { "tune", NULL, MAVLINK_TYPE_CHAR, 30, 2, offsetof(mavlink_play_tune_t, tune) }, \ + { "tune2", NULL, MAVLINK_TYPE_CHAR, 200, 32, offsetof(mavlink_play_tune_t, tune2) }, \ + } \ +} +#endif + +/** + * @brief Pack a play_tune message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param tune tune in board specific format + * @param tune2 tune extension (appended to tune) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_play_tune_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const char *tune, const char *tune2) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PLAY_TUNE_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_char_array(buf, 2, tune, 30); + _mav_put_char_array(buf, 32, tune2, 200); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PLAY_TUNE_LEN); +#else + mavlink_play_tune_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.tune, tune, sizeof(char)*30); + mav_array_memcpy(packet.tune2, tune2, sizeof(char)*200); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PLAY_TUNE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PLAY_TUNE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); +} + +/** + * @brief Pack a play_tune message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param tune tune in board specific format + * @param tune2 tune extension (appended to tune) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_play_tune_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const char *tune,const char *tune2) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PLAY_TUNE_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_char_array(buf, 2, tune, 30); + _mav_put_char_array(buf, 32, tune2, 200); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PLAY_TUNE_LEN); +#else + mavlink_play_tune_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.tune, tune, sizeof(char)*30); + mav_array_memcpy(packet.tune2, tune2, sizeof(char)*200); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PLAY_TUNE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PLAY_TUNE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); +} + +/** + * @brief Encode a play_tune struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param play_tune C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_play_tune_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_play_tune_t* play_tune) +{ + return mavlink_msg_play_tune_pack(system_id, component_id, msg, play_tune->target_system, play_tune->target_component, play_tune->tune, play_tune->tune2); +} + +/** + * @brief Encode a play_tune struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param play_tune C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_play_tune_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_play_tune_t* play_tune) +{ + return mavlink_msg_play_tune_pack_chan(system_id, component_id, chan, msg, play_tune->target_system, play_tune->target_component, play_tune->tune, play_tune->tune2); +} + +/** + * @brief Send a play_tune message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param tune tune in board specific format + * @param tune2 tune extension (appended to tune) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_play_tune_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *tune, const char *tune2) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PLAY_TUNE_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_char_array(buf, 2, tune, 30); + _mav_put_char_array(buf, 32, tune2, 200); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE, buf, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); +#else + mavlink_play_tune_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.tune, tune, sizeof(char)*30); + mav_array_memcpy(packet.tune2, tune2, sizeof(char)*200); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE, (const char *)&packet, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); +#endif +} + +/** + * @brief Send a play_tune message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_play_tune_send_struct(mavlink_channel_t chan, const mavlink_play_tune_t* play_tune) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_play_tune_send(chan, play_tune->target_system, play_tune->target_component, play_tune->tune, play_tune->tune2); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE, (const char *)play_tune, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_PLAY_TUNE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_play_tune_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *tune, const char *tune2) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_char_array(buf, 2, tune, 30); + _mav_put_char_array(buf, 32, tune2, 200); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE, buf, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); +#else + mavlink_play_tune_t *packet = (mavlink_play_tune_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->tune, tune, sizeof(char)*30); + mav_array_memcpy(packet->tune2, tune2, sizeof(char)*200); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE, (const char *)packet, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE PLAY_TUNE UNPACKING + + +/** + * @brief Get field target_system from play_tune message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_play_tune_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from play_tune message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_play_tune_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field tune from play_tune message + * + * @return tune in board specific format + */ +static inline uint16_t mavlink_msg_play_tune_get_tune(const mavlink_message_t* msg, char *tune) +{ + return _MAV_RETURN_char_array(msg, tune, 30, 2); +} + +/** + * @brief Get field tune2 from play_tune message + * + * @return tune extension (appended to tune) + */ +static inline uint16_t mavlink_msg_play_tune_get_tune2(const mavlink_message_t* msg, char *tune2) +{ + return _MAV_RETURN_char_array(msg, tune2, 200, 32); +} + +/** + * @brief Decode a play_tune message into a struct + * + * @param msg The message to decode + * @param play_tune C-struct to decode the message contents into + */ +static inline void mavlink_msg_play_tune_decode(const mavlink_message_t* msg, mavlink_play_tune_t* play_tune) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + play_tune->target_system = mavlink_msg_play_tune_get_target_system(msg); + play_tune->target_component = mavlink_msg_play_tune_get_target_component(msg); + mavlink_msg_play_tune_get_tune(msg, play_tune->tune); + mavlink_msg_play_tune_get_tune2(msg, play_tune->tune2); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_PLAY_TUNE_LEN? msg->len : MAVLINK_MSG_ID_PLAY_TUNE_LEN; + memset(play_tune, 0, MAVLINK_MSG_ID_PLAY_TUNE_LEN); + memcpy(play_tune, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_play_tune_v2.h b/lib/main/MAVLink/common/mavlink_msg_play_tune_v2.h new file mode 100644 index 0000000000..c47d6e6a8a --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_play_tune_v2.h @@ -0,0 +1,280 @@ +#pragma once +// MESSAGE PLAY_TUNE_V2 PACKING + +#define MAVLINK_MSG_ID_PLAY_TUNE_V2 400 + + +typedef struct __mavlink_play_tune_v2_t { + uint32_t format; /*< Tune format*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + char tune[248]; /*< Tune definition as a NULL-terminated string.*/ +} mavlink_play_tune_v2_t; + +#define MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN 254 +#define MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN 254 +#define MAVLINK_MSG_ID_400_LEN 254 +#define MAVLINK_MSG_ID_400_MIN_LEN 254 + +#define MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC 110 +#define MAVLINK_MSG_ID_400_CRC 110 + +#define MAVLINK_MSG_PLAY_TUNE_V2_FIELD_TUNE_LEN 248 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_PLAY_TUNE_V2 { \ + 400, \ + "PLAY_TUNE_V2", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_play_tune_v2_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_play_tune_v2_t, target_component) }, \ + { "format", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_play_tune_v2_t, format) }, \ + { "tune", NULL, MAVLINK_TYPE_CHAR, 248, 6, offsetof(mavlink_play_tune_v2_t, tune) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_PLAY_TUNE_V2 { \ + "PLAY_TUNE_V2", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_play_tune_v2_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_play_tune_v2_t, target_component) }, \ + { "format", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_play_tune_v2_t, format) }, \ + { "tune", NULL, MAVLINK_TYPE_CHAR, 248, 6, offsetof(mavlink_play_tune_v2_t, tune) }, \ + } \ +} +#endif + +/** + * @brief Pack a play_tune_v2 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param format Tune format + * @param tune Tune definition as a NULL-terminated string. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_play_tune_v2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t format, const char *tune) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN]; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_char_array(buf, 6, tune, 248); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN); +#else + mavlink_play_tune_v2_t packet; + packet.format = format; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.tune, tune, sizeof(char)*248); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PLAY_TUNE_V2; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); +} + +/** + * @brief Pack a play_tune_v2 message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param format Tune format + * @param tune Tune definition as a NULL-terminated string. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_play_tune_v2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint32_t format,const char *tune) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN]; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_char_array(buf, 6, tune, 248); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN); +#else + mavlink_play_tune_v2_t packet; + packet.format = format; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.tune, tune, sizeof(char)*248); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PLAY_TUNE_V2; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); +} + +/** + * @brief Encode a play_tune_v2 struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param play_tune_v2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_play_tune_v2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_play_tune_v2_t* play_tune_v2) +{ + return mavlink_msg_play_tune_v2_pack(system_id, component_id, msg, play_tune_v2->target_system, play_tune_v2->target_component, play_tune_v2->format, play_tune_v2->tune); +} + +/** + * @brief Encode a play_tune_v2 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param play_tune_v2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_play_tune_v2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_play_tune_v2_t* play_tune_v2) +{ + return mavlink_msg_play_tune_v2_pack_chan(system_id, component_id, chan, msg, play_tune_v2->target_system, play_tune_v2->target_component, play_tune_v2->format, play_tune_v2->tune); +} + +/** + * @brief Send a play_tune_v2 message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param format Tune format + * @param tune Tune definition as a NULL-terminated string. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_play_tune_v2_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t format, const char *tune) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN]; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_char_array(buf, 6, tune, 248); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE_V2, buf, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); +#else + mavlink_play_tune_v2_t packet; + packet.format = format; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.tune, tune, sizeof(char)*248); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE_V2, (const char *)&packet, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); +#endif +} + +/** + * @brief Send a play_tune_v2 message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_play_tune_v2_send_struct(mavlink_channel_t chan, const mavlink_play_tune_v2_t* play_tune_v2) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_play_tune_v2_send(chan, play_tune_v2->target_system, play_tune_v2->target_component, play_tune_v2->format, play_tune_v2->tune); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE_V2, (const char *)play_tune_v2, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); +#endif +} + +#if MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_play_tune_v2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t format, const char *tune) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_char_array(buf, 6, tune, 248); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE_V2, buf, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); +#else + mavlink_play_tune_v2_t *packet = (mavlink_play_tune_v2_t *)msgbuf; + packet->format = format; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->tune, tune, sizeof(char)*248); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE_V2, (const char *)packet, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); +#endif +} +#endif + +#endif + +// MESSAGE PLAY_TUNE_V2 UNPACKING + + +/** + * @brief Get field target_system from play_tune_v2 message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_play_tune_v2_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from play_tune_v2 message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_play_tune_v2_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field format from play_tune_v2 message + * + * @return Tune format + */ +static inline uint32_t mavlink_msg_play_tune_v2_get_format(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field tune from play_tune_v2 message + * + * @return Tune definition as a NULL-terminated string. + */ +static inline uint16_t mavlink_msg_play_tune_v2_get_tune(const mavlink_message_t* msg, char *tune) +{ + return _MAV_RETURN_char_array(msg, tune, 248, 6); +} + +/** + * @brief Decode a play_tune_v2 message into a struct + * + * @param msg The message to decode + * @param play_tune_v2 C-struct to decode the message contents into + */ +static inline void mavlink_msg_play_tune_v2_decode(const mavlink_message_t* msg, mavlink_play_tune_v2_t* play_tune_v2) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + play_tune_v2->format = mavlink_msg_play_tune_v2_get_format(msg); + play_tune_v2->target_system = mavlink_msg_play_tune_v2_get_target_system(msg); + play_tune_v2->target_component = mavlink_msg_play_tune_v2_get_target_component(msg); + mavlink_msg_play_tune_v2_get_tune(msg, play_tune_v2->tune); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN? msg->len : MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN; + memset(play_tune_v2, 0, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN); + memcpy(play_tune_v2, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_raw_imu.h b/lib/main/MAVLink/common/mavlink_msg_raw_imu.h index 0e0cf7141c..eeb4eacb2e 100755 --- a/lib/main/MAVLink/common/mavlink_msg_raw_imu.h +++ b/lib/main/MAVLink/common/mavlink_msg_raw_imu.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_RAW_IMU 27 - +MAVPACKED( typedef struct __mavlink_raw_imu_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ int16_t xacc; /*< X acceleration (raw)*/ @@ -15,11 +15,13 @@ typedef struct __mavlink_raw_imu_t { int16_t xmag; /*< X Magnetic field (raw)*/ int16_t ymag; /*< Y Magnetic field (raw)*/ int16_t zmag; /*< Z Magnetic field (raw)*/ -} mavlink_raw_imu_t; + uint8_t id; /*< Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0)*/ + int16_t temperature; /*< [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C).*/ +}) mavlink_raw_imu_t; -#define MAVLINK_MSG_ID_RAW_IMU_LEN 26 +#define MAVLINK_MSG_ID_RAW_IMU_LEN 29 #define MAVLINK_MSG_ID_RAW_IMU_MIN_LEN 26 -#define MAVLINK_MSG_ID_27_LEN 26 +#define MAVLINK_MSG_ID_27_LEN 29 #define MAVLINK_MSG_ID_27_MIN_LEN 26 #define MAVLINK_MSG_ID_RAW_IMU_CRC 144 @@ -31,7 +33,7 @@ typedef struct __mavlink_raw_imu_t { #define MAVLINK_MESSAGE_INFO_RAW_IMU { \ 27, \ "RAW_IMU", \ - 10, \ + 12, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_raw_imu_t, time_usec) }, \ { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_raw_imu_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_raw_imu_t, yacc) }, \ @@ -42,12 +44,14 @@ typedef struct __mavlink_raw_imu_t { { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_raw_imu_t, xmag) }, \ { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_raw_imu_t, ymag) }, \ { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_raw_imu_t, zmag) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_raw_imu_t, id) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 27, offsetof(mavlink_raw_imu_t, temperature) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_RAW_IMU { \ "RAW_IMU", \ - 10, \ + 12, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_raw_imu_t, time_usec) }, \ { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_raw_imu_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_raw_imu_t, yacc) }, \ @@ -58,6 +62,8 @@ typedef struct __mavlink_raw_imu_t { { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_raw_imu_t, xmag) }, \ { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_raw_imu_t, ymag) }, \ { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_raw_imu_t, zmag) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_raw_imu_t, id) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 27, offsetof(mavlink_raw_imu_t, temperature) }, \ } \ } #endif @@ -78,10 +84,12 @@ typedef struct __mavlink_raw_imu_t { * @param xmag X Magnetic field (raw) * @param ymag Y Magnetic field (raw) * @param zmag Z Magnetic field (raw) + * @param id Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) + uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, uint8_t id, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_RAW_IMU_LEN]; @@ -95,6 +103,8 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo _mav_put_int16_t(buf, 20, xmag); _mav_put_int16_t(buf, 22, ymag); _mav_put_int16_t(buf, 24, zmag); + _mav_put_uint8_t(buf, 26, id); + _mav_put_int16_t(buf, 27, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_IMU_LEN); #else @@ -109,6 +119,8 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.id = id; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_IMU_LEN); #endif @@ -133,11 +145,13 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo * @param xmag X Magnetic field (raw) * @param ymag Y Magnetic field (raw) * @param zmag Z Magnetic field (raw) + * @param id Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) + uint64_t time_usec,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag,uint8_t id,int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_RAW_IMU_LEN]; @@ -151,6 +165,8 @@ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t _mav_put_int16_t(buf, 20, xmag); _mav_put_int16_t(buf, 22, ymag); _mav_put_int16_t(buf, 24, zmag); + _mav_put_uint8_t(buf, 26, id); + _mav_put_int16_t(buf, 27, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_IMU_LEN); #else @@ -165,6 +181,8 @@ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.id = id; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_IMU_LEN); #endif @@ -183,7 +201,7 @@ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_raw_imu_t* raw_imu) { - return mavlink_msg_raw_imu_pack(system_id, component_id, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag); + return mavlink_msg_raw_imu_pack(system_id, component_id, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag, raw_imu->id, raw_imu->temperature); } /** @@ -197,7 +215,7 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com */ static inline uint16_t mavlink_msg_raw_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_imu_t* raw_imu) { - return mavlink_msg_raw_imu_pack_chan(system_id, component_id, chan, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag); + return mavlink_msg_raw_imu_pack_chan(system_id, component_id, chan, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag, raw_imu->id, raw_imu->temperature); } /** @@ -214,10 +232,12 @@ static inline uint16_t mavlink_msg_raw_imu_encode_chan(uint8_t system_id, uint8_ * @param xmag X Magnetic field (raw) * @param ymag Y Magnetic field (raw) * @param zmag Z Magnetic field (raw) + * @param id Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, uint8_t id, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_RAW_IMU_LEN]; @@ -231,6 +251,8 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t tim _mav_put_int16_t(buf, 20, xmag); _mav_put_int16_t(buf, 22, ymag); _mav_put_int16_t(buf, 24, zmag); + _mav_put_uint8_t(buf, 26, id); + _mav_put_int16_t(buf, 27, temperature); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_MIN_LEN, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); #else @@ -245,6 +267,8 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t tim packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.id = id; + packet.temperature = temperature; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)&packet, MAVLINK_MSG_ID_RAW_IMU_MIN_LEN, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); #endif @@ -258,7 +282,7 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t tim static inline void mavlink_msg_raw_imu_send_struct(mavlink_channel_t chan, const mavlink_raw_imu_t* raw_imu) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_raw_imu_send(chan, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag); + mavlink_msg_raw_imu_send(chan, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag, raw_imu->id, raw_imu->temperature); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)raw_imu, MAVLINK_MSG_ID_RAW_IMU_MIN_LEN, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); #endif @@ -272,7 +296,7 @@ static inline void mavlink_msg_raw_imu_send_struct(mavlink_channel_t chan, const is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_raw_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +static inline void mavlink_msg_raw_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, uint8_t id, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -286,6 +310,8 @@ static inline void mavlink_msg_raw_imu_send_buf(mavlink_message_t *msgbuf, mavli _mav_put_int16_t(buf, 20, xmag); _mav_put_int16_t(buf, 22, ymag); _mav_put_int16_t(buf, 24, zmag); + _mav_put_uint8_t(buf, 26, id); + _mav_put_int16_t(buf, 27, temperature); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_MIN_LEN, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); #else @@ -300,6 +326,8 @@ static inline void mavlink_msg_raw_imu_send_buf(mavlink_message_t *msgbuf, mavli packet->xmag = xmag; packet->ymag = ymag; packet->zmag = zmag; + packet->id = id; + packet->temperature = temperature; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)packet, MAVLINK_MSG_ID_RAW_IMU_MIN_LEN, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); #endif @@ -411,6 +439,26 @@ static inline int16_t mavlink_msg_raw_imu_get_zmag(const mavlink_message_t* msg) return _MAV_RETURN_int16_t(msg, 24); } +/** + * @brief Get field id from raw_imu message + * + * @return Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) + */ +static inline uint8_t mavlink_msg_raw_imu_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 26); +} + +/** + * @brief Get field temperature from raw_imu message + * + * @return [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + */ +static inline int16_t mavlink_msg_raw_imu_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 27); +} + /** * @brief Decode a raw_imu message into a struct * @@ -430,6 +478,8 @@ static inline void mavlink_msg_raw_imu_decode(const mavlink_message_t* msg, mavl raw_imu->xmag = mavlink_msg_raw_imu_get_xmag(msg); raw_imu->ymag = mavlink_msg_raw_imu_get_ymag(msg); raw_imu->zmag = mavlink_msg_raw_imu_get_zmag(msg); + raw_imu->id = mavlink_msg_raw_imu_get_id(msg); + raw_imu->temperature = mavlink_msg_raw_imu_get_temperature(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_RAW_IMU_LEN? msg->len : MAVLINK_MSG_ID_RAW_IMU_LEN; memset(raw_imu, 0, MAVLINK_MSG_ID_RAW_IMU_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_raw_rpm.h b/lib/main/MAVLink/common/mavlink_msg_raw_rpm.h new file mode 100644 index 0000000000..0fa4f1a0b5 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_raw_rpm.h @@ -0,0 +1,238 @@ +#pragma once +// MESSAGE RAW_RPM PACKING + +#define MAVLINK_MSG_ID_RAW_RPM 339 + + +typedef struct __mavlink_raw_rpm_t { + float frequency; /*< [rpm] Indicated rate*/ + uint8_t index; /*< Index of this RPM sensor (0-indexed)*/ +} mavlink_raw_rpm_t; + +#define MAVLINK_MSG_ID_RAW_RPM_LEN 5 +#define MAVLINK_MSG_ID_RAW_RPM_MIN_LEN 5 +#define MAVLINK_MSG_ID_339_LEN 5 +#define MAVLINK_MSG_ID_339_MIN_LEN 5 + +#define MAVLINK_MSG_ID_RAW_RPM_CRC 199 +#define MAVLINK_MSG_ID_339_CRC 199 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_RAW_RPM { \ + 339, \ + "RAW_RPM", \ + 2, \ + { { "index", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_raw_rpm_t, index) }, \ + { "frequency", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_raw_rpm_t, frequency) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_RAW_RPM { \ + "RAW_RPM", \ + 2, \ + { { "index", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_raw_rpm_t, index) }, \ + { "frequency", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_raw_rpm_t, frequency) }, \ + } \ +} +#endif + +/** + * @brief Pack a raw_rpm message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param index Index of this RPM sensor (0-indexed) + * @param frequency [rpm] Indicated rate + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_raw_rpm_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t index, float frequency) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RAW_RPM_LEN]; + _mav_put_float(buf, 0, frequency); + _mav_put_uint8_t(buf, 4, index); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_RPM_LEN); +#else + mavlink_raw_rpm_t packet; + packet.frequency = frequency; + packet.index = index; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_RPM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RAW_RPM; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_RPM_MIN_LEN, MAVLINK_MSG_ID_RAW_RPM_LEN, MAVLINK_MSG_ID_RAW_RPM_CRC); +} + +/** + * @brief Pack a raw_rpm message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param index Index of this RPM sensor (0-indexed) + * @param frequency [rpm] Indicated rate + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_raw_rpm_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t index,float frequency) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RAW_RPM_LEN]; + _mav_put_float(buf, 0, frequency); + _mav_put_uint8_t(buf, 4, index); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_RPM_LEN); +#else + mavlink_raw_rpm_t packet; + packet.frequency = frequency; + packet.index = index; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_RPM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RAW_RPM; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_RPM_MIN_LEN, MAVLINK_MSG_ID_RAW_RPM_LEN, MAVLINK_MSG_ID_RAW_RPM_CRC); +} + +/** + * @brief Encode a raw_rpm struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param raw_rpm C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_rpm_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_raw_rpm_t* raw_rpm) +{ + return mavlink_msg_raw_rpm_pack(system_id, component_id, msg, raw_rpm->index, raw_rpm->frequency); +} + +/** + * @brief Encode a raw_rpm struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param raw_rpm C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_rpm_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_rpm_t* raw_rpm) +{ + return mavlink_msg_raw_rpm_pack_chan(system_id, component_id, chan, msg, raw_rpm->index, raw_rpm->frequency); +} + +/** + * @brief Send a raw_rpm message + * @param chan MAVLink channel to send the message + * + * @param index Index of this RPM sensor (0-indexed) + * @param frequency [rpm] Indicated rate + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_raw_rpm_send(mavlink_channel_t chan, uint8_t index, float frequency) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RAW_RPM_LEN]; + _mav_put_float(buf, 0, frequency); + _mav_put_uint8_t(buf, 4, index); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_RPM, buf, MAVLINK_MSG_ID_RAW_RPM_MIN_LEN, MAVLINK_MSG_ID_RAW_RPM_LEN, MAVLINK_MSG_ID_RAW_RPM_CRC); +#else + mavlink_raw_rpm_t packet; + packet.frequency = frequency; + packet.index = index; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_RPM, (const char *)&packet, MAVLINK_MSG_ID_RAW_RPM_MIN_LEN, MAVLINK_MSG_ID_RAW_RPM_LEN, MAVLINK_MSG_ID_RAW_RPM_CRC); +#endif +} + +/** + * @brief Send a raw_rpm message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_raw_rpm_send_struct(mavlink_channel_t chan, const mavlink_raw_rpm_t* raw_rpm) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_raw_rpm_send(chan, raw_rpm->index, raw_rpm->frequency); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_RPM, (const char *)raw_rpm, MAVLINK_MSG_ID_RAW_RPM_MIN_LEN, MAVLINK_MSG_ID_RAW_RPM_LEN, MAVLINK_MSG_ID_RAW_RPM_CRC); +#endif +} + +#if MAVLINK_MSG_ID_RAW_RPM_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_raw_rpm_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t index, float frequency) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, frequency); + _mav_put_uint8_t(buf, 4, index); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_RPM, buf, MAVLINK_MSG_ID_RAW_RPM_MIN_LEN, MAVLINK_MSG_ID_RAW_RPM_LEN, MAVLINK_MSG_ID_RAW_RPM_CRC); +#else + mavlink_raw_rpm_t *packet = (mavlink_raw_rpm_t *)msgbuf; + packet->frequency = frequency; + packet->index = index; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_RPM, (const char *)packet, MAVLINK_MSG_ID_RAW_RPM_MIN_LEN, MAVLINK_MSG_ID_RAW_RPM_LEN, MAVLINK_MSG_ID_RAW_RPM_CRC); +#endif +} +#endif + +#endif + +// MESSAGE RAW_RPM UNPACKING + + +/** + * @brief Get field index from raw_rpm message + * + * @return Index of this RPM sensor (0-indexed) + */ +static inline uint8_t mavlink_msg_raw_rpm_get_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field frequency from raw_rpm message + * + * @return [rpm] Indicated rate + */ +static inline float mavlink_msg_raw_rpm_get_frequency(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Decode a raw_rpm message into a struct + * + * @param msg The message to decode + * @param raw_rpm C-struct to decode the message contents into + */ +static inline void mavlink_msg_raw_rpm_decode(const mavlink_message_t* msg, mavlink_raw_rpm_t* raw_rpm) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + raw_rpm->frequency = mavlink_msg_raw_rpm_get_frequency(msg); + raw_rpm->index = mavlink_msg_raw_rpm_get_index(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_RAW_RPM_LEN? msg->len : MAVLINK_MSG_ID_RAW_RPM_LEN; + memset(raw_rpm, 0, MAVLINK_MSG_ID_RAW_RPM_LEN); + memcpy(raw_rpm, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_rc_channels_override.h b/lib/main/MAVLink/common/mavlink_msg_rc_channels_override.h index b45c6ae85b..a0d2a767ec 100755 --- a/lib/main/MAVLink/common/mavlink_msg_rc_channels_override.h +++ b/lib/main/MAVLink/common/mavlink_msg_rc_channels_override.h @@ -5,21 +5,31 @@ typedef struct __mavlink_rc_channels_override_t { - uint16_t chan1_raw; /*< [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field.*/ - uint16_t chan2_raw; /*< [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field.*/ - uint16_t chan3_raw; /*< [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field.*/ - uint16_t chan4_raw; /*< [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field.*/ - uint16_t chan5_raw; /*< [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field.*/ - uint16_t chan6_raw; /*< [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field.*/ - uint16_t chan7_raw; /*< [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field.*/ - uint16_t chan8_raw; /*< [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field.*/ + uint16_t chan1_raw; /*< [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio.*/ + uint16_t chan2_raw; /*< [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio.*/ + uint16_t chan3_raw; /*< [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio.*/ + uint16_t chan4_raw; /*< [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio.*/ + uint16_t chan5_raw; /*< [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio.*/ + uint16_t chan6_raw; /*< [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio.*/ + uint16_t chan7_raw; /*< [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio.*/ + uint16_t chan8_raw; /*< [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio.*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ + uint16_t chan9_raw; /*< [us] RC channel 9 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan10_raw; /*< [us] RC channel 10 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan11_raw; /*< [us] RC channel 11 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan12_raw; /*< [us] RC channel 12 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan13_raw; /*< [us] RC channel 13 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan14_raw; /*< [us] RC channel 14 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan15_raw; /*< [us] RC channel 15 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan16_raw; /*< [us] RC channel 16 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan17_raw; /*< [us] RC channel 17 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan18_raw; /*< [us] RC channel 18 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ } mavlink_rc_channels_override_t; -#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN 18 +#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN 38 #define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_MIN_LEN 18 -#define MAVLINK_MSG_ID_70_LEN 18 +#define MAVLINK_MSG_ID_70_LEN 38 #define MAVLINK_MSG_ID_70_MIN_LEN 18 #define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC 124 @@ -31,7 +41,7 @@ typedef struct __mavlink_rc_channels_override_t { #define MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE { \ 70, \ "RC_CHANNELS_OVERRIDE", \ - 10, \ + 20, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rc_channels_override_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_rc_channels_override_t, target_component) }, \ { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_rc_channels_override_t, chan1_raw) }, \ @@ -42,12 +52,22 @@ typedef struct __mavlink_rc_channels_override_t { { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_override_t, chan6_raw) }, \ { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_override_t, chan7_raw) }, \ { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_override_t, chan8_raw) }, \ + { "chan9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_rc_channels_override_t, chan9_raw) }, \ + { "chan10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_rc_channels_override_t, chan10_raw) }, \ + { "chan11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_rc_channels_override_t, chan11_raw) }, \ + { "chan12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_rc_channels_override_t, chan12_raw) }, \ + { "chan13_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_rc_channels_override_t, chan13_raw) }, \ + { "chan14_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_rc_channels_override_t, chan14_raw) }, \ + { "chan15_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_rc_channels_override_t, chan15_raw) }, \ + { "chan16_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_rc_channels_override_t, chan16_raw) }, \ + { "chan17_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 34, offsetof(mavlink_rc_channels_override_t, chan17_raw) }, \ + { "chan18_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_rc_channels_override_t, chan18_raw) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE { \ "RC_CHANNELS_OVERRIDE", \ - 10, \ + 20, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rc_channels_override_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_rc_channels_override_t, target_component) }, \ { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_rc_channels_override_t, chan1_raw) }, \ @@ -58,6 +78,16 @@ typedef struct __mavlink_rc_channels_override_t { { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_override_t, chan6_raw) }, \ { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_override_t, chan7_raw) }, \ { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_override_t, chan8_raw) }, \ + { "chan9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_rc_channels_override_t, chan9_raw) }, \ + { "chan10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_rc_channels_override_t, chan10_raw) }, \ + { "chan11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_rc_channels_override_t, chan11_raw) }, \ + { "chan12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_rc_channels_override_t, chan12_raw) }, \ + { "chan13_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_rc_channels_override_t, chan13_raw) }, \ + { "chan14_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_rc_channels_override_t, chan14_raw) }, \ + { "chan15_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_rc_channels_override_t, chan15_raw) }, \ + { "chan16_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_rc_channels_override_t, chan16_raw) }, \ + { "chan17_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 34, offsetof(mavlink_rc_channels_override_t, chan17_raw) }, \ + { "chan18_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_rc_channels_override_t, chan18_raw) }, \ } \ } #endif @@ -70,18 +100,28 @@ typedef struct __mavlink_rc_channels_override_t { * * @param target_system System ID * @param target_component Component ID - * @param chan1_raw [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. - * @param chan2_raw [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. - * @param chan3_raw [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. - * @param chan4_raw [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. - * @param chan5_raw [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. - * @param chan6_raw [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. - * @param chan7_raw [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. - * @param chan8_raw [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. + * @param chan1_raw [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan2_raw [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan3_raw [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan4_raw [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan5_raw [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan6_raw [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan7_raw [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan8_raw [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan9_raw [us] RC channel 9 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan10_raw [us] RC channel 10 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan11_raw [us] RC channel 11 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan12_raw [us] RC channel 12 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan13_raw [us] RC channel 13 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan14_raw [us] RC channel 14 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan15_raw [us] RC channel 15 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan16_raw [us] RC channel 16 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan17_raw [us] RC channel 17 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan18_raw [us] RC channel 18 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) + uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN]; @@ -95,6 +135,16 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, _mav_put_uint16_t(buf, 14, chan8_raw); _mav_put_uint8_t(buf, 16, target_system); _mav_put_uint8_t(buf, 17, target_component); + _mav_put_uint16_t(buf, 18, chan9_raw); + _mav_put_uint16_t(buf, 20, chan10_raw); + _mav_put_uint16_t(buf, 22, chan11_raw); + _mav_put_uint16_t(buf, 24, chan12_raw); + _mav_put_uint16_t(buf, 26, chan13_raw); + _mav_put_uint16_t(buf, 28, chan14_raw); + _mav_put_uint16_t(buf, 30, chan15_raw); + _mav_put_uint16_t(buf, 32, chan16_raw); + _mav_put_uint16_t(buf, 34, chan17_raw); + _mav_put_uint16_t(buf, 36, chan18_raw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); #else @@ -109,6 +159,16 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, packet.chan8_raw = chan8_raw; packet.target_system = target_system; packet.target_component = target_component; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.chan13_raw = chan13_raw; + packet.chan14_raw = chan14_raw; + packet.chan15_raw = chan15_raw; + packet.chan16_raw = chan16_raw; + packet.chan17_raw = chan17_raw; + packet.chan18_raw = chan18_raw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); #endif @@ -125,19 +185,29 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID - * @param chan1_raw [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. - * @param chan2_raw [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. - * @param chan3_raw [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. - * @param chan4_raw [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. - * @param chan5_raw [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. - * @param chan6_raw [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. - * @param chan7_raw [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. - * @param chan8_raw [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. + * @param chan1_raw [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan2_raw [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan3_raw [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan4_raw [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan5_raw [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan6_raw [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan7_raw [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan8_raw [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan9_raw [us] RC channel 9 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan10_raw [us] RC channel 10 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan11_raw [us] RC channel 11 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan12_raw [us] RC channel 12 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan13_raw [us] RC channel 13 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan14_raw [us] RC channel 14 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan15_raw [us] RC channel 15 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan16_raw [us] RC channel 16 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan17_raw [us] RC channel 17 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan18_raw [us] RC channel 18 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw) + uint8_t target_system,uint8_t target_component,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw,uint16_t chan9_raw,uint16_t chan10_raw,uint16_t chan11_raw,uint16_t chan12_raw,uint16_t chan13_raw,uint16_t chan14_raw,uint16_t chan15_raw,uint16_t chan16_raw,uint16_t chan17_raw,uint16_t chan18_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN]; @@ -151,6 +221,16 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system _mav_put_uint16_t(buf, 14, chan8_raw); _mav_put_uint8_t(buf, 16, target_system); _mav_put_uint8_t(buf, 17, target_component); + _mav_put_uint16_t(buf, 18, chan9_raw); + _mav_put_uint16_t(buf, 20, chan10_raw); + _mav_put_uint16_t(buf, 22, chan11_raw); + _mav_put_uint16_t(buf, 24, chan12_raw); + _mav_put_uint16_t(buf, 26, chan13_raw); + _mav_put_uint16_t(buf, 28, chan14_raw); + _mav_put_uint16_t(buf, 30, chan15_raw); + _mav_put_uint16_t(buf, 32, chan16_raw); + _mav_put_uint16_t(buf, 34, chan17_raw); + _mav_put_uint16_t(buf, 36, chan18_raw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); #else @@ -165,6 +245,16 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system packet.chan8_raw = chan8_raw; packet.target_system = target_system; packet.target_component = target_component; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.chan13_raw = chan13_raw; + packet.chan14_raw = chan14_raw; + packet.chan15_raw = chan15_raw; + packet.chan16_raw = chan16_raw; + packet.chan17_raw = chan17_raw; + packet.chan18_raw = chan18_raw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); #endif @@ -183,7 +273,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system */ static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override) { - return mavlink_msg_rc_channels_override_pack(system_id, component_id, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); + return mavlink_msg_rc_channels_override_pack(system_id, component_id, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw, rc_channels_override->chan9_raw, rc_channels_override->chan10_raw, rc_channels_override->chan11_raw, rc_channels_override->chan12_raw, rc_channels_override->chan13_raw, rc_channels_override->chan14_raw, rc_channels_override->chan15_raw, rc_channels_override->chan16_raw, rc_channels_override->chan17_raw, rc_channels_override->chan18_raw); } /** @@ -197,7 +287,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id */ static inline uint16_t mavlink_msg_rc_channels_override_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override) { - return mavlink_msg_rc_channels_override_pack_chan(system_id, component_id, chan, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); + return mavlink_msg_rc_channels_override_pack_chan(system_id, component_id, chan, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw, rc_channels_override->chan9_raw, rc_channels_override->chan10_raw, rc_channels_override->chan11_raw, rc_channels_override->chan12_raw, rc_channels_override->chan13_raw, rc_channels_override->chan14_raw, rc_channels_override->chan15_raw, rc_channels_override->chan16_raw, rc_channels_override->chan17_raw, rc_channels_override->chan18_raw); } /** @@ -206,18 +296,28 @@ static inline uint16_t mavlink_msg_rc_channels_override_encode_chan(uint8_t syst * * @param target_system System ID * @param target_component Component ID - * @param chan1_raw [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. - * @param chan2_raw [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. - * @param chan3_raw [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. - * @param chan4_raw [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. - * @param chan5_raw [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. - * @param chan6_raw [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. - * @param chan7_raw [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. - * @param chan8_raw [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. + * @param chan1_raw [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan2_raw [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan3_raw [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan4_raw [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan5_raw [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan6_raw [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan7_raw [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan8_raw [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan9_raw [us] RC channel 9 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan10_raw [us] RC channel 10 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan11_raw [us] RC channel 11 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan12_raw [us] RC channel 12 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan13_raw [us] RC channel 13 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan14_raw [us] RC channel 14 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan15_raw [us] RC channel 15 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan16_raw [us] RC channel 16 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan17_raw [us] RC channel 17 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan18_raw [us] RC channel 18 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN]; @@ -231,6 +331,16 @@ static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, _mav_put_uint16_t(buf, 14, chan8_raw); _mav_put_uint8_t(buf, 16, target_system); _mav_put_uint8_t(buf, 17, target_component); + _mav_put_uint16_t(buf, 18, chan9_raw); + _mav_put_uint16_t(buf, 20, chan10_raw); + _mav_put_uint16_t(buf, 22, chan11_raw); + _mav_put_uint16_t(buf, 24, chan12_raw); + _mav_put_uint16_t(buf, 26, chan13_raw); + _mav_put_uint16_t(buf, 28, chan14_raw); + _mav_put_uint16_t(buf, 30, chan15_raw); + _mav_put_uint16_t(buf, 32, chan16_raw); + _mav_put_uint16_t(buf, 34, chan17_raw); + _mav_put_uint16_t(buf, 36, chan18_raw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); #else @@ -245,6 +355,16 @@ static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, packet.chan8_raw = chan8_raw; packet.target_system = target_system; packet.target_component = target_component; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.chan13_raw = chan13_raw; + packet.chan14_raw = chan14_raw; + packet.chan15_raw = chan15_raw; + packet.chan16_raw = chan16_raw; + packet.chan17_raw = chan17_raw; + packet.chan18_raw = chan18_raw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); #endif @@ -258,7 +378,7 @@ static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, static inline void mavlink_msg_rc_channels_override_send_struct(mavlink_channel_t chan, const mavlink_rc_channels_override_t* rc_channels_override) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_rc_channels_override_send(chan, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); + mavlink_msg_rc_channels_override_send(chan, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw, rc_channels_override->chan9_raw, rc_channels_override->chan10_raw, rc_channels_override->chan11_raw, rc_channels_override->chan12_raw, rc_channels_override->chan13_raw, rc_channels_override->chan14_raw, rc_channels_override->chan15_raw, rc_channels_override->chan16_raw, rc_channels_override->chan17_raw, rc_channels_override->chan18_raw); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)rc_channels_override, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); #endif @@ -272,7 +392,7 @@ static inline void mavlink_msg_rc_channels_override_send_struct(mavlink_channel_ is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_rc_channels_override_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +static inline void mavlink_msg_rc_channels_override_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -286,6 +406,16 @@ static inline void mavlink_msg_rc_channels_override_send_buf(mavlink_message_t * _mav_put_uint16_t(buf, 14, chan8_raw); _mav_put_uint8_t(buf, 16, target_system); _mav_put_uint8_t(buf, 17, target_component); + _mav_put_uint16_t(buf, 18, chan9_raw); + _mav_put_uint16_t(buf, 20, chan10_raw); + _mav_put_uint16_t(buf, 22, chan11_raw); + _mav_put_uint16_t(buf, 24, chan12_raw); + _mav_put_uint16_t(buf, 26, chan13_raw); + _mav_put_uint16_t(buf, 28, chan14_raw); + _mav_put_uint16_t(buf, 30, chan15_raw); + _mav_put_uint16_t(buf, 32, chan16_raw); + _mav_put_uint16_t(buf, 34, chan17_raw); + _mav_put_uint16_t(buf, 36, chan18_raw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); #else @@ -300,6 +430,16 @@ static inline void mavlink_msg_rc_channels_override_send_buf(mavlink_message_t * packet->chan8_raw = chan8_raw; packet->target_system = target_system; packet->target_component = target_component; + packet->chan9_raw = chan9_raw; + packet->chan10_raw = chan10_raw; + packet->chan11_raw = chan11_raw; + packet->chan12_raw = chan12_raw; + packet->chan13_raw = chan13_raw; + packet->chan14_raw = chan14_raw; + packet->chan15_raw = chan15_raw; + packet->chan16_raw = chan16_raw; + packet->chan17_raw = chan17_raw; + packet->chan18_raw = chan18_raw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); #endif @@ -334,7 +474,7 @@ static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(cons /** * @brief Get field chan1_raw from rc_channels_override message * - * @return [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. + * @return [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavlink_message_t* msg) { @@ -344,7 +484,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavl /** * @brief Get field chan2_raw from rc_channels_override message * - * @return [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. + * @return [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavlink_message_t* msg) { @@ -354,7 +494,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavl /** * @brief Get field chan3_raw from rc_channels_override message * - * @return [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. + * @return [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavlink_message_t* msg) { @@ -364,7 +504,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavl /** * @brief Get field chan4_raw from rc_channels_override message * - * @return [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. + * @return [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavlink_message_t* msg) { @@ -374,7 +514,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavl /** * @brief Get field chan5_raw from rc_channels_override message * - * @return [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. + * @return [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavlink_message_t* msg) { @@ -384,7 +524,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavl /** * @brief Get field chan6_raw from rc_channels_override message * - * @return [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. + * @return [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavlink_message_t* msg) { @@ -394,7 +534,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavl /** * @brief Get field chan7_raw from rc_channels_override message * - * @return [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. + * @return [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavlink_message_t* msg) { @@ -404,13 +544,113 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavl /** * @brief Get field chan8_raw from rc_channels_override message * - * @return [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. + * @return [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan8_raw(const mavlink_message_t* msg) { return _MAV_RETURN_uint16_t(msg, 14); } +/** + * @brief Get field chan9_raw from rc_channels_override message + * + * @return [us] RC channel 9 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan9_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field chan10_raw from rc_channels_override message + * + * @return [us] RC channel 10 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan10_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 20); +} + +/** + * @brief Get field chan11_raw from rc_channels_override message + * + * @return [us] RC channel 11 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan11_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 22); +} + +/** + * @brief Get field chan12_raw from rc_channels_override message + * + * @return [us] RC channel 12 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan12_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field chan13_raw from rc_channels_override message + * + * @return [us] RC channel 13 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan13_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 26); +} + +/** + * @brief Get field chan14_raw from rc_channels_override message + * + * @return [us] RC channel 14 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan14_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field chan15_raw from rc_channels_override message + * + * @return [us] RC channel 15 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan15_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 30); +} + +/** + * @brief Get field chan16_raw from rc_channels_override message + * + * @return [us] RC channel 16 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan16_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 32); +} + +/** + * @brief Get field chan17_raw from rc_channels_override message + * + * @return [us] RC channel 17 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan17_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 34); +} + +/** + * @brief Get field chan18_raw from rc_channels_override message + * + * @return [us] RC channel 18 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan18_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 36); +} + /** * @brief Decode a rc_channels_override message into a struct * @@ -430,6 +670,16 @@ static inline void mavlink_msg_rc_channels_override_decode(const mavlink_message rc_channels_override->chan8_raw = mavlink_msg_rc_channels_override_get_chan8_raw(msg); rc_channels_override->target_system = mavlink_msg_rc_channels_override_get_target_system(msg); rc_channels_override->target_component = mavlink_msg_rc_channels_override_get_target_component(msg); + rc_channels_override->chan9_raw = mavlink_msg_rc_channels_override_get_chan9_raw(msg); + rc_channels_override->chan10_raw = mavlink_msg_rc_channels_override_get_chan10_raw(msg); + rc_channels_override->chan11_raw = mavlink_msg_rc_channels_override_get_chan11_raw(msg); + rc_channels_override->chan12_raw = mavlink_msg_rc_channels_override_get_chan12_raw(msg); + rc_channels_override->chan13_raw = mavlink_msg_rc_channels_override_get_chan13_raw(msg); + rc_channels_override->chan14_raw = mavlink_msg_rc_channels_override_get_chan14_raw(msg); + rc_channels_override->chan15_raw = mavlink_msg_rc_channels_override_get_chan15_raw(msg); + rc_channels_override->chan16_raw = mavlink_msg_rc_channels_override_get_chan16_raw(msg); + rc_channels_override->chan17_raw = mavlink_msg_rc_channels_override_get_chan17_raw(msg); + rc_channels_override->chan18_raw = mavlink_msg_rc_channels_override_get_chan18_raw(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN? msg->len : MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN; memset(rc_channels_override, 0, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_imu.h b/lib/main/MAVLink/common/mavlink_msg_scaled_imu.h index f94b146bf2..14b1257ce9 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_imu.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_imu.h @@ -15,11 +15,12 @@ typedef struct __mavlink_scaled_imu_t { int16_t xmag; /*< [mgauss] X Magnetic field*/ int16_t ymag; /*< [mgauss] Y Magnetic field*/ int16_t zmag; /*< [mgauss] Z Magnetic field*/ + int16_t temperature; /*< [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C).*/ } mavlink_scaled_imu_t; -#define MAVLINK_MSG_ID_SCALED_IMU_LEN 22 +#define MAVLINK_MSG_ID_SCALED_IMU_LEN 24 #define MAVLINK_MSG_ID_SCALED_IMU_MIN_LEN 22 -#define MAVLINK_MSG_ID_26_LEN 22 +#define MAVLINK_MSG_ID_26_LEN 24 #define MAVLINK_MSG_ID_26_MIN_LEN 22 #define MAVLINK_MSG_ID_SCALED_IMU_CRC 170 @@ -31,7 +32,7 @@ typedef struct __mavlink_scaled_imu_t { #define MAVLINK_MESSAGE_INFO_SCALED_IMU { \ 26, \ "SCALED_IMU", \ - 10, \ + 11, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu_t, time_boot_ms) }, \ { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu_t, yacc) }, \ @@ -42,12 +43,13 @@ typedef struct __mavlink_scaled_imu_t { { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu_t, xmag) }, \ { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu_t, ymag) }, \ { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu_t, zmag) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_scaled_imu_t, temperature) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SCALED_IMU { \ "SCALED_IMU", \ - 10, \ + 11, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu_t, time_boot_ms) }, \ { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu_t, yacc) }, \ @@ -58,6 +60,7 @@ typedef struct __mavlink_scaled_imu_t { { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu_t, xmag) }, \ { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu_t, ymag) }, \ { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu_t, zmag) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_scaled_imu_t, temperature) }, \ } \ } #endif @@ -78,10 +81,11 @@ typedef struct __mavlink_scaled_imu_t { * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) + uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU_LEN]; @@ -95,6 +99,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU_LEN); #else @@ -109,6 +114,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU_LEN); #endif @@ -133,11 +139,12 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) + uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag,int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU_LEN]; @@ -151,6 +158,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8 _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU_LEN); #else @@ -165,6 +173,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8 packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU_LEN); #endif @@ -183,7 +192,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_imu_t* scaled_imu) { - return mavlink_msg_scaled_imu_pack(system_id, component_id, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag); + return mavlink_msg_scaled_imu_pack(system_id, component_id, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag, scaled_imu->temperature); } /** @@ -197,7 +206,7 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_scaled_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu_t* scaled_imu) { - return mavlink_msg_scaled_imu_pack_chan(system_id, component_id, chan, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag); + return mavlink_msg_scaled_imu_pack_chan(system_id, component_id, chan, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag, scaled_imu->temperature); } /** @@ -214,10 +223,11 @@ static inline uint16_t mavlink_msg_scaled_imu_encode_chan(uint8_t system_id, uin * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU_LEN]; @@ -231,6 +241,7 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); #else @@ -245,6 +256,7 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); #endif @@ -258,7 +270,7 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t static inline void mavlink_msg_scaled_imu_send_struct(mavlink_channel_t chan, const mavlink_scaled_imu_t* scaled_imu) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_scaled_imu_send(chan, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag); + mavlink_msg_scaled_imu_send(chan, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag, scaled_imu->temperature); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)scaled_imu, MAVLINK_MSG_ID_SCALED_IMU_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); #endif @@ -272,7 +284,7 @@ static inline void mavlink_msg_scaled_imu_send_struct(mavlink_channel_t chan, co is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_scaled_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +static inline void mavlink_msg_scaled_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -286,6 +298,7 @@ static inline void mavlink_msg_scaled_imu_send_buf(mavlink_message_t *msgbuf, ma _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); #else @@ -300,6 +313,7 @@ static inline void mavlink_msg_scaled_imu_send_buf(mavlink_message_t *msgbuf, ma packet->xmag = xmag; packet->ymag = ymag; packet->zmag = zmag; + packet->temperature = temperature; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); #endif @@ -411,6 +425,16 @@ static inline int16_t mavlink_msg_scaled_imu_get_zmag(const mavlink_message_t* m return _MAV_RETURN_int16_t(msg, 20); } +/** + * @brief Get field temperature from scaled_imu message + * + * @return [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + */ +static inline int16_t mavlink_msg_scaled_imu_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 22); +} + /** * @brief Decode a scaled_imu message into a struct * @@ -430,6 +454,7 @@ static inline void mavlink_msg_scaled_imu_decode(const mavlink_message_t* msg, m scaled_imu->xmag = mavlink_msg_scaled_imu_get_xmag(msg); scaled_imu->ymag = mavlink_msg_scaled_imu_get_ymag(msg); scaled_imu->zmag = mavlink_msg_scaled_imu_get_zmag(msg); + scaled_imu->temperature = mavlink_msg_scaled_imu_get_temperature(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SCALED_IMU_LEN? msg->len : MAVLINK_MSG_ID_SCALED_IMU_LEN; memset(scaled_imu, 0, MAVLINK_MSG_ID_SCALED_IMU_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_imu2.h b/lib/main/MAVLink/common/mavlink_msg_scaled_imu2.h index 297874cb4a..2106cefd1e 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_imu2.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_imu2.h @@ -15,11 +15,12 @@ typedef struct __mavlink_scaled_imu2_t { int16_t xmag; /*< [mgauss] X Magnetic field*/ int16_t ymag; /*< [mgauss] Y Magnetic field*/ int16_t zmag; /*< [mgauss] Z Magnetic field*/ + int16_t temperature; /*< [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C).*/ } mavlink_scaled_imu2_t; -#define MAVLINK_MSG_ID_SCALED_IMU2_LEN 22 +#define MAVLINK_MSG_ID_SCALED_IMU2_LEN 24 #define MAVLINK_MSG_ID_SCALED_IMU2_MIN_LEN 22 -#define MAVLINK_MSG_ID_116_LEN 22 +#define MAVLINK_MSG_ID_116_LEN 24 #define MAVLINK_MSG_ID_116_MIN_LEN 22 #define MAVLINK_MSG_ID_SCALED_IMU2_CRC 76 @@ -31,7 +32,7 @@ typedef struct __mavlink_scaled_imu2_t { #define MAVLINK_MESSAGE_INFO_SCALED_IMU2 { \ 116, \ "SCALED_IMU2", \ - 10, \ + 11, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu2_t, time_boot_ms) }, \ { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu2_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu2_t, yacc) }, \ @@ -42,12 +43,13 @@ typedef struct __mavlink_scaled_imu2_t { { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu2_t, xmag) }, \ { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu2_t, ymag) }, \ { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu2_t, zmag) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_scaled_imu2_t, temperature) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SCALED_IMU2 { \ "SCALED_IMU2", \ - 10, \ + 11, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu2_t, time_boot_ms) }, \ { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu2_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu2_t, yacc) }, \ @@ -58,6 +60,7 @@ typedef struct __mavlink_scaled_imu2_t { { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu2_t, xmag) }, \ { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu2_t, ymag) }, \ { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu2_t, zmag) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_scaled_imu2_t, temperature) }, \ } \ } #endif @@ -78,10 +81,11 @@ typedef struct __mavlink_scaled_imu2_t { * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_imu2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) + uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN]; @@ -95,6 +99,7 @@ static inline uint16_t mavlink_msg_scaled_imu2_pack(uint8_t system_id, uint8_t c _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); #else @@ -109,6 +114,7 @@ static inline uint16_t mavlink_msg_scaled_imu2_pack(uint8_t system_id, uint8_t c packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); #endif @@ -133,11 +139,12 @@ static inline uint16_t mavlink_msg_scaled_imu2_pack(uint8_t system_id, uint8_t c * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_imu2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) + uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag,int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN]; @@ -151,6 +158,7 @@ static inline uint16_t mavlink_msg_scaled_imu2_pack_chan(uint8_t system_id, uint _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); #else @@ -165,6 +173,7 @@ static inline uint16_t mavlink_msg_scaled_imu2_pack_chan(uint8_t system_id, uint packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); #endif @@ -183,7 +192,7 @@ static inline uint16_t mavlink_msg_scaled_imu2_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_scaled_imu2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_imu2_t* scaled_imu2) { - return mavlink_msg_scaled_imu2_pack(system_id, component_id, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag); + return mavlink_msg_scaled_imu2_pack(system_id, component_id, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag, scaled_imu2->temperature); } /** @@ -197,7 +206,7 @@ static inline uint16_t mavlink_msg_scaled_imu2_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_scaled_imu2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu2_t* scaled_imu2) { - return mavlink_msg_scaled_imu2_pack_chan(system_id, component_id, chan, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag); + return mavlink_msg_scaled_imu2_pack_chan(system_id, component_id, chan, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag, scaled_imu2->temperature); } /** @@ -214,10 +223,11 @@ static inline uint16_t mavlink_msg_scaled_imu2_encode_chan(uint8_t system_id, ui * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN]; @@ -231,6 +241,7 @@ static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); #else @@ -245,6 +256,7 @@ static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU2_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); #endif @@ -258,7 +270,7 @@ static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t static inline void mavlink_msg_scaled_imu2_send_struct(mavlink_channel_t chan, const mavlink_scaled_imu2_t* scaled_imu2) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_scaled_imu2_send(chan, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag); + mavlink_msg_scaled_imu2_send(chan, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag, scaled_imu2->temperature); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)scaled_imu2, MAVLINK_MSG_ID_SCALED_IMU2_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); #endif @@ -272,7 +284,7 @@ static inline void mavlink_msg_scaled_imu2_send_struct(mavlink_channel_t chan, c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_scaled_imu2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +static inline void mavlink_msg_scaled_imu2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -286,6 +298,7 @@ static inline void mavlink_msg_scaled_imu2_send_buf(mavlink_message_t *msgbuf, m _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); #else @@ -300,6 +313,7 @@ static inline void mavlink_msg_scaled_imu2_send_buf(mavlink_message_t *msgbuf, m packet->xmag = xmag; packet->ymag = ymag; packet->zmag = zmag; + packet->temperature = temperature; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU2_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); #endif @@ -411,6 +425,16 @@ static inline int16_t mavlink_msg_scaled_imu2_get_zmag(const mavlink_message_t* return _MAV_RETURN_int16_t(msg, 20); } +/** + * @brief Get field temperature from scaled_imu2 message + * + * @return [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + */ +static inline int16_t mavlink_msg_scaled_imu2_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 22); +} + /** * @brief Decode a scaled_imu2 message into a struct * @@ -430,6 +454,7 @@ static inline void mavlink_msg_scaled_imu2_decode(const mavlink_message_t* msg, scaled_imu2->xmag = mavlink_msg_scaled_imu2_get_xmag(msg); scaled_imu2->ymag = mavlink_msg_scaled_imu2_get_ymag(msg); scaled_imu2->zmag = mavlink_msg_scaled_imu2_get_zmag(msg); + scaled_imu2->temperature = mavlink_msg_scaled_imu2_get_temperature(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SCALED_IMU2_LEN? msg->len : MAVLINK_MSG_ID_SCALED_IMU2_LEN; memset(scaled_imu2, 0, MAVLINK_MSG_ID_SCALED_IMU2_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_imu3.h b/lib/main/MAVLink/common/mavlink_msg_scaled_imu3.h index d01ac948fd..cd20f23566 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_imu3.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_imu3.h @@ -15,11 +15,12 @@ typedef struct __mavlink_scaled_imu3_t { int16_t xmag; /*< [mgauss] X Magnetic field*/ int16_t ymag; /*< [mgauss] Y Magnetic field*/ int16_t zmag; /*< [mgauss] Z Magnetic field*/ + int16_t temperature; /*< [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C).*/ } mavlink_scaled_imu3_t; -#define MAVLINK_MSG_ID_SCALED_IMU3_LEN 22 +#define MAVLINK_MSG_ID_SCALED_IMU3_LEN 24 #define MAVLINK_MSG_ID_SCALED_IMU3_MIN_LEN 22 -#define MAVLINK_MSG_ID_129_LEN 22 +#define MAVLINK_MSG_ID_129_LEN 24 #define MAVLINK_MSG_ID_129_MIN_LEN 22 #define MAVLINK_MSG_ID_SCALED_IMU3_CRC 46 @@ -31,7 +32,7 @@ typedef struct __mavlink_scaled_imu3_t { #define MAVLINK_MESSAGE_INFO_SCALED_IMU3 { \ 129, \ "SCALED_IMU3", \ - 10, \ + 11, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu3_t, time_boot_ms) }, \ { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu3_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu3_t, yacc) }, \ @@ -42,12 +43,13 @@ typedef struct __mavlink_scaled_imu3_t { { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu3_t, xmag) }, \ { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu3_t, ymag) }, \ { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu3_t, zmag) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_scaled_imu3_t, temperature) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SCALED_IMU3 { \ "SCALED_IMU3", \ - 10, \ + 11, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu3_t, time_boot_ms) }, \ { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu3_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu3_t, yacc) }, \ @@ -58,6 +60,7 @@ typedef struct __mavlink_scaled_imu3_t { { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu3_t, xmag) }, \ { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu3_t, ymag) }, \ { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu3_t, zmag) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_scaled_imu3_t, temperature) }, \ } \ } #endif @@ -78,10 +81,11 @@ typedef struct __mavlink_scaled_imu3_t { * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_imu3_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) + uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU3_LEN]; @@ -95,6 +99,7 @@ static inline uint16_t mavlink_msg_scaled_imu3_pack(uint8_t system_id, uint8_t c _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU3_LEN); #else @@ -109,6 +114,7 @@ static inline uint16_t mavlink_msg_scaled_imu3_pack(uint8_t system_id, uint8_t c packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU3_LEN); #endif @@ -133,11 +139,12 @@ static inline uint16_t mavlink_msg_scaled_imu3_pack(uint8_t system_id, uint8_t c * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_imu3_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) + uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag,int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU3_LEN]; @@ -151,6 +158,7 @@ static inline uint16_t mavlink_msg_scaled_imu3_pack_chan(uint8_t system_id, uint _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU3_LEN); #else @@ -165,6 +173,7 @@ static inline uint16_t mavlink_msg_scaled_imu3_pack_chan(uint8_t system_id, uint packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU3_LEN); #endif @@ -183,7 +192,7 @@ static inline uint16_t mavlink_msg_scaled_imu3_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_scaled_imu3_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_imu3_t* scaled_imu3) { - return mavlink_msg_scaled_imu3_pack(system_id, component_id, msg, scaled_imu3->time_boot_ms, scaled_imu3->xacc, scaled_imu3->yacc, scaled_imu3->zacc, scaled_imu3->xgyro, scaled_imu3->ygyro, scaled_imu3->zgyro, scaled_imu3->xmag, scaled_imu3->ymag, scaled_imu3->zmag); + return mavlink_msg_scaled_imu3_pack(system_id, component_id, msg, scaled_imu3->time_boot_ms, scaled_imu3->xacc, scaled_imu3->yacc, scaled_imu3->zacc, scaled_imu3->xgyro, scaled_imu3->ygyro, scaled_imu3->zgyro, scaled_imu3->xmag, scaled_imu3->ymag, scaled_imu3->zmag, scaled_imu3->temperature); } /** @@ -197,7 +206,7 @@ static inline uint16_t mavlink_msg_scaled_imu3_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_scaled_imu3_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu3_t* scaled_imu3) { - return mavlink_msg_scaled_imu3_pack_chan(system_id, component_id, chan, msg, scaled_imu3->time_boot_ms, scaled_imu3->xacc, scaled_imu3->yacc, scaled_imu3->zacc, scaled_imu3->xgyro, scaled_imu3->ygyro, scaled_imu3->zgyro, scaled_imu3->xmag, scaled_imu3->ymag, scaled_imu3->zmag); + return mavlink_msg_scaled_imu3_pack_chan(system_id, component_id, chan, msg, scaled_imu3->time_boot_ms, scaled_imu3->xacc, scaled_imu3->yacc, scaled_imu3->zacc, scaled_imu3->xgyro, scaled_imu3->ygyro, scaled_imu3->zgyro, scaled_imu3->xmag, scaled_imu3->ymag, scaled_imu3->zmag, scaled_imu3->temperature); } /** @@ -214,10 +223,11 @@ static inline uint16_t mavlink_msg_scaled_imu3_encode_chan(uint8_t system_id, ui * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_scaled_imu3_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +static inline void mavlink_msg_scaled_imu3_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU3_LEN]; @@ -231,6 +241,7 @@ static inline void mavlink_msg_scaled_imu3_send(mavlink_channel_t chan, uint32_t _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, buf, MAVLINK_MSG_ID_SCALED_IMU3_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); #else @@ -245,6 +256,7 @@ static inline void mavlink_msg_scaled_imu3_send(mavlink_channel_t chan, uint32_t packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU3_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); #endif @@ -258,7 +270,7 @@ static inline void mavlink_msg_scaled_imu3_send(mavlink_channel_t chan, uint32_t static inline void mavlink_msg_scaled_imu3_send_struct(mavlink_channel_t chan, const mavlink_scaled_imu3_t* scaled_imu3) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_scaled_imu3_send(chan, scaled_imu3->time_boot_ms, scaled_imu3->xacc, scaled_imu3->yacc, scaled_imu3->zacc, scaled_imu3->xgyro, scaled_imu3->ygyro, scaled_imu3->zgyro, scaled_imu3->xmag, scaled_imu3->ymag, scaled_imu3->zmag); + mavlink_msg_scaled_imu3_send(chan, scaled_imu3->time_boot_ms, scaled_imu3->xacc, scaled_imu3->yacc, scaled_imu3->zacc, scaled_imu3->xgyro, scaled_imu3->ygyro, scaled_imu3->zgyro, scaled_imu3->xmag, scaled_imu3->ymag, scaled_imu3->zmag, scaled_imu3->temperature); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, (const char *)scaled_imu3, MAVLINK_MSG_ID_SCALED_IMU3_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); #endif @@ -272,7 +284,7 @@ static inline void mavlink_msg_scaled_imu3_send_struct(mavlink_channel_t chan, c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_scaled_imu3_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +static inline void mavlink_msg_scaled_imu3_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -286,6 +298,7 @@ static inline void mavlink_msg_scaled_imu3_send_buf(mavlink_message_t *msgbuf, m _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, buf, MAVLINK_MSG_ID_SCALED_IMU3_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); #else @@ -300,6 +313,7 @@ static inline void mavlink_msg_scaled_imu3_send_buf(mavlink_message_t *msgbuf, m packet->xmag = xmag; packet->ymag = ymag; packet->zmag = zmag; + packet->temperature = temperature; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU3_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); #endif @@ -411,6 +425,16 @@ static inline int16_t mavlink_msg_scaled_imu3_get_zmag(const mavlink_message_t* return _MAV_RETURN_int16_t(msg, 20); } +/** + * @brief Get field temperature from scaled_imu3 message + * + * @return [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + */ +static inline int16_t mavlink_msg_scaled_imu3_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 22); +} + /** * @brief Decode a scaled_imu3 message into a struct * @@ -430,6 +454,7 @@ static inline void mavlink_msg_scaled_imu3_decode(const mavlink_message_t* msg, scaled_imu3->xmag = mavlink_msg_scaled_imu3_get_xmag(msg); scaled_imu3->ymag = mavlink_msg_scaled_imu3_get_ymag(msg); scaled_imu3->zmag = mavlink_msg_scaled_imu3_get_zmag(msg); + scaled_imu3->temperature = mavlink_msg_scaled_imu3_get_temperature(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SCALED_IMU3_LEN? msg->len : MAVLINK_MSG_ID_SCALED_IMU3_LEN; memset(scaled_imu3, 0, MAVLINK_MSG_ID_SCALED_IMU3_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure.h b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure.h index 9889c4676a..13344a8a68 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure.h @@ -9,11 +9,12 @@ typedef struct __mavlink_scaled_pressure_t { float press_abs; /*< [hPa] Absolute pressure*/ float press_diff; /*< [hPa] Differential pressure 1*/ int16_t temperature; /*< [cdegC] Absolute pressure temperature*/ + int16_t temperature_press_diff; /*< [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC.*/ } mavlink_scaled_pressure_t; -#define MAVLINK_MSG_ID_SCALED_PRESSURE_LEN 14 +#define MAVLINK_MSG_ID_SCALED_PRESSURE_LEN 16 #define MAVLINK_MSG_ID_SCALED_PRESSURE_MIN_LEN 14 -#define MAVLINK_MSG_ID_29_LEN 14 +#define MAVLINK_MSG_ID_29_LEN 16 #define MAVLINK_MSG_ID_29_MIN_LEN 14 #define MAVLINK_MSG_ID_SCALED_PRESSURE_CRC 115 @@ -25,21 +26,23 @@ typedef struct __mavlink_scaled_pressure_t { #define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE { \ 29, \ "SCALED_PRESSURE", \ - 4, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_pressure_t, time_boot_ms) }, \ { "press_abs", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_scaled_pressure_t, press_abs) }, \ { "press_diff", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure_t, press_diff) }, \ { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_pressure_t, temperature) }, \ + { "temperature_press_diff", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_pressure_t, temperature_press_diff) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE { \ "SCALED_PRESSURE", \ - 4, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_pressure_t, time_boot_ms) }, \ { "press_abs", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_scaled_pressure_t, press_abs) }, \ { "press_diff", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure_t, press_diff) }, \ { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_pressure_t, temperature) }, \ + { "temperature_press_diff", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_pressure_t, temperature_press_diff) }, \ } \ } #endif @@ -54,10 +57,11 @@ typedef struct __mavlink_scaled_pressure_t { * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure 1 * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) + uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE_LEN]; @@ -65,6 +69,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8 _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); #else @@ -73,6 +78,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8 packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); #endif @@ -91,11 +97,12 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8 * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure 1 * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature) + uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature,int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE_LEN]; @@ -103,6 +110,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); #else @@ -111,6 +119,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); #endif @@ -129,7 +138,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_pressure_t* scaled_pressure) { - return mavlink_msg_scaled_pressure_pack(system_id, component_id, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature); + return mavlink_msg_scaled_pressure_pack(system_id, component_id, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature, scaled_pressure->temperature_press_diff); } /** @@ -143,7 +152,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_scaled_pressure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_pressure_t* scaled_pressure) { - return mavlink_msg_scaled_pressure_pack_chan(system_id, component_id, chan, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature); + return mavlink_msg_scaled_pressure_pack_chan(system_id, component_id, chan, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature, scaled_pressure->temperature_press_diff); } /** @@ -154,10 +163,11 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode_chan(uint8_t system_id * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure 1 * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE_LEN]; @@ -165,6 +175,7 @@ static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); #else @@ -173,6 +184,7 @@ static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)&packet, MAVLINK_MSG_ID_SCALED_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); #endif @@ -186,7 +198,7 @@ static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint static inline void mavlink_msg_scaled_pressure_send_struct(mavlink_channel_t chan, const mavlink_scaled_pressure_t* scaled_pressure) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_scaled_pressure_send(chan, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature); + mavlink_msg_scaled_pressure_send(chan, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature, scaled_pressure->temperature_press_diff); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)scaled_pressure, MAVLINK_MSG_ID_SCALED_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); #endif @@ -200,7 +212,7 @@ static inline void mavlink_msg_scaled_pressure_send_struct(mavlink_channel_t cha is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_scaled_pressure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +static inline void mavlink_msg_scaled_pressure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +220,7 @@ static inline void mavlink_msg_scaled_pressure_send_buf(mavlink_message_t *msgbu _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); #else @@ -216,6 +229,7 @@ static inline void mavlink_msg_scaled_pressure_send_buf(mavlink_message_t *msgbu packet->press_abs = press_abs; packet->press_diff = press_diff; packet->temperature = temperature; + packet->temperature_press_diff = temperature_press_diff; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); #endif @@ -267,6 +281,16 @@ static inline int16_t mavlink_msg_scaled_pressure_get_temperature(const mavlink_ return _MAV_RETURN_int16_t(msg, 12); } +/** + * @brief Get field temperature_press_diff from scaled_pressure message + * + * @return [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. + */ +static inline int16_t mavlink_msg_scaled_pressure_get_temperature_press_diff(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + /** * @brief Decode a scaled_pressure message into a struct * @@ -280,6 +304,7 @@ static inline void mavlink_msg_scaled_pressure_decode(const mavlink_message_t* m scaled_pressure->press_abs = mavlink_msg_scaled_pressure_get_press_abs(msg); scaled_pressure->press_diff = mavlink_msg_scaled_pressure_get_press_diff(msg); scaled_pressure->temperature = mavlink_msg_scaled_pressure_get_temperature(msg); + scaled_pressure->temperature_press_diff = mavlink_msg_scaled_pressure_get_temperature_press_diff(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SCALED_PRESSURE_LEN? msg->len : MAVLINK_MSG_ID_SCALED_PRESSURE_LEN; memset(scaled_pressure, 0, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure2.h b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure2.h index 7ac74c6758..8dc28850e8 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure2.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure2.h @@ -9,11 +9,12 @@ typedef struct __mavlink_scaled_pressure2_t { float press_abs; /*< [hPa] Absolute pressure*/ float press_diff; /*< [hPa] Differential pressure*/ int16_t temperature; /*< [cdegC] Absolute pressure temperature*/ + int16_t temperature_press_diff; /*< [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC.*/ } mavlink_scaled_pressure2_t; -#define MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN 14 +#define MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN 16 #define MAVLINK_MSG_ID_SCALED_PRESSURE2_MIN_LEN 14 -#define MAVLINK_MSG_ID_137_LEN 14 +#define MAVLINK_MSG_ID_137_LEN 16 #define MAVLINK_MSG_ID_137_MIN_LEN 14 #define MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC 195 @@ -25,21 +26,23 @@ typedef struct __mavlink_scaled_pressure2_t { #define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE2 { \ 137, \ "SCALED_PRESSURE2", \ - 4, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_pressure2_t, time_boot_ms) }, \ { "press_abs", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_scaled_pressure2_t, press_abs) }, \ { "press_diff", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure2_t, press_diff) }, \ { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_pressure2_t, temperature) }, \ + { "temperature_press_diff", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_pressure2_t, temperature_press_diff) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE2 { \ "SCALED_PRESSURE2", \ - 4, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_pressure2_t, time_boot_ms) }, \ { "press_abs", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_scaled_pressure2_t, press_abs) }, \ { "press_diff", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure2_t, press_diff) }, \ { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_pressure2_t, temperature) }, \ + { "temperature_press_diff", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_pressure2_t, temperature_press_diff) }, \ } \ } #endif @@ -54,10 +57,11 @@ typedef struct __mavlink_scaled_pressure2_t { * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_pressure2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) + uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN]; @@ -65,6 +69,7 @@ static inline uint16_t mavlink_msg_scaled_pressure2_pack(uint8_t system_id, uint _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); #else @@ -73,6 +78,7 @@ static inline uint16_t mavlink_msg_scaled_pressure2_pack(uint8_t system_id, uint packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); #endif @@ -91,11 +97,12 @@ static inline uint16_t mavlink_msg_scaled_pressure2_pack(uint8_t system_id, uint * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_pressure2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature) + uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature,int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN]; @@ -103,6 +110,7 @@ static inline uint16_t mavlink_msg_scaled_pressure2_pack_chan(uint8_t system_id, _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); #else @@ -111,6 +119,7 @@ static inline uint16_t mavlink_msg_scaled_pressure2_pack_chan(uint8_t system_id, packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); #endif @@ -129,7 +138,7 @@ static inline uint16_t mavlink_msg_scaled_pressure2_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_scaled_pressure2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_pressure2_t* scaled_pressure2) { - return mavlink_msg_scaled_pressure2_pack(system_id, component_id, msg, scaled_pressure2->time_boot_ms, scaled_pressure2->press_abs, scaled_pressure2->press_diff, scaled_pressure2->temperature); + return mavlink_msg_scaled_pressure2_pack(system_id, component_id, msg, scaled_pressure2->time_boot_ms, scaled_pressure2->press_abs, scaled_pressure2->press_diff, scaled_pressure2->temperature, scaled_pressure2->temperature_press_diff); } /** @@ -143,7 +152,7 @@ static inline uint16_t mavlink_msg_scaled_pressure2_encode(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_scaled_pressure2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_pressure2_t* scaled_pressure2) { - return mavlink_msg_scaled_pressure2_pack_chan(system_id, component_id, chan, msg, scaled_pressure2->time_boot_ms, scaled_pressure2->press_abs, scaled_pressure2->press_diff, scaled_pressure2->temperature); + return mavlink_msg_scaled_pressure2_pack_chan(system_id, component_id, chan, msg, scaled_pressure2->time_boot_ms, scaled_pressure2->press_abs, scaled_pressure2->press_diff, scaled_pressure2->temperature, scaled_pressure2->temperature_press_diff); } /** @@ -154,10 +163,11 @@ static inline uint16_t mavlink_msg_scaled_pressure2_encode_chan(uint8_t system_i * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_scaled_pressure2_send(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +static inline void mavlink_msg_scaled_pressure2_send(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN]; @@ -165,6 +175,7 @@ static inline void mavlink_msg_scaled_pressure2_send(mavlink_channel_t chan, uin _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, buf, MAVLINK_MSG_ID_SCALED_PRESSURE2_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); #else @@ -173,6 +184,7 @@ static inline void mavlink_msg_scaled_pressure2_send(mavlink_channel_t chan, uin packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, (const char *)&packet, MAVLINK_MSG_ID_SCALED_PRESSURE2_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); #endif @@ -186,7 +198,7 @@ static inline void mavlink_msg_scaled_pressure2_send(mavlink_channel_t chan, uin static inline void mavlink_msg_scaled_pressure2_send_struct(mavlink_channel_t chan, const mavlink_scaled_pressure2_t* scaled_pressure2) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_scaled_pressure2_send(chan, scaled_pressure2->time_boot_ms, scaled_pressure2->press_abs, scaled_pressure2->press_diff, scaled_pressure2->temperature); + mavlink_msg_scaled_pressure2_send(chan, scaled_pressure2->time_boot_ms, scaled_pressure2->press_abs, scaled_pressure2->press_diff, scaled_pressure2->temperature, scaled_pressure2->temperature_press_diff); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, (const char *)scaled_pressure2, MAVLINK_MSG_ID_SCALED_PRESSURE2_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); #endif @@ -200,7 +212,7 @@ static inline void mavlink_msg_scaled_pressure2_send_struct(mavlink_channel_t ch is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_scaled_pressure2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +static inline void mavlink_msg_scaled_pressure2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +220,7 @@ static inline void mavlink_msg_scaled_pressure2_send_buf(mavlink_message_t *msgb _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, buf, MAVLINK_MSG_ID_SCALED_PRESSURE2_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); #else @@ -216,6 +229,7 @@ static inline void mavlink_msg_scaled_pressure2_send_buf(mavlink_message_t *msgb packet->press_abs = press_abs; packet->press_diff = press_diff; packet->temperature = temperature; + packet->temperature_press_diff = temperature_press_diff; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE2_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); #endif @@ -267,6 +281,16 @@ static inline int16_t mavlink_msg_scaled_pressure2_get_temperature(const mavlink return _MAV_RETURN_int16_t(msg, 12); } +/** + * @brief Get field temperature_press_diff from scaled_pressure2 message + * + * @return [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. + */ +static inline int16_t mavlink_msg_scaled_pressure2_get_temperature_press_diff(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + /** * @brief Decode a scaled_pressure2 message into a struct * @@ -280,6 +304,7 @@ static inline void mavlink_msg_scaled_pressure2_decode(const mavlink_message_t* scaled_pressure2->press_abs = mavlink_msg_scaled_pressure2_get_press_abs(msg); scaled_pressure2->press_diff = mavlink_msg_scaled_pressure2_get_press_diff(msg); scaled_pressure2->temperature = mavlink_msg_scaled_pressure2_get_temperature(msg); + scaled_pressure2->temperature_press_diff = mavlink_msg_scaled_pressure2_get_temperature_press_diff(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN? msg->len : MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN; memset(scaled_pressure2, 0, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure3.h b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure3.h index e0007cf8ed..34368b1efb 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure3.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure3.h @@ -9,11 +9,12 @@ typedef struct __mavlink_scaled_pressure3_t { float press_abs; /*< [hPa] Absolute pressure*/ float press_diff; /*< [hPa] Differential pressure*/ int16_t temperature; /*< [cdegC] Absolute pressure temperature*/ + int16_t temperature_press_diff; /*< [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC.*/ } mavlink_scaled_pressure3_t; -#define MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN 14 +#define MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN 16 #define MAVLINK_MSG_ID_SCALED_PRESSURE3_MIN_LEN 14 -#define MAVLINK_MSG_ID_143_LEN 14 +#define MAVLINK_MSG_ID_143_LEN 16 #define MAVLINK_MSG_ID_143_MIN_LEN 14 #define MAVLINK_MSG_ID_SCALED_PRESSURE3_CRC 131 @@ -25,21 +26,23 @@ typedef struct __mavlink_scaled_pressure3_t { #define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE3 { \ 143, \ "SCALED_PRESSURE3", \ - 4, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_pressure3_t, time_boot_ms) }, \ { "press_abs", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_scaled_pressure3_t, press_abs) }, \ { "press_diff", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure3_t, press_diff) }, \ { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_pressure3_t, temperature) }, \ + { "temperature_press_diff", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_pressure3_t, temperature_press_diff) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE3 { \ "SCALED_PRESSURE3", \ - 4, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_pressure3_t, time_boot_ms) }, \ { "press_abs", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_scaled_pressure3_t, press_abs) }, \ { "press_diff", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure3_t, press_diff) }, \ { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_pressure3_t, temperature) }, \ + { "temperature_press_diff", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_pressure3_t, temperature_press_diff) }, \ } \ } #endif @@ -54,10 +57,11 @@ typedef struct __mavlink_scaled_pressure3_t { * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_pressure3_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) + uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN]; @@ -65,6 +69,7 @@ static inline uint16_t mavlink_msg_scaled_pressure3_pack(uint8_t system_id, uint _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN); #else @@ -73,6 +78,7 @@ static inline uint16_t mavlink_msg_scaled_pressure3_pack(uint8_t system_id, uint packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN); #endif @@ -91,11 +97,12 @@ static inline uint16_t mavlink_msg_scaled_pressure3_pack(uint8_t system_id, uint * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_pressure3_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature) + uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature,int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN]; @@ -103,6 +110,7 @@ static inline uint16_t mavlink_msg_scaled_pressure3_pack_chan(uint8_t system_id, _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN); #else @@ -111,6 +119,7 @@ static inline uint16_t mavlink_msg_scaled_pressure3_pack_chan(uint8_t system_id, packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN); #endif @@ -129,7 +138,7 @@ static inline uint16_t mavlink_msg_scaled_pressure3_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_scaled_pressure3_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_pressure3_t* scaled_pressure3) { - return mavlink_msg_scaled_pressure3_pack(system_id, component_id, msg, scaled_pressure3->time_boot_ms, scaled_pressure3->press_abs, scaled_pressure3->press_diff, scaled_pressure3->temperature); + return mavlink_msg_scaled_pressure3_pack(system_id, component_id, msg, scaled_pressure3->time_boot_ms, scaled_pressure3->press_abs, scaled_pressure3->press_diff, scaled_pressure3->temperature, scaled_pressure3->temperature_press_diff); } /** @@ -143,7 +152,7 @@ static inline uint16_t mavlink_msg_scaled_pressure3_encode(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_scaled_pressure3_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_pressure3_t* scaled_pressure3) { - return mavlink_msg_scaled_pressure3_pack_chan(system_id, component_id, chan, msg, scaled_pressure3->time_boot_ms, scaled_pressure3->press_abs, scaled_pressure3->press_diff, scaled_pressure3->temperature); + return mavlink_msg_scaled_pressure3_pack_chan(system_id, component_id, chan, msg, scaled_pressure3->time_boot_ms, scaled_pressure3->press_abs, scaled_pressure3->press_diff, scaled_pressure3->temperature, scaled_pressure3->temperature_press_diff); } /** @@ -154,10 +163,11 @@ static inline uint16_t mavlink_msg_scaled_pressure3_encode_chan(uint8_t system_i * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_scaled_pressure3_send(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +static inline void mavlink_msg_scaled_pressure3_send(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN]; @@ -165,6 +175,7 @@ static inline void mavlink_msg_scaled_pressure3_send(mavlink_channel_t chan, uin _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE3, buf, MAVLINK_MSG_ID_SCALED_PRESSURE3_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_CRC); #else @@ -173,6 +184,7 @@ static inline void mavlink_msg_scaled_pressure3_send(mavlink_channel_t chan, uin packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE3, (const char *)&packet, MAVLINK_MSG_ID_SCALED_PRESSURE3_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_CRC); #endif @@ -186,7 +198,7 @@ static inline void mavlink_msg_scaled_pressure3_send(mavlink_channel_t chan, uin static inline void mavlink_msg_scaled_pressure3_send_struct(mavlink_channel_t chan, const mavlink_scaled_pressure3_t* scaled_pressure3) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_scaled_pressure3_send(chan, scaled_pressure3->time_boot_ms, scaled_pressure3->press_abs, scaled_pressure3->press_diff, scaled_pressure3->temperature); + mavlink_msg_scaled_pressure3_send(chan, scaled_pressure3->time_boot_ms, scaled_pressure3->press_abs, scaled_pressure3->press_diff, scaled_pressure3->temperature, scaled_pressure3->temperature_press_diff); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE3, (const char *)scaled_pressure3, MAVLINK_MSG_ID_SCALED_PRESSURE3_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_CRC); #endif @@ -200,7 +212,7 @@ static inline void mavlink_msg_scaled_pressure3_send_struct(mavlink_channel_t ch is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_scaled_pressure3_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +static inline void mavlink_msg_scaled_pressure3_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +220,7 @@ static inline void mavlink_msg_scaled_pressure3_send_buf(mavlink_message_t *msgb _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE3, buf, MAVLINK_MSG_ID_SCALED_PRESSURE3_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_CRC); #else @@ -216,6 +229,7 @@ static inline void mavlink_msg_scaled_pressure3_send_buf(mavlink_message_t *msgb packet->press_abs = press_abs; packet->press_diff = press_diff; packet->temperature = temperature; + packet->temperature_press_diff = temperature_press_diff; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE3, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE3_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_CRC); #endif @@ -267,6 +281,16 @@ static inline int16_t mavlink_msg_scaled_pressure3_get_temperature(const mavlink return _MAV_RETURN_int16_t(msg, 12); } +/** + * @brief Get field temperature_press_diff from scaled_pressure3 message + * + * @return [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. + */ +static inline int16_t mavlink_msg_scaled_pressure3_get_temperature_press_diff(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + /** * @brief Decode a scaled_pressure3 message into a struct * @@ -280,6 +304,7 @@ static inline void mavlink_msg_scaled_pressure3_decode(const mavlink_message_t* scaled_pressure3->press_abs = mavlink_msg_scaled_pressure3_get_press_abs(msg); scaled_pressure3->press_diff = mavlink_msg_scaled_pressure3_get_press_diff(msg); scaled_pressure3->temperature = mavlink_msg_scaled_pressure3_get_temperature(msg); + scaled_pressure3->temperature_press_diff = mavlink_msg_scaled_pressure3_get_temperature_press_diff(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN? msg->len : MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN; memset(scaled_pressure3, 0, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_servo_output_raw.h b/lib/main/MAVLink/common/mavlink_msg_servo_output_raw.h index 840c6a25f6..4b76d20a8f 100755 --- a/lib/main/MAVLink/common/mavlink_msg_servo_output_raw.h +++ b/lib/main/MAVLink/common/mavlink_msg_servo_output_raw.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW 36 - +MAVPACKED( typedef struct __mavlink_servo_output_raw_t { uint32_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ uint16_t servo1_raw; /*< [us] Servo output 1 value*/ @@ -15,11 +15,19 @@ typedef struct __mavlink_servo_output_raw_t { uint16_t servo7_raw; /*< [us] Servo output 7 value*/ uint16_t servo8_raw; /*< [us] Servo output 8 value*/ uint8_t port; /*< Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX.*/ -} mavlink_servo_output_raw_t; + uint16_t servo9_raw; /*< [us] Servo output 9 value*/ + uint16_t servo10_raw; /*< [us] Servo output 10 value*/ + uint16_t servo11_raw; /*< [us] Servo output 11 value*/ + uint16_t servo12_raw; /*< [us] Servo output 12 value*/ + uint16_t servo13_raw; /*< [us] Servo output 13 value*/ + uint16_t servo14_raw; /*< [us] Servo output 14 value*/ + uint16_t servo15_raw; /*< [us] Servo output 15 value*/ + uint16_t servo16_raw; /*< [us] Servo output 16 value*/ +}) mavlink_servo_output_raw_t; -#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN 21 +#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN 37 #define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN 21 -#define MAVLINK_MSG_ID_36_LEN 21 +#define MAVLINK_MSG_ID_36_LEN 37 #define MAVLINK_MSG_ID_36_MIN_LEN 21 #define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC 222 @@ -31,7 +39,7 @@ typedef struct __mavlink_servo_output_raw_t { #define MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW { \ 36, \ "SERVO_OUTPUT_RAW", \ - 10, \ + 18, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_servo_output_raw_t, time_usec) }, \ { "port", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_servo_output_raw_t, port) }, \ { "servo1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_servo_output_raw_t, servo1_raw) }, \ @@ -42,12 +50,20 @@ typedef struct __mavlink_servo_output_raw_t { { "servo6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_servo_output_raw_t, servo6_raw) }, \ { "servo7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_servo_output_raw_t, servo7_raw) }, \ { "servo8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_servo_output_raw_t, servo8_raw) }, \ + { "servo9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 21, offsetof(mavlink_servo_output_raw_t, servo9_raw) }, \ + { "servo10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 23, offsetof(mavlink_servo_output_raw_t, servo10_raw) }, \ + { "servo11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 25, offsetof(mavlink_servo_output_raw_t, servo11_raw) }, \ + { "servo12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 27, offsetof(mavlink_servo_output_raw_t, servo12_raw) }, \ + { "servo13_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 29, offsetof(mavlink_servo_output_raw_t, servo13_raw) }, \ + { "servo14_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 31, offsetof(mavlink_servo_output_raw_t, servo14_raw) }, \ + { "servo15_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 33, offsetof(mavlink_servo_output_raw_t, servo15_raw) }, \ + { "servo16_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 35, offsetof(mavlink_servo_output_raw_t, servo16_raw) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW { \ "SERVO_OUTPUT_RAW", \ - 10, \ + 18, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_servo_output_raw_t, time_usec) }, \ { "port", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_servo_output_raw_t, port) }, \ { "servo1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_servo_output_raw_t, servo1_raw) }, \ @@ -58,6 +74,14 @@ typedef struct __mavlink_servo_output_raw_t { { "servo6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_servo_output_raw_t, servo6_raw) }, \ { "servo7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_servo_output_raw_t, servo7_raw) }, \ { "servo8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_servo_output_raw_t, servo8_raw) }, \ + { "servo9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 21, offsetof(mavlink_servo_output_raw_t, servo9_raw) }, \ + { "servo10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 23, offsetof(mavlink_servo_output_raw_t, servo10_raw) }, \ + { "servo11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 25, offsetof(mavlink_servo_output_raw_t, servo11_raw) }, \ + { "servo12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 27, offsetof(mavlink_servo_output_raw_t, servo12_raw) }, \ + { "servo13_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 29, offsetof(mavlink_servo_output_raw_t, servo13_raw) }, \ + { "servo14_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 31, offsetof(mavlink_servo_output_raw_t, servo14_raw) }, \ + { "servo15_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 33, offsetof(mavlink_servo_output_raw_t, servo15_raw) }, \ + { "servo16_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 35, offsetof(mavlink_servo_output_raw_t, servo16_raw) }, \ } \ } #endif @@ -78,10 +102,18 @@ typedef struct __mavlink_servo_output_raw_t { * @param servo6_raw [us] Servo output 6 value * @param servo7_raw [us] Servo output 7 value * @param servo8_raw [us] Servo output 8 value + * @param servo9_raw [us] Servo output 9 value + * @param servo10_raw [us] Servo output 10 value + * @param servo11_raw [us] Servo output 11 value + * @param servo12_raw [us] Servo output 12 value + * @param servo13_raw [us] Servo output 13 value + * @param servo14_raw [us] Servo output 14 value + * @param servo15_raw [us] Servo output 15 value + * @param servo16_raw [us] Servo output 16 value * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) + uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw, uint16_t servo9_raw, uint16_t servo10_raw, uint16_t servo11_raw, uint16_t servo12_raw, uint16_t servo13_raw, uint16_t servo14_raw, uint16_t servo15_raw, uint16_t servo16_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN]; @@ -95,6 +127,14 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint _mav_put_uint16_t(buf, 16, servo7_raw); _mav_put_uint16_t(buf, 18, servo8_raw); _mav_put_uint8_t(buf, 20, port); + _mav_put_uint16_t(buf, 21, servo9_raw); + _mav_put_uint16_t(buf, 23, servo10_raw); + _mav_put_uint16_t(buf, 25, servo11_raw); + _mav_put_uint16_t(buf, 27, servo12_raw); + _mav_put_uint16_t(buf, 29, servo13_raw); + _mav_put_uint16_t(buf, 31, servo14_raw); + _mav_put_uint16_t(buf, 33, servo15_raw); + _mav_put_uint16_t(buf, 35, servo16_raw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); #else @@ -109,6 +149,14 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint packet.servo7_raw = servo7_raw; packet.servo8_raw = servo8_raw; packet.port = port; + packet.servo9_raw = servo9_raw; + packet.servo10_raw = servo10_raw; + packet.servo11_raw = servo11_raw; + packet.servo12_raw = servo12_raw; + packet.servo13_raw = servo13_raw; + packet.servo14_raw = servo14_raw; + packet.servo15_raw = servo15_raw; + packet.servo16_raw = servo16_raw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); #endif @@ -133,11 +181,19 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint * @param servo6_raw [us] Servo output 6 value * @param servo7_raw [us] Servo output 7 value * @param servo8_raw [us] Servo output 8 value + * @param servo9_raw [us] Servo output 9 value + * @param servo10_raw [us] Servo output 10 value + * @param servo11_raw [us] Servo output 11 value + * @param servo12_raw [us] Servo output 12 value + * @param servo13_raw [us] Servo output 13 value + * @param servo14_raw [us] Servo output 14 value + * @param servo15_raw [us] Servo output 15 value + * @param servo16_raw [us] Servo output 16 value * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_usec,uint8_t port,uint16_t servo1_raw,uint16_t servo2_raw,uint16_t servo3_raw,uint16_t servo4_raw,uint16_t servo5_raw,uint16_t servo6_raw,uint16_t servo7_raw,uint16_t servo8_raw) + uint32_t time_usec,uint8_t port,uint16_t servo1_raw,uint16_t servo2_raw,uint16_t servo3_raw,uint16_t servo4_raw,uint16_t servo5_raw,uint16_t servo6_raw,uint16_t servo7_raw,uint16_t servo8_raw,uint16_t servo9_raw,uint16_t servo10_raw,uint16_t servo11_raw,uint16_t servo12_raw,uint16_t servo13_raw,uint16_t servo14_raw,uint16_t servo15_raw,uint16_t servo16_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN]; @@ -151,6 +207,14 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, _mav_put_uint16_t(buf, 16, servo7_raw); _mav_put_uint16_t(buf, 18, servo8_raw); _mav_put_uint8_t(buf, 20, port); + _mav_put_uint16_t(buf, 21, servo9_raw); + _mav_put_uint16_t(buf, 23, servo10_raw); + _mav_put_uint16_t(buf, 25, servo11_raw); + _mav_put_uint16_t(buf, 27, servo12_raw); + _mav_put_uint16_t(buf, 29, servo13_raw); + _mav_put_uint16_t(buf, 31, servo14_raw); + _mav_put_uint16_t(buf, 33, servo15_raw); + _mav_put_uint16_t(buf, 35, servo16_raw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); #else @@ -165,6 +229,14 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, packet.servo7_raw = servo7_raw; packet.servo8_raw = servo8_raw; packet.port = port; + packet.servo9_raw = servo9_raw; + packet.servo10_raw = servo10_raw; + packet.servo11_raw = servo11_raw; + packet.servo12_raw = servo12_raw; + packet.servo13_raw = servo13_raw; + packet.servo14_raw = servo14_raw; + packet.servo15_raw = servo15_raw; + packet.servo16_raw = servo16_raw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); #endif @@ -183,7 +255,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw) { - return mavlink_msg_servo_output_raw_pack(system_id, component_id, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); + return mavlink_msg_servo_output_raw_pack(system_id, component_id, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw, servo_output_raw->servo9_raw, servo_output_raw->servo10_raw, servo_output_raw->servo11_raw, servo_output_raw->servo12_raw, servo_output_raw->servo13_raw, servo_output_raw->servo14_raw, servo_output_raw->servo15_raw, servo_output_raw->servo16_raw); } /** @@ -197,7 +269,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_servo_output_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw) { - return mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, chan, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); + return mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, chan, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw, servo_output_raw->servo9_raw, servo_output_raw->servo10_raw, servo_output_raw->servo11_raw, servo_output_raw->servo12_raw, servo_output_raw->servo13_raw, servo_output_raw->servo14_raw, servo_output_raw->servo15_raw, servo_output_raw->servo16_raw); } /** @@ -214,10 +286,18 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode_chan(uint8_t system_i * @param servo6_raw [us] Servo output 6 value * @param servo7_raw [us] Servo output 7 value * @param servo8_raw [us] Servo output 8 value + * @param servo9_raw [us] Servo output 9 value + * @param servo10_raw [us] Servo output 10 value + * @param servo11_raw [us] Servo output 11 value + * @param servo12_raw [us] Servo output 12 value + * @param servo13_raw [us] Servo output 13 value + * @param servo14_raw [us] Servo output 14 value + * @param servo15_raw [us] Servo output 15 value + * @param servo16_raw [us] Servo output 16 value */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) +static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw, uint16_t servo9_raw, uint16_t servo10_raw, uint16_t servo11_raw, uint16_t servo12_raw, uint16_t servo13_raw, uint16_t servo14_raw, uint16_t servo15_raw, uint16_t servo16_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN]; @@ -231,6 +311,14 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin _mav_put_uint16_t(buf, 16, servo7_raw); _mav_put_uint16_t(buf, 18, servo8_raw); _mav_put_uint8_t(buf, 20, port); + _mav_put_uint16_t(buf, 21, servo9_raw); + _mav_put_uint16_t(buf, 23, servo10_raw); + _mav_put_uint16_t(buf, 25, servo11_raw); + _mav_put_uint16_t(buf, 27, servo12_raw); + _mav_put_uint16_t(buf, 29, servo13_raw); + _mav_put_uint16_t(buf, 31, servo14_raw); + _mav_put_uint16_t(buf, 33, servo15_raw); + _mav_put_uint16_t(buf, 35, servo16_raw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); #else @@ -245,6 +333,14 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin packet.servo7_raw = servo7_raw; packet.servo8_raw = servo8_raw; packet.port = port; + packet.servo9_raw = servo9_raw; + packet.servo10_raw = servo10_raw; + packet.servo11_raw = servo11_raw; + packet.servo12_raw = servo12_raw; + packet.servo13_raw = servo13_raw; + packet.servo14_raw = servo14_raw; + packet.servo15_raw = servo15_raw; + packet.servo16_raw = servo16_raw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)&packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); #endif @@ -258,7 +354,7 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin static inline void mavlink_msg_servo_output_raw_send_struct(mavlink_channel_t chan, const mavlink_servo_output_raw_t* servo_output_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_servo_output_raw_send(chan, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); + mavlink_msg_servo_output_raw_send(chan, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw, servo_output_raw->servo9_raw, servo_output_raw->servo10_raw, servo_output_raw->servo11_raw, servo_output_raw->servo12_raw, servo_output_raw->servo13_raw, servo_output_raw->servo14_raw, servo_output_raw->servo15_raw, servo_output_raw->servo16_raw); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)servo_output_raw, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); #endif @@ -272,7 +368,7 @@ static inline void mavlink_msg_servo_output_raw_send_struct(mavlink_channel_t ch is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_servo_output_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) +static inline void mavlink_msg_servo_output_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw, uint16_t servo9_raw, uint16_t servo10_raw, uint16_t servo11_raw, uint16_t servo12_raw, uint16_t servo13_raw, uint16_t servo14_raw, uint16_t servo15_raw, uint16_t servo16_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -286,6 +382,14 @@ static inline void mavlink_msg_servo_output_raw_send_buf(mavlink_message_t *msgb _mav_put_uint16_t(buf, 16, servo7_raw); _mav_put_uint16_t(buf, 18, servo8_raw); _mav_put_uint8_t(buf, 20, port); + _mav_put_uint16_t(buf, 21, servo9_raw); + _mav_put_uint16_t(buf, 23, servo10_raw); + _mav_put_uint16_t(buf, 25, servo11_raw); + _mav_put_uint16_t(buf, 27, servo12_raw); + _mav_put_uint16_t(buf, 29, servo13_raw); + _mav_put_uint16_t(buf, 31, servo14_raw); + _mav_put_uint16_t(buf, 33, servo15_raw); + _mav_put_uint16_t(buf, 35, servo16_raw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); #else @@ -300,6 +404,14 @@ static inline void mavlink_msg_servo_output_raw_send_buf(mavlink_message_t *msgb packet->servo7_raw = servo7_raw; packet->servo8_raw = servo8_raw; packet->port = port; + packet->servo9_raw = servo9_raw; + packet->servo10_raw = servo10_raw; + packet->servo11_raw = servo11_raw; + packet->servo12_raw = servo12_raw; + packet->servo13_raw = servo13_raw; + packet->servo14_raw = servo14_raw; + packet->servo15_raw = servo15_raw; + packet->servo16_raw = servo16_raw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); #endif @@ -411,6 +523,86 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo8_raw(const mavlink return _MAV_RETURN_uint16_t(msg, 18); } +/** + * @brief Get field servo9_raw from servo_output_raw message + * + * @return [us] Servo output 9 value + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo9_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 21); +} + +/** + * @brief Get field servo10_raw from servo_output_raw message + * + * @return [us] Servo output 10 value + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo10_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 23); +} + +/** + * @brief Get field servo11_raw from servo_output_raw message + * + * @return [us] Servo output 11 value + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo11_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 25); +} + +/** + * @brief Get field servo12_raw from servo_output_raw message + * + * @return [us] Servo output 12 value + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo12_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 27); +} + +/** + * @brief Get field servo13_raw from servo_output_raw message + * + * @return [us] Servo output 13 value + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo13_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 29); +} + +/** + * @brief Get field servo14_raw from servo_output_raw message + * + * @return [us] Servo output 14 value + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo14_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 31); +} + +/** + * @brief Get field servo15_raw from servo_output_raw message + * + * @return [us] Servo output 15 value + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo15_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 33); +} + +/** + * @brief Get field servo16_raw from servo_output_raw message + * + * @return [us] Servo output 16 value + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo16_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 35); +} + /** * @brief Decode a servo_output_raw message into a struct * @@ -430,6 +622,14 @@ static inline void mavlink_msg_servo_output_raw_decode(const mavlink_message_t* servo_output_raw->servo7_raw = mavlink_msg_servo_output_raw_get_servo7_raw(msg); servo_output_raw->servo8_raw = mavlink_msg_servo_output_raw_get_servo8_raw(msg); servo_output_raw->port = mavlink_msg_servo_output_raw_get_port(msg); + servo_output_raw->servo9_raw = mavlink_msg_servo_output_raw_get_servo9_raw(msg); + servo_output_raw->servo10_raw = mavlink_msg_servo_output_raw_get_servo10_raw(msg); + servo_output_raw->servo11_raw = mavlink_msg_servo_output_raw_get_servo11_raw(msg); + servo_output_raw->servo12_raw = mavlink_msg_servo_output_raw_get_servo12_raw(msg); + servo_output_raw->servo13_raw = mavlink_msg_servo_output_raw_get_servo13_raw(msg); + servo_output_raw->servo14_raw = mavlink_msg_servo_output_raw_get_servo14_raw(msg); + servo_output_raw->servo15_raw = mavlink_msg_servo_output_raw_get_servo15_raw(msg); + servo_output_raw->servo16_raw = mavlink_msg_servo_output_raw_get_servo16_raw(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN? msg->len : MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN; memset(servo_output_raw, 0, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_set_attitude_target.h b/lib/main/MAVLink/common/mavlink_msg_set_attitude_target.h index 94a2e856b8..df1e851ebe 100755 --- a/lib/main/MAVLink/common/mavlink_msg_set_attitude_target.h +++ b/lib/main/MAVLink/common/mavlink_msg_set_attitude_target.h @@ -13,7 +13,7 @@ typedef struct __mavlink_set_attitude_target_t { float thrust; /*< Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust)*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ - uint8_t type_mask; /*< Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude*/ + uint8_t type_mask; /*< Bitmap to indicate which dimensions should be ignored by the vehicle.*/ } mavlink_set_attitude_target_t; #define MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN 39 @@ -68,7 +68,7 @@ typedef struct __mavlink_set_attitude_target_t { * @param time_boot_ms [ms] Timestamp (time since system boot). * @param target_system System ID * @param target_component Component ID - * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param body_roll_rate [rad/s] Body roll rate * @param body_pitch_rate [rad/s] Body pitch rate @@ -118,7 +118,7 @@ static inline uint16_t mavlink_msg_set_attitude_target_pack(uint8_t system_id, u * @param time_boot_ms [ms] Timestamp (time since system boot). * @param target_system System ID * @param target_component Component ID - * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param body_roll_rate [rad/s] Body roll rate * @param body_pitch_rate [rad/s] Body pitch rate @@ -194,7 +194,7 @@ static inline uint16_t mavlink_msg_set_attitude_target_encode_chan(uint8_t syste * @param time_boot_ms [ms] Timestamp (time since system boot). * @param target_system System ID * @param target_component Component ID - * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param body_roll_rate [rad/s] Body roll rate * @param body_pitch_rate [rad/s] Body pitch rate @@ -322,7 +322,7 @@ static inline uint8_t mavlink_msg_set_attitude_target_get_target_component(const /** * @brief Get field type_mask from set_attitude_target message * - * @return Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude + * @return Bitmap to indicate which dimensions should be ignored by the vehicle. */ static inline uint8_t mavlink_msg_set_attitude_target_get_type_mask(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_set_gps_global_origin.h b/lib/main/MAVLink/common/mavlink_msg_set_gps_global_origin.h index 7da7fbb510..62a2f448f8 100755 --- a/lib/main/MAVLink/common/mavlink_msg_set_gps_global_origin.h +++ b/lib/main/MAVLink/common/mavlink_msg_set_gps_global_origin.h @@ -3,17 +3,18 @@ #define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN 48 - +MAVPACKED( typedef struct __mavlink_set_gps_global_origin_t { int32_t latitude; /*< [degE7] Latitude (WGS84)*/ int32_t longitude; /*< [degE7] Longitude (WGS84)*/ int32_t altitude; /*< [mm] Altitude (MSL). Positive for up.*/ uint8_t target_system; /*< System ID*/ -} mavlink_set_gps_global_origin_t; + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ +}) mavlink_set_gps_global_origin_t; -#define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN 13 +#define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN 21 #define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_MIN_LEN 13 -#define MAVLINK_MSG_ID_48_LEN 13 +#define MAVLINK_MSG_ID_48_LEN 21 #define MAVLINK_MSG_ID_48_MIN_LEN 13 #define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC 41 @@ -25,21 +26,23 @@ typedef struct __mavlink_set_gps_global_origin_t { #define MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN { \ 48, \ "SET_GPS_GLOBAL_ORIGIN", \ - 4, \ + 5, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_set_gps_global_origin_t, target_system) }, \ { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_set_gps_global_origin_t, latitude) }, \ { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_set_gps_global_origin_t, longitude) }, \ { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_set_gps_global_origin_t, altitude) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 13, offsetof(mavlink_set_gps_global_origin_t, time_usec) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN { \ "SET_GPS_GLOBAL_ORIGIN", \ - 4, \ + 5, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_set_gps_global_origin_t, target_system) }, \ { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_set_gps_global_origin_t, latitude) }, \ { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_set_gps_global_origin_t, longitude) }, \ { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_set_gps_global_origin_t, altitude) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 13, offsetof(mavlink_set_gps_global_origin_t, time_usec) }, \ } \ } #endif @@ -54,10 +57,11 @@ typedef struct __mavlink_set_gps_global_origin_t { * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_gps_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude) + uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN]; @@ -65,6 +69,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack(uint8_t system_id, _mav_put_int32_t(buf, 4, longitude); _mav_put_int32_t(buf, 8, altitude); _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint64_t(buf, 13, time_usec); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); #else @@ -73,6 +78,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack(uint8_t system_id, packet.longitude = longitude; packet.altitude = altitude; packet.target_system = target_system; + packet.time_usec = time_usec; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); #endif @@ -91,11 +97,12 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack(uint8_t system_id, * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_gps_global_origin_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,int32_t latitude,int32_t longitude,int32_t altitude) + uint8_t target_system,int32_t latitude,int32_t longitude,int32_t altitude,uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN]; @@ -103,6 +110,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack_chan(uint8_t syste _mav_put_int32_t(buf, 4, longitude); _mav_put_int32_t(buf, 8, altitude); _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint64_t(buf, 13, time_usec); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); #else @@ -111,6 +119,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack_chan(uint8_t syste packet.longitude = longitude; packet.altitude = altitude; packet.target_system = target_system; + packet.time_usec = time_usec; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); #endif @@ -129,7 +138,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack_chan(uint8_t syste */ static inline uint16_t mavlink_msg_set_gps_global_origin_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_gps_global_origin_t* set_gps_global_origin) { - return mavlink_msg_set_gps_global_origin_pack(system_id, component_id, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude); + return mavlink_msg_set_gps_global_origin_pack(system_id, component_id, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude, set_gps_global_origin->time_usec); } /** @@ -143,7 +152,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_encode(uint8_t system_i */ static inline uint16_t mavlink_msg_set_gps_global_origin_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_gps_global_origin_t* set_gps_global_origin) { - return mavlink_msg_set_gps_global_origin_pack_chan(system_id, component_id, chan, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude); + return mavlink_msg_set_gps_global_origin_pack_chan(system_id, component_id, chan, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude, set_gps_global_origin->time_usec); } /** @@ -154,10 +163,11 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_encode_chan(uint8_t sys * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude) +static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN]; @@ -165,6 +175,7 @@ static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan _mav_put_int32_t(buf, 4, longitude); _mav_put_int32_t(buf, 8, altitude); _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint64_t(buf, 13, time_usec); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); #else @@ -173,6 +184,7 @@ static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan packet.longitude = longitude; packet.altitude = altitude; packet.target_system = target_system; + packet.time_usec = time_usec; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)&packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); #endif @@ -186,7 +198,7 @@ static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan static inline void mavlink_msg_set_gps_global_origin_send_struct(mavlink_channel_t chan, const mavlink_set_gps_global_origin_t* set_gps_global_origin) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_set_gps_global_origin_send(chan, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude); + mavlink_msg_set_gps_global_origin_send(chan, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude, set_gps_global_origin->time_usec); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)set_gps_global_origin, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); #endif @@ -200,7 +212,7 @@ static inline void mavlink_msg_set_gps_global_origin_send_struct(mavlink_channel is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_set_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude) +static inline void mavlink_msg_set_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +220,7 @@ static inline void mavlink_msg_set_gps_global_origin_send_buf(mavlink_message_t _mav_put_int32_t(buf, 4, longitude); _mav_put_int32_t(buf, 8, altitude); _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint64_t(buf, 13, time_usec); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); #else @@ -216,6 +229,7 @@ static inline void mavlink_msg_set_gps_global_origin_send_buf(mavlink_message_t packet->longitude = longitude; packet->altitude = altitude; packet->target_system = target_system; + packet->time_usec = time_usec; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); #endif @@ -267,6 +281,16 @@ static inline int32_t mavlink_msg_set_gps_global_origin_get_altitude(const mavli return _MAV_RETURN_int32_t(msg, 8); } +/** + * @brief Get field time_usec from set_gps_global_origin message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_set_gps_global_origin_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 13); +} + /** * @brief Decode a set_gps_global_origin message into a struct * @@ -280,6 +304,7 @@ static inline void mavlink_msg_set_gps_global_origin_decode(const mavlink_messag set_gps_global_origin->longitude = mavlink_msg_set_gps_global_origin_get_longitude(msg); set_gps_global_origin->altitude = mavlink_msg_set_gps_global_origin_get_altitude(msg); set_gps_global_origin->target_system = mavlink_msg_set_gps_global_origin_get_target_system(msg); + set_gps_global_origin->time_usec = mavlink_msg_set_gps_global_origin_get_time_usec(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN? msg->len : MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN; memset(set_gps_global_origin, 0, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_set_home_position.h b/lib/main/MAVLink/common/mavlink_msg_set_home_position.h index 7ddcae5a09..ea7d2d0f26 100755 --- a/lib/main/MAVLink/common/mavlink_msg_set_home_position.h +++ b/lib/main/MAVLink/common/mavlink_msg_set_home_position.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_SET_HOME_POSITION 243 - +MAVPACKED( typedef struct __mavlink_set_home_position_t { int32_t latitude; /*< [degE7] Latitude (WGS84)*/ int32_t longitude; /*< [degE7] Longitude (WGS84)*/ @@ -16,11 +16,12 @@ typedef struct __mavlink_set_home_position_t { float approach_y; /*< [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone.*/ float approach_z; /*< [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone.*/ uint8_t target_system; /*< System ID.*/ -} mavlink_set_home_position_t; + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ +}) mavlink_set_home_position_t; -#define MAVLINK_MSG_ID_SET_HOME_POSITION_LEN 53 +#define MAVLINK_MSG_ID_SET_HOME_POSITION_LEN 61 #define MAVLINK_MSG_ID_SET_HOME_POSITION_MIN_LEN 53 -#define MAVLINK_MSG_ID_243_LEN 53 +#define MAVLINK_MSG_ID_243_LEN 61 #define MAVLINK_MSG_ID_243_MIN_LEN 53 #define MAVLINK_MSG_ID_SET_HOME_POSITION_CRC 85 @@ -32,7 +33,7 @@ typedef struct __mavlink_set_home_position_t { #define MAVLINK_MESSAGE_INFO_SET_HOME_POSITION { \ 243, \ "SET_HOME_POSITION", \ - 11, \ + 12, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_set_home_position_t, target_system) }, \ { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_set_home_position_t, latitude) }, \ { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_set_home_position_t, longitude) }, \ @@ -44,12 +45,13 @@ typedef struct __mavlink_set_home_position_t { { "approach_x", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_set_home_position_t, approach_x) }, \ { "approach_y", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_set_home_position_t, approach_y) }, \ { "approach_z", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_set_home_position_t, approach_z) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 53, offsetof(mavlink_set_home_position_t, time_usec) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SET_HOME_POSITION { \ "SET_HOME_POSITION", \ - 11, \ + 12, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_set_home_position_t, target_system) }, \ { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_set_home_position_t, latitude) }, \ { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_set_home_position_t, longitude) }, \ @@ -61,6 +63,7 @@ typedef struct __mavlink_set_home_position_t { { "approach_x", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_set_home_position_t, approach_x) }, \ { "approach_y", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_set_home_position_t, approach_y) }, \ { "approach_z", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_set_home_position_t, approach_z) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 53, offsetof(mavlink_set_home_position_t, time_usec) }, \ } \ } #endif @@ -82,10 +85,11 @@ typedef struct __mavlink_set_home_position_t { * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_z [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_home_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z) + uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SET_HOME_POSITION_LEN]; @@ -99,6 +103,7 @@ static inline uint16_t mavlink_msg_set_home_position_pack(uint8_t system_id, uin _mav_put_float(buf, 44, approach_y); _mav_put_float(buf, 48, approach_z); _mav_put_uint8_t(buf, 52, target_system); + _mav_put_uint64_t(buf, 53, time_usec); _mav_put_float_array(buf, 24, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN); #else @@ -113,6 +118,7 @@ static inline uint16_t mavlink_msg_set_home_position_pack(uint8_t system_id, uin packet.approach_y = approach_y; packet.approach_z = approach_z; packet.target_system = target_system; + packet.time_usec = time_usec; mav_array_memcpy(packet.q, q, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN); #endif @@ -138,11 +144,12 @@ static inline uint16_t mavlink_msg_set_home_position_pack(uint8_t system_id, uin * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_z [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_home_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,int32_t latitude,int32_t longitude,int32_t altitude,float x,float y,float z,const float *q,float approach_x,float approach_y,float approach_z) + uint8_t target_system,int32_t latitude,int32_t longitude,int32_t altitude,float x,float y,float z,const float *q,float approach_x,float approach_y,float approach_z,uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SET_HOME_POSITION_LEN]; @@ -156,6 +163,7 @@ static inline uint16_t mavlink_msg_set_home_position_pack_chan(uint8_t system_id _mav_put_float(buf, 44, approach_y); _mav_put_float(buf, 48, approach_z); _mav_put_uint8_t(buf, 52, target_system); + _mav_put_uint64_t(buf, 53, time_usec); _mav_put_float_array(buf, 24, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN); #else @@ -170,6 +178,7 @@ static inline uint16_t mavlink_msg_set_home_position_pack_chan(uint8_t system_id packet.approach_y = approach_y; packet.approach_z = approach_z; packet.target_system = target_system; + packet.time_usec = time_usec; mav_array_memcpy(packet.q, q, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN); #endif @@ -188,7 +197,7 @@ static inline uint16_t mavlink_msg_set_home_position_pack_chan(uint8_t system_id */ static inline uint16_t mavlink_msg_set_home_position_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_home_position_t* set_home_position) { - return mavlink_msg_set_home_position_pack(system_id, component_id, msg, set_home_position->target_system, set_home_position->latitude, set_home_position->longitude, set_home_position->altitude, set_home_position->x, set_home_position->y, set_home_position->z, set_home_position->q, set_home_position->approach_x, set_home_position->approach_y, set_home_position->approach_z); + return mavlink_msg_set_home_position_pack(system_id, component_id, msg, set_home_position->target_system, set_home_position->latitude, set_home_position->longitude, set_home_position->altitude, set_home_position->x, set_home_position->y, set_home_position->z, set_home_position->q, set_home_position->approach_x, set_home_position->approach_y, set_home_position->approach_z, set_home_position->time_usec); } /** @@ -202,7 +211,7 @@ static inline uint16_t mavlink_msg_set_home_position_encode(uint8_t system_id, u */ static inline uint16_t mavlink_msg_set_home_position_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_home_position_t* set_home_position) { - return mavlink_msg_set_home_position_pack_chan(system_id, component_id, chan, msg, set_home_position->target_system, set_home_position->latitude, set_home_position->longitude, set_home_position->altitude, set_home_position->x, set_home_position->y, set_home_position->z, set_home_position->q, set_home_position->approach_x, set_home_position->approach_y, set_home_position->approach_z); + return mavlink_msg_set_home_position_pack_chan(system_id, component_id, chan, msg, set_home_position->target_system, set_home_position->latitude, set_home_position->longitude, set_home_position->altitude, set_home_position->x, set_home_position->y, set_home_position->z, set_home_position->q, set_home_position->approach_x, set_home_position->approach_y, set_home_position->approach_z, set_home_position->time_usec); } /** @@ -220,10 +229,11 @@ static inline uint16_t mavlink_msg_set_home_position_encode_chan(uint8_t system_ * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_z [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_set_home_position_send(mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z) +static inline void mavlink_msg_set_home_position_send(mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SET_HOME_POSITION_LEN]; @@ -237,6 +247,7 @@ static inline void mavlink_msg_set_home_position_send(mavlink_channel_t chan, ui _mav_put_float(buf, 44, approach_y); _mav_put_float(buf, 48, approach_z); _mav_put_uint8_t(buf, 52, target_system); + _mav_put_uint64_t(buf, 53, time_usec); _mav_put_float_array(buf, 24, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_HOME_POSITION, buf, MAVLINK_MSG_ID_SET_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_CRC); #else @@ -251,6 +262,7 @@ static inline void mavlink_msg_set_home_position_send(mavlink_channel_t chan, ui packet.approach_y = approach_y; packet.approach_z = approach_z; packet.target_system = target_system; + packet.time_usec = time_usec; mav_array_memcpy(packet.q, q, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_HOME_POSITION, (const char *)&packet, MAVLINK_MSG_ID_SET_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_CRC); #endif @@ -264,7 +276,7 @@ static inline void mavlink_msg_set_home_position_send(mavlink_channel_t chan, ui static inline void mavlink_msg_set_home_position_send_struct(mavlink_channel_t chan, const mavlink_set_home_position_t* set_home_position) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_set_home_position_send(chan, set_home_position->target_system, set_home_position->latitude, set_home_position->longitude, set_home_position->altitude, set_home_position->x, set_home_position->y, set_home_position->z, set_home_position->q, set_home_position->approach_x, set_home_position->approach_y, set_home_position->approach_z); + mavlink_msg_set_home_position_send(chan, set_home_position->target_system, set_home_position->latitude, set_home_position->longitude, set_home_position->altitude, set_home_position->x, set_home_position->y, set_home_position->z, set_home_position->q, set_home_position->approach_x, set_home_position->approach_y, set_home_position->approach_z, set_home_position->time_usec); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_HOME_POSITION, (const char *)set_home_position, MAVLINK_MSG_ID_SET_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_CRC); #endif @@ -278,7 +290,7 @@ static inline void mavlink_msg_set_home_position_send_struct(mavlink_channel_t c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_set_home_position_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z) +static inline void mavlink_msg_set_home_position_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -292,6 +304,7 @@ static inline void mavlink_msg_set_home_position_send_buf(mavlink_message_t *msg _mav_put_float(buf, 44, approach_y); _mav_put_float(buf, 48, approach_z); _mav_put_uint8_t(buf, 52, target_system); + _mav_put_uint64_t(buf, 53, time_usec); _mav_put_float_array(buf, 24, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_HOME_POSITION, buf, MAVLINK_MSG_ID_SET_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_CRC); #else @@ -306,6 +319,7 @@ static inline void mavlink_msg_set_home_position_send_buf(mavlink_message_t *msg packet->approach_y = approach_y; packet->approach_z = approach_z; packet->target_system = target_system; + packet->time_usec = time_usec; mav_array_memcpy(packet->q, q, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_HOME_POSITION, (const char *)packet, MAVLINK_MSG_ID_SET_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_CRC); #endif @@ -427,6 +441,16 @@ static inline float mavlink_msg_set_home_position_get_approach_z(const mavlink_m return _MAV_RETURN_float(msg, 48); } +/** + * @brief Get field time_usec from set_home_position message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_set_home_position_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 53); +} + /** * @brief Decode a set_home_position message into a struct * @@ -447,6 +471,7 @@ static inline void mavlink_msg_set_home_position_decode(const mavlink_message_t* set_home_position->approach_y = mavlink_msg_set_home_position_get_approach_y(msg); set_home_position->approach_z = mavlink_msg_set_home_position_get_approach_z(msg); set_home_position->target_system = mavlink_msg_set_home_position_get_target_system(msg); + set_home_position->time_usec = mavlink_msg_set_home_position_get_time_usec(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SET_HOME_POSITION_LEN? msg->len : MAVLINK_MSG_ID_SET_HOME_POSITION_LEN; memset(set_home_position, 0, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_setup_signing.h b/lib/main/MAVLink/common/mavlink_msg_setup_signing.h new file mode 100644 index 0000000000..e9bab0cbaa --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_setup_signing.h @@ -0,0 +1,280 @@ +#pragma once +// MESSAGE SETUP_SIGNING PACKING + +#define MAVLINK_MSG_ID_SETUP_SIGNING 256 + + +typedef struct __mavlink_setup_signing_t { + uint64_t initial_timestamp; /*< initial timestamp*/ + uint8_t target_system; /*< system id of the target*/ + uint8_t target_component; /*< component ID of the target*/ + uint8_t secret_key[32]; /*< signing key*/ +} mavlink_setup_signing_t; + +#define MAVLINK_MSG_ID_SETUP_SIGNING_LEN 42 +#define MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN 42 +#define MAVLINK_MSG_ID_256_LEN 42 +#define MAVLINK_MSG_ID_256_MIN_LEN 42 + +#define MAVLINK_MSG_ID_SETUP_SIGNING_CRC 71 +#define MAVLINK_MSG_ID_256_CRC 71 + +#define MAVLINK_MSG_SETUP_SIGNING_FIELD_SECRET_KEY_LEN 32 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_SETUP_SIGNING { \ + 256, \ + "SETUP_SIGNING", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_setup_signing_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_setup_signing_t, target_component) }, \ + { "secret_key", NULL, MAVLINK_TYPE_UINT8_T, 32, 10, offsetof(mavlink_setup_signing_t, secret_key) }, \ + { "initial_timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_setup_signing_t, initial_timestamp) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_SETUP_SIGNING { \ + "SETUP_SIGNING", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_setup_signing_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_setup_signing_t, target_component) }, \ + { "secret_key", NULL, MAVLINK_TYPE_UINT8_T, 32, 10, offsetof(mavlink_setup_signing_t, secret_key) }, \ + { "initial_timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_setup_signing_t, initial_timestamp) }, \ + } \ +} +#endif + +/** + * @brief Pack a setup_signing message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system system id of the target + * @param target_component component ID of the target + * @param secret_key signing key + * @param initial_timestamp initial timestamp + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_setup_signing_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *secret_key, uint64_t initial_timestamp) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SETUP_SIGNING_LEN]; + _mav_put_uint64_t(buf, 0, initial_timestamp); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + _mav_put_uint8_t_array(buf, 10, secret_key, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETUP_SIGNING_LEN); +#else + mavlink_setup_signing_t packet; + packet.initial_timestamp = initial_timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.secret_key, secret_key, sizeof(uint8_t)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETUP_SIGNING_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SETUP_SIGNING; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); +} + +/** + * @brief Pack a setup_signing message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system system id of the target + * @param target_component component ID of the target + * @param secret_key signing key + * @param initial_timestamp initial timestamp + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_setup_signing_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const uint8_t *secret_key,uint64_t initial_timestamp) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SETUP_SIGNING_LEN]; + _mav_put_uint64_t(buf, 0, initial_timestamp); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + _mav_put_uint8_t_array(buf, 10, secret_key, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETUP_SIGNING_LEN); +#else + mavlink_setup_signing_t packet; + packet.initial_timestamp = initial_timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.secret_key, secret_key, sizeof(uint8_t)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETUP_SIGNING_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SETUP_SIGNING; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); +} + +/** + * @brief Encode a setup_signing struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param setup_signing C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_setup_signing_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_setup_signing_t* setup_signing) +{ + return mavlink_msg_setup_signing_pack(system_id, component_id, msg, setup_signing->target_system, setup_signing->target_component, setup_signing->secret_key, setup_signing->initial_timestamp); +} + +/** + * @brief Encode a setup_signing struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param setup_signing C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_setup_signing_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_setup_signing_t* setup_signing) +{ + return mavlink_msg_setup_signing_pack_chan(system_id, component_id, chan, msg, setup_signing->target_system, setup_signing->target_component, setup_signing->secret_key, setup_signing->initial_timestamp); +} + +/** + * @brief Send a setup_signing message + * @param chan MAVLink channel to send the message + * + * @param target_system system id of the target + * @param target_component component ID of the target + * @param secret_key signing key + * @param initial_timestamp initial timestamp + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_setup_signing_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *secret_key, uint64_t initial_timestamp) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SETUP_SIGNING_LEN]; + _mav_put_uint64_t(buf, 0, initial_timestamp); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + _mav_put_uint8_t_array(buf, 10, secret_key, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETUP_SIGNING, buf, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); +#else + mavlink_setup_signing_t packet; + packet.initial_timestamp = initial_timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.secret_key, secret_key, sizeof(uint8_t)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETUP_SIGNING, (const char *)&packet, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); +#endif +} + +/** + * @brief Send a setup_signing message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_setup_signing_send_struct(mavlink_channel_t chan, const mavlink_setup_signing_t* setup_signing) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_setup_signing_send(chan, setup_signing->target_system, setup_signing->target_component, setup_signing->secret_key, setup_signing->initial_timestamp); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETUP_SIGNING, (const char *)setup_signing, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); +#endif +} + +#if MAVLINK_MSG_ID_SETUP_SIGNING_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_setup_signing_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *secret_key, uint64_t initial_timestamp) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, initial_timestamp); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + _mav_put_uint8_t_array(buf, 10, secret_key, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETUP_SIGNING, buf, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); +#else + mavlink_setup_signing_t *packet = (mavlink_setup_signing_t *)msgbuf; + packet->initial_timestamp = initial_timestamp; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->secret_key, secret_key, sizeof(uint8_t)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETUP_SIGNING, (const char *)packet, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); +#endif +} +#endif + +#endif + +// MESSAGE SETUP_SIGNING UNPACKING + + +/** + * @brief Get field target_system from setup_signing message + * + * @return system id of the target + */ +static inline uint8_t mavlink_msg_setup_signing_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field target_component from setup_signing message + * + * @return component ID of the target + */ +static inline uint8_t mavlink_msg_setup_signing_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 9); +} + +/** + * @brief Get field secret_key from setup_signing message + * + * @return signing key + */ +static inline uint16_t mavlink_msg_setup_signing_get_secret_key(const mavlink_message_t* msg, uint8_t *secret_key) +{ + return _MAV_RETURN_uint8_t_array(msg, secret_key, 32, 10); +} + +/** + * @brief Get field initial_timestamp from setup_signing message + * + * @return initial timestamp + */ +static inline uint64_t mavlink_msg_setup_signing_get_initial_timestamp(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Decode a setup_signing message into a struct + * + * @param msg The message to decode + * @param setup_signing C-struct to decode the message contents into + */ +static inline void mavlink_msg_setup_signing_decode(const mavlink_message_t* msg, mavlink_setup_signing_t* setup_signing) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + setup_signing->initial_timestamp = mavlink_msg_setup_signing_get_initial_timestamp(msg); + setup_signing->target_system = mavlink_msg_setup_signing_get_target_system(msg); + setup_signing->target_component = mavlink_msg_setup_signing_get_target_component(msg); + mavlink_msg_setup_signing_get_secret_key(msg, setup_signing->secret_key); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_SETUP_SIGNING_LEN? msg->len : MAVLINK_MSG_ID_SETUP_SIGNING_LEN; + memset(setup_signing, 0, MAVLINK_MSG_ID_SETUP_SIGNING_LEN); + memcpy(setup_signing, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_smart_battery_info.h b/lib/main/MAVLink/common/mavlink_msg_smart_battery_info.h new file mode 100644 index 0000000000..174c197f31 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_smart_battery_info.h @@ -0,0 +1,481 @@ +#pragma once +// MESSAGE SMART_BATTERY_INFO PACKING + +#define MAVLINK_MSG_ID_SMART_BATTERY_INFO 370 + + +typedef struct __mavlink_smart_battery_info_t { + int32_t capacity_full_specification; /*< [mAh] Capacity when full according to manufacturer, -1: field not provided.*/ + int32_t capacity_full; /*< [mAh] Capacity when full (accounting for battery degradation), -1: field not provided.*/ + uint16_t cycle_count; /*< Charge/discharge cycle count. UINT16_MAX: field not provided.*/ + uint16_t weight; /*< [g] Battery weight. 0: field not provided.*/ + uint16_t discharge_minimum_voltage; /*< [mV] Minimum per-cell voltage when discharging. If not supplied set to UINT16_MAX value.*/ + uint16_t charging_minimum_voltage; /*< [mV] Minimum per-cell voltage when charging. If not supplied set to UINT16_MAX value.*/ + uint16_t resting_minimum_voltage; /*< [mV] Minimum per-cell voltage when resting. If not supplied set to UINT16_MAX value.*/ + uint8_t id; /*< Battery ID*/ + uint8_t battery_function; /*< Function of the battery*/ + uint8_t type; /*< Type (chemistry) of the battery*/ + char serial_number[16]; /*< Serial number in ASCII characters, 0 terminated. All 0: field not provided.*/ + char device_name[50]; /*< Static device name. Encode as manufacturer and product names separated using an underscore.*/ +} mavlink_smart_battery_info_t; + +#define MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN 87 +#define MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN 87 +#define MAVLINK_MSG_ID_370_LEN 87 +#define MAVLINK_MSG_ID_370_MIN_LEN 87 + +#define MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC 75 +#define MAVLINK_MSG_ID_370_CRC 75 + +#define MAVLINK_MSG_SMART_BATTERY_INFO_FIELD_SERIAL_NUMBER_LEN 16 +#define MAVLINK_MSG_SMART_BATTERY_INFO_FIELD_DEVICE_NAME_LEN 50 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_SMART_BATTERY_INFO { \ + 370, \ + "SMART_BATTERY_INFO", \ + 12, \ + { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_smart_battery_info_t, id) }, \ + { "battery_function", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_smart_battery_info_t, battery_function) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_smart_battery_info_t, type) }, \ + { "capacity_full_specification", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_smart_battery_info_t, capacity_full_specification) }, \ + { "capacity_full", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_smart_battery_info_t, capacity_full) }, \ + { "cycle_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_smart_battery_info_t, cycle_count) }, \ + { "serial_number", NULL, MAVLINK_TYPE_CHAR, 16, 21, offsetof(mavlink_smart_battery_info_t, serial_number) }, \ + { "device_name", NULL, MAVLINK_TYPE_CHAR, 50, 37, offsetof(mavlink_smart_battery_info_t, device_name) }, \ + { "weight", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_smart_battery_info_t, weight) }, \ + { "discharge_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_smart_battery_info_t, discharge_minimum_voltage) }, \ + { "charging_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_smart_battery_info_t, charging_minimum_voltage) }, \ + { "resting_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_smart_battery_info_t, resting_minimum_voltage) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_SMART_BATTERY_INFO { \ + "SMART_BATTERY_INFO", \ + 12, \ + { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_smart_battery_info_t, id) }, \ + { "battery_function", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_smart_battery_info_t, battery_function) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_smart_battery_info_t, type) }, \ + { "capacity_full_specification", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_smart_battery_info_t, capacity_full_specification) }, \ + { "capacity_full", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_smart_battery_info_t, capacity_full) }, \ + { "cycle_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_smart_battery_info_t, cycle_count) }, \ + { "serial_number", NULL, MAVLINK_TYPE_CHAR, 16, 21, offsetof(mavlink_smart_battery_info_t, serial_number) }, \ + { "device_name", NULL, MAVLINK_TYPE_CHAR, 50, 37, offsetof(mavlink_smart_battery_info_t, device_name) }, \ + { "weight", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_smart_battery_info_t, weight) }, \ + { "discharge_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_smart_battery_info_t, discharge_minimum_voltage) }, \ + { "charging_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_smart_battery_info_t, charging_minimum_voltage) }, \ + { "resting_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_smart_battery_info_t, resting_minimum_voltage) }, \ + } \ +} +#endif + +/** + * @brief Pack a smart_battery_info message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param id Battery ID + * @param battery_function Function of the battery + * @param type Type (chemistry) of the battery + * @param capacity_full_specification [mAh] Capacity when full according to manufacturer, -1: field not provided. + * @param capacity_full [mAh] Capacity when full (accounting for battery degradation), -1: field not provided. + * @param cycle_count Charge/discharge cycle count. UINT16_MAX: field not provided. + * @param serial_number Serial number in ASCII characters, 0 terminated. All 0: field not provided. + * @param device_name Static device name. Encode as manufacturer and product names separated using an underscore. + * @param weight [g] Battery weight. 0: field not provided. + * @param discharge_minimum_voltage [mV] Minimum per-cell voltage when discharging. If not supplied set to UINT16_MAX value. + * @param charging_minimum_voltage [mV] Minimum per-cell voltage when charging. If not supplied set to UINT16_MAX value. + * @param resting_minimum_voltage [mV] Minimum per-cell voltage when resting. If not supplied set to UINT16_MAX value. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_smart_battery_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t id, uint8_t battery_function, uint8_t type, int32_t capacity_full_specification, int32_t capacity_full, uint16_t cycle_count, const char *serial_number, const char *device_name, uint16_t weight, uint16_t discharge_minimum_voltage, uint16_t charging_minimum_voltage, uint16_t resting_minimum_voltage) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN]; + _mav_put_int32_t(buf, 0, capacity_full_specification); + _mav_put_int32_t(buf, 4, capacity_full); + _mav_put_uint16_t(buf, 8, cycle_count); + _mav_put_uint16_t(buf, 10, weight); + _mav_put_uint16_t(buf, 12, discharge_minimum_voltage); + _mav_put_uint16_t(buf, 14, charging_minimum_voltage); + _mav_put_uint16_t(buf, 16, resting_minimum_voltage); + _mav_put_uint8_t(buf, 18, id); + _mav_put_uint8_t(buf, 19, battery_function); + _mav_put_uint8_t(buf, 20, type); + _mav_put_char_array(buf, 21, serial_number, 16); + _mav_put_char_array(buf, 37, device_name, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); +#else + mavlink_smart_battery_info_t packet; + packet.capacity_full_specification = capacity_full_specification; + packet.capacity_full = capacity_full; + packet.cycle_count = cycle_count; + packet.weight = weight; + packet.discharge_minimum_voltage = discharge_minimum_voltage; + packet.charging_minimum_voltage = charging_minimum_voltage; + packet.resting_minimum_voltage = resting_minimum_voltage; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; + mav_array_memcpy(packet.serial_number, serial_number, sizeof(char)*16); + mav_array_memcpy(packet.device_name, device_name, sizeof(char)*50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SMART_BATTERY_INFO; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); +} + +/** + * @brief Pack a smart_battery_info message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param id Battery ID + * @param battery_function Function of the battery + * @param type Type (chemistry) of the battery + * @param capacity_full_specification [mAh] Capacity when full according to manufacturer, -1: field not provided. + * @param capacity_full [mAh] Capacity when full (accounting for battery degradation), -1: field not provided. + * @param cycle_count Charge/discharge cycle count. UINT16_MAX: field not provided. + * @param serial_number Serial number in ASCII characters, 0 terminated. All 0: field not provided. + * @param device_name Static device name. Encode as manufacturer and product names separated using an underscore. + * @param weight [g] Battery weight. 0: field not provided. + * @param discharge_minimum_voltage [mV] Minimum per-cell voltage when discharging. If not supplied set to UINT16_MAX value. + * @param charging_minimum_voltage [mV] Minimum per-cell voltage when charging. If not supplied set to UINT16_MAX value. + * @param resting_minimum_voltage [mV] Minimum per-cell voltage when resting. If not supplied set to UINT16_MAX value. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_smart_battery_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t id,uint8_t battery_function,uint8_t type,int32_t capacity_full_specification,int32_t capacity_full,uint16_t cycle_count,const char *serial_number,const char *device_name,uint16_t weight,uint16_t discharge_minimum_voltage,uint16_t charging_minimum_voltage,uint16_t resting_minimum_voltage) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN]; + _mav_put_int32_t(buf, 0, capacity_full_specification); + _mav_put_int32_t(buf, 4, capacity_full); + _mav_put_uint16_t(buf, 8, cycle_count); + _mav_put_uint16_t(buf, 10, weight); + _mav_put_uint16_t(buf, 12, discharge_minimum_voltage); + _mav_put_uint16_t(buf, 14, charging_minimum_voltage); + _mav_put_uint16_t(buf, 16, resting_minimum_voltage); + _mav_put_uint8_t(buf, 18, id); + _mav_put_uint8_t(buf, 19, battery_function); + _mav_put_uint8_t(buf, 20, type); + _mav_put_char_array(buf, 21, serial_number, 16); + _mav_put_char_array(buf, 37, device_name, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); +#else + mavlink_smart_battery_info_t packet; + packet.capacity_full_specification = capacity_full_specification; + packet.capacity_full = capacity_full; + packet.cycle_count = cycle_count; + packet.weight = weight; + packet.discharge_minimum_voltage = discharge_minimum_voltage; + packet.charging_minimum_voltage = charging_minimum_voltage; + packet.resting_minimum_voltage = resting_minimum_voltage; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; + mav_array_memcpy(packet.serial_number, serial_number, sizeof(char)*16); + mav_array_memcpy(packet.device_name, device_name, sizeof(char)*50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SMART_BATTERY_INFO; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); +} + +/** + * @brief Encode a smart_battery_info struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param smart_battery_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_smart_battery_info_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_smart_battery_info_t* smart_battery_info) +{ + return mavlink_msg_smart_battery_info_pack(system_id, component_id, msg, smart_battery_info->id, smart_battery_info->battery_function, smart_battery_info->type, smart_battery_info->capacity_full_specification, smart_battery_info->capacity_full, smart_battery_info->cycle_count, smart_battery_info->serial_number, smart_battery_info->device_name, smart_battery_info->weight, smart_battery_info->discharge_minimum_voltage, smart_battery_info->charging_minimum_voltage, smart_battery_info->resting_minimum_voltage); +} + +/** + * @brief Encode a smart_battery_info struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param smart_battery_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_smart_battery_info_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_smart_battery_info_t* smart_battery_info) +{ + return mavlink_msg_smart_battery_info_pack_chan(system_id, component_id, chan, msg, smart_battery_info->id, smart_battery_info->battery_function, smart_battery_info->type, smart_battery_info->capacity_full_specification, smart_battery_info->capacity_full, smart_battery_info->cycle_count, smart_battery_info->serial_number, smart_battery_info->device_name, smart_battery_info->weight, smart_battery_info->discharge_minimum_voltage, smart_battery_info->charging_minimum_voltage, smart_battery_info->resting_minimum_voltage); +} + +/** + * @brief Send a smart_battery_info message + * @param chan MAVLink channel to send the message + * + * @param id Battery ID + * @param battery_function Function of the battery + * @param type Type (chemistry) of the battery + * @param capacity_full_specification [mAh] Capacity when full according to manufacturer, -1: field not provided. + * @param capacity_full [mAh] Capacity when full (accounting for battery degradation), -1: field not provided. + * @param cycle_count Charge/discharge cycle count. UINT16_MAX: field not provided. + * @param serial_number Serial number in ASCII characters, 0 terminated. All 0: field not provided. + * @param device_name Static device name. Encode as manufacturer and product names separated using an underscore. + * @param weight [g] Battery weight. 0: field not provided. + * @param discharge_minimum_voltage [mV] Minimum per-cell voltage when discharging. If not supplied set to UINT16_MAX value. + * @param charging_minimum_voltage [mV] Minimum per-cell voltage when charging. If not supplied set to UINT16_MAX value. + * @param resting_minimum_voltage [mV] Minimum per-cell voltage when resting. If not supplied set to UINT16_MAX value. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_smart_battery_info_send(mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int32_t capacity_full_specification, int32_t capacity_full, uint16_t cycle_count, const char *serial_number, const char *device_name, uint16_t weight, uint16_t discharge_minimum_voltage, uint16_t charging_minimum_voltage, uint16_t resting_minimum_voltage) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN]; + _mav_put_int32_t(buf, 0, capacity_full_specification); + _mav_put_int32_t(buf, 4, capacity_full); + _mav_put_uint16_t(buf, 8, cycle_count); + _mav_put_uint16_t(buf, 10, weight); + _mav_put_uint16_t(buf, 12, discharge_minimum_voltage); + _mav_put_uint16_t(buf, 14, charging_minimum_voltage); + _mav_put_uint16_t(buf, 16, resting_minimum_voltage); + _mav_put_uint8_t(buf, 18, id); + _mav_put_uint8_t(buf, 19, battery_function); + _mav_put_uint8_t(buf, 20, type); + _mav_put_char_array(buf, 21, serial_number, 16); + _mav_put_char_array(buf, 37, device_name, 50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMART_BATTERY_INFO, buf, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); +#else + mavlink_smart_battery_info_t packet; + packet.capacity_full_specification = capacity_full_specification; + packet.capacity_full = capacity_full; + packet.cycle_count = cycle_count; + packet.weight = weight; + packet.discharge_minimum_voltage = discharge_minimum_voltage; + packet.charging_minimum_voltage = charging_minimum_voltage; + packet.resting_minimum_voltage = resting_minimum_voltage; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; + mav_array_memcpy(packet.serial_number, serial_number, sizeof(char)*16); + mav_array_memcpy(packet.device_name, device_name, sizeof(char)*50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMART_BATTERY_INFO, (const char *)&packet, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); +#endif +} + +/** + * @brief Send a smart_battery_info message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_smart_battery_info_send_struct(mavlink_channel_t chan, const mavlink_smart_battery_info_t* smart_battery_info) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_smart_battery_info_send(chan, smart_battery_info->id, smart_battery_info->battery_function, smart_battery_info->type, smart_battery_info->capacity_full_specification, smart_battery_info->capacity_full, smart_battery_info->cycle_count, smart_battery_info->serial_number, smart_battery_info->device_name, smart_battery_info->weight, smart_battery_info->discharge_minimum_voltage, smart_battery_info->charging_minimum_voltage, smart_battery_info->resting_minimum_voltage); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMART_BATTERY_INFO, (const char *)smart_battery_info, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); +#endif +} + +#if MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_smart_battery_info_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int32_t capacity_full_specification, int32_t capacity_full, uint16_t cycle_count, const char *serial_number, const char *device_name, uint16_t weight, uint16_t discharge_minimum_voltage, uint16_t charging_minimum_voltage, uint16_t resting_minimum_voltage) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, capacity_full_specification); + _mav_put_int32_t(buf, 4, capacity_full); + _mav_put_uint16_t(buf, 8, cycle_count); + _mav_put_uint16_t(buf, 10, weight); + _mav_put_uint16_t(buf, 12, discharge_minimum_voltage); + _mav_put_uint16_t(buf, 14, charging_minimum_voltage); + _mav_put_uint16_t(buf, 16, resting_minimum_voltage); + _mav_put_uint8_t(buf, 18, id); + _mav_put_uint8_t(buf, 19, battery_function); + _mav_put_uint8_t(buf, 20, type); + _mav_put_char_array(buf, 21, serial_number, 16); + _mav_put_char_array(buf, 37, device_name, 50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMART_BATTERY_INFO, buf, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); +#else + mavlink_smart_battery_info_t *packet = (mavlink_smart_battery_info_t *)msgbuf; + packet->capacity_full_specification = capacity_full_specification; + packet->capacity_full = capacity_full; + packet->cycle_count = cycle_count; + packet->weight = weight; + packet->discharge_minimum_voltage = discharge_minimum_voltage; + packet->charging_minimum_voltage = charging_minimum_voltage; + packet->resting_minimum_voltage = resting_minimum_voltage; + packet->id = id; + packet->battery_function = battery_function; + packet->type = type; + mav_array_memcpy(packet->serial_number, serial_number, sizeof(char)*16); + mav_array_memcpy(packet->device_name, device_name, sizeof(char)*50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMART_BATTERY_INFO, (const char *)packet, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); +#endif +} +#endif + +#endif + +// MESSAGE SMART_BATTERY_INFO UNPACKING + + +/** + * @brief Get field id from smart_battery_info message + * + * @return Battery ID + */ +static inline uint8_t mavlink_msg_smart_battery_info_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 18); +} + +/** + * @brief Get field battery_function from smart_battery_info message + * + * @return Function of the battery + */ +static inline uint8_t mavlink_msg_smart_battery_info_get_battery_function(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 19); +} + +/** + * @brief Get field type from smart_battery_info message + * + * @return Type (chemistry) of the battery + */ +static inline uint8_t mavlink_msg_smart_battery_info_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field capacity_full_specification from smart_battery_info message + * + * @return [mAh] Capacity when full according to manufacturer, -1: field not provided. + */ +static inline int32_t mavlink_msg_smart_battery_info_get_capacity_full_specification(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field capacity_full from smart_battery_info message + * + * @return [mAh] Capacity when full (accounting for battery degradation), -1: field not provided. + */ +static inline int32_t mavlink_msg_smart_battery_info_get_capacity_full(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field cycle_count from smart_battery_info message + * + * @return Charge/discharge cycle count. UINT16_MAX: field not provided. + */ +static inline uint16_t mavlink_msg_smart_battery_info_get_cycle_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field serial_number from smart_battery_info message + * + * @return Serial number in ASCII characters, 0 terminated. All 0: field not provided. + */ +static inline uint16_t mavlink_msg_smart_battery_info_get_serial_number(const mavlink_message_t* msg, char *serial_number) +{ + return _MAV_RETURN_char_array(msg, serial_number, 16, 21); +} + +/** + * @brief Get field device_name from smart_battery_info message + * + * @return Static device name. Encode as manufacturer and product names separated using an underscore. + */ +static inline uint16_t mavlink_msg_smart_battery_info_get_device_name(const mavlink_message_t* msg, char *device_name) +{ + return _MAV_RETURN_char_array(msg, device_name, 50, 37); +} + +/** + * @brief Get field weight from smart_battery_info message + * + * @return [g] Battery weight. 0: field not provided. + */ +static inline uint16_t mavlink_msg_smart_battery_info_get_weight(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field discharge_minimum_voltage from smart_battery_info message + * + * @return [mV] Minimum per-cell voltage when discharging. If not supplied set to UINT16_MAX value. + */ +static inline uint16_t mavlink_msg_smart_battery_info_get_discharge_minimum_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field charging_minimum_voltage from smart_battery_info message + * + * @return [mV] Minimum per-cell voltage when charging. If not supplied set to UINT16_MAX value. + */ +static inline uint16_t mavlink_msg_smart_battery_info_get_charging_minimum_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Get field resting_minimum_voltage from smart_battery_info message + * + * @return [mV] Minimum per-cell voltage when resting. If not supplied set to UINT16_MAX value. + */ +static inline uint16_t mavlink_msg_smart_battery_info_get_resting_minimum_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Decode a smart_battery_info message into a struct + * + * @param msg The message to decode + * @param smart_battery_info C-struct to decode the message contents into + */ +static inline void mavlink_msg_smart_battery_info_decode(const mavlink_message_t* msg, mavlink_smart_battery_info_t* smart_battery_info) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + smart_battery_info->capacity_full_specification = mavlink_msg_smart_battery_info_get_capacity_full_specification(msg); + smart_battery_info->capacity_full = mavlink_msg_smart_battery_info_get_capacity_full(msg); + smart_battery_info->cycle_count = mavlink_msg_smart_battery_info_get_cycle_count(msg); + smart_battery_info->weight = mavlink_msg_smart_battery_info_get_weight(msg); + smart_battery_info->discharge_minimum_voltage = mavlink_msg_smart_battery_info_get_discharge_minimum_voltage(msg); + smart_battery_info->charging_minimum_voltage = mavlink_msg_smart_battery_info_get_charging_minimum_voltage(msg); + smart_battery_info->resting_minimum_voltage = mavlink_msg_smart_battery_info_get_resting_minimum_voltage(msg); + smart_battery_info->id = mavlink_msg_smart_battery_info_get_id(msg); + smart_battery_info->battery_function = mavlink_msg_smart_battery_info_get_battery_function(msg); + smart_battery_info->type = mavlink_msg_smart_battery_info_get_type(msg); + mavlink_msg_smart_battery_info_get_serial_number(msg, smart_battery_info->serial_number); + mavlink_msg_smart_battery_info_get_device_name(msg, smart_battery_info->device_name); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN? msg->len : MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN; + memset(smart_battery_info, 0, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); + memcpy(smart_battery_info, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_statustext.h b/lib/main/MAVLink/common/mavlink_msg_statustext.h index 2193fac839..28e0c0b0fd 100755 --- a/lib/main/MAVLink/common/mavlink_msg_statustext.h +++ b/lib/main/MAVLink/common/mavlink_msg_statustext.h @@ -3,15 +3,17 @@ #define MAVLINK_MSG_ID_STATUSTEXT 253 - +MAVPACKED( typedef struct __mavlink_statustext_t { uint8_t severity; /*< Severity of status. Relies on the definitions within RFC-5424.*/ char text[50]; /*< Status text message, without null termination character*/ -} mavlink_statustext_t; + uint16_t id; /*< Unique (opaque) identifier for this statustext message. May be used to reassemble a logical long-statustext message from a sequence of chunks. A value of zero indicates this is the only chunk in the sequence and the message can be emitted immediately.*/ + uint8_t chunk_seq; /*< This chunk's sequence number; indexing is from zero. Any null character in the text field is taken to mean this was the last chunk.*/ +}) mavlink_statustext_t; -#define MAVLINK_MSG_ID_STATUSTEXT_LEN 51 +#define MAVLINK_MSG_ID_STATUSTEXT_LEN 54 #define MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN 51 -#define MAVLINK_MSG_ID_253_LEN 51 +#define MAVLINK_MSG_ID_253_LEN 54 #define MAVLINK_MSG_ID_253_MIN_LEN 51 #define MAVLINK_MSG_ID_STATUSTEXT_CRC 83 @@ -23,17 +25,21 @@ typedef struct __mavlink_statustext_t { #define MAVLINK_MESSAGE_INFO_STATUSTEXT { \ 253, \ "STATUSTEXT", \ - 2, \ + 4, \ { { "severity", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_statustext_t, severity) }, \ { "text", NULL, MAVLINK_TYPE_CHAR, 50, 1, offsetof(mavlink_statustext_t, text) }, \ + { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 51, offsetof(mavlink_statustext_t, id) }, \ + { "chunk_seq", NULL, MAVLINK_TYPE_UINT8_T, 0, 53, offsetof(mavlink_statustext_t, chunk_seq) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_STATUSTEXT { \ "STATUSTEXT", \ - 2, \ + 4, \ { { "severity", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_statustext_t, severity) }, \ { "text", NULL, MAVLINK_TYPE_CHAR, 50, 1, offsetof(mavlink_statustext_t, text) }, \ + { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 51, offsetof(mavlink_statustext_t, id) }, \ + { "chunk_seq", NULL, MAVLINK_TYPE_UINT8_T, 0, 53, offsetof(mavlink_statustext_t, chunk_seq) }, \ } \ } #endif @@ -46,19 +52,25 @@ typedef struct __mavlink_statustext_t { * * @param severity Severity of status. Relies on the definitions within RFC-5424. * @param text Status text message, without null termination character + * @param id Unique (opaque) identifier for this statustext message. May be used to reassemble a logical long-statustext message from a sequence of chunks. A value of zero indicates this is the only chunk in the sequence and the message can be emitted immediately. + * @param chunk_seq This chunk's sequence number; indexing is from zero. Any null character in the text field is taken to mean this was the last chunk. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t severity, const char *text) + uint8_t severity, const char *text, uint16_t id, uint8_t chunk_seq) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; _mav_put_uint8_t(buf, 0, severity); + _mav_put_uint16_t(buf, 51, id); + _mav_put_uint8_t(buf, 53, chunk_seq); _mav_put_char_array(buf, 1, text, 50); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); #else mavlink_statustext_t packet; packet.severity = severity; + packet.id = id; + packet.chunk_seq = chunk_seq; mav_array_memcpy(packet.text, text, sizeof(char)*50); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); #endif @@ -75,20 +87,26 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co * @param msg The MAVLink message to compress the data into * @param severity Severity of status. Relies on the definitions within RFC-5424. * @param text Status text message, without null termination character + * @param id Unique (opaque) identifier for this statustext message. May be used to reassemble a logical long-statustext message from a sequence of chunks. A value of zero indicates this is the only chunk in the sequence and the message can be emitted immediately. + * @param chunk_seq This chunk's sequence number; indexing is from zero. Any null character in the text field is taken to mean this was the last chunk. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t severity,const char *text) + uint8_t severity,const char *text,uint16_t id,uint8_t chunk_seq) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; _mav_put_uint8_t(buf, 0, severity); + _mav_put_uint16_t(buf, 51, id); + _mav_put_uint8_t(buf, 53, chunk_seq); _mav_put_char_array(buf, 1, text, 50); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); #else mavlink_statustext_t packet; packet.severity = severity; + packet.id = id; + packet.chunk_seq = chunk_seq; mav_array_memcpy(packet.text, text, sizeof(char)*50); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); #endif @@ -107,7 +125,7 @@ static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_statustext_t* statustext) { - return mavlink_msg_statustext_pack(system_id, component_id, msg, statustext->severity, statustext->text); + return mavlink_msg_statustext_pack(system_id, component_id, msg, statustext->severity, statustext->text, statustext->id, statustext->chunk_seq); } /** @@ -121,7 +139,7 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_statustext_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_statustext_t* statustext) { - return mavlink_msg_statustext_pack_chan(system_id, component_id, chan, msg, statustext->severity, statustext->text); + return mavlink_msg_statustext_pack_chan(system_id, component_id, chan, msg, statustext->severity, statustext->text, statustext->id, statustext->chunk_seq); } /** @@ -130,19 +148,25 @@ static inline uint16_t mavlink_msg_statustext_encode_chan(uint8_t system_id, uin * * @param severity Severity of status. Relies on the definitions within RFC-5424. * @param text Status text message, without null termination character + * @param id Unique (opaque) identifier for this statustext message. May be used to reassemble a logical long-statustext message from a sequence of chunks. A value of zero indicates this is the only chunk in the sequence and the message can be emitted immediately. + * @param chunk_seq This chunk's sequence number; indexing is from zero. Any null character in the text field is taken to mean this was the last chunk. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char *text) +static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char *text, uint16_t id, uint8_t chunk_seq) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; _mav_put_uint8_t(buf, 0, severity); + _mav_put_uint16_t(buf, 51, id); + _mav_put_uint8_t(buf, 53, chunk_seq); _mav_put_char_array(buf, 1, text, 50); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); #else mavlink_statustext_t packet; packet.severity = severity; + packet.id = id; + packet.chunk_seq = chunk_seq; mav_array_memcpy(packet.text, text, sizeof(char)*50); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)&packet, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); #endif @@ -156,7 +180,7 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s static inline void mavlink_msg_statustext_send_struct(mavlink_channel_t chan, const mavlink_statustext_t* statustext) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_statustext_send(chan, statustext->severity, statustext->text); + mavlink_msg_statustext_send(chan, statustext->severity, statustext->text, statustext->id, statustext->chunk_seq); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)statustext, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); #endif @@ -170,16 +194,20 @@ static inline void mavlink_msg_statustext_send_struct(mavlink_channel_t chan, co is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_statustext_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t severity, const char *text) +static inline void mavlink_msg_statustext_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t severity, const char *text, uint16_t id, uint8_t chunk_seq) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint8_t(buf, 0, severity); + _mav_put_uint16_t(buf, 51, id); + _mav_put_uint8_t(buf, 53, chunk_seq); _mav_put_char_array(buf, 1, text, 50); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); #else mavlink_statustext_t *packet = (mavlink_statustext_t *)msgbuf; packet->severity = severity; + packet->id = id; + packet->chunk_seq = chunk_seq; mav_array_memcpy(packet->text, text, sizeof(char)*50); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)packet, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); #endif @@ -211,6 +239,26 @@ static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* return _MAV_RETURN_char_array(msg, text, 50, 1); } +/** + * @brief Get field id from statustext message + * + * @return Unique (opaque) identifier for this statustext message. May be used to reassemble a logical long-statustext message from a sequence of chunks. A value of zero indicates this is the only chunk in the sequence and the message can be emitted immediately. + */ +static inline uint16_t mavlink_msg_statustext_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 51); +} + +/** + * @brief Get field chunk_seq from statustext message + * + * @return This chunk's sequence number; indexing is from zero. Any null character in the text field is taken to mean this was the last chunk. + */ +static inline uint8_t mavlink_msg_statustext_get_chunk_seq(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 53); +} + /** * @brief Decode a statustext message into a struct * @@ -222,6 +270,8 @@ static inline void mavlink_msg_statustext_decode(const mavlink_message_t* msg, m #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS statustext->severity = mavlink_msg_statustext_get_severity(msg); mavlink_msg_statustext_get_text(msg, statustext->text); + statustext->id = mavlink_msg_statustext_get_id(msg); + statustext->chunk_seq = mavlink_msg_statustext_get_chunk_seq(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_STATUSTEXT_LEN? msg->len : MAVLINK_MSG_ID_STATUSTEXT_LEN; memset(statustext, 0, MAVLINK_MSG_ID_STATUSTEXT_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_storage_information.h b/lib/main/MAVLink/common/mavlink_msg_storage_information.h new file mode 100644 index 0000000000..76d8afd57b --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_storage_information.h @@ -0,0 +1,455 @@ +#pragma once +// MESSAGE STORAGE_INFORMATION PACKING + +#define MAVLINK_MSG_ID_STORAGE_INFORMATION 261 + + +typedef struct __mavlink_storage_information_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + float total_capacity; /*< [MiB] Total capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored.*/ + float used_capacity; /*< [MiB] Used capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored.*/ + float available_capacity; /*< [MiB] Available storage capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored.*/ + float read_speed; /*< [MiB/s] Read speed.*/ + float write_speed; /*< [MiB/s] Write speed.*/ + uint8_t storage_id; /*< Storage ID (1 for first, 2 for second, etc.)*/ + uint8_t storage_count; /*< Number of storage devices*/ + uint8_t status; /*< Status of storage*/ + uint8_t type; /*< Type of storage*/ + char name[32]; /*< Textual storage name to be used in UI (microSD 1, Internal Memory, etc.) This is a NULL terminated string. If it is exactly 32 characters long, add a terminating NULL. If this string is empty, the generic type is shown to the user.*/ +} mavlink_storage_information_t; + +#define MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN 60 +#define MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN 27 +#define MAVLINK_MSG_ID_261_LEN 60 +#define MAVLINK_MSG_ID_261_MIN_LEN 27 + +#define MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC 179 +#define MAVLINK_MSG_ID_261_CRC 179 + +#define MAVLINK_MSG_STORAGE_INFORMATION_FIELD_NAME_LEN 32 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_STORAGE_INFORMATION { \ + 261, \ + "STORAGE_INFORMATION", \ + 11, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_storage_information_t, time_boot_ms) }, \ + { "storage_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_storage_information_t, storage_id) }, \ + { "storage_count", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_storage_information_t, storage_count) }, \ + { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_storage_information_t, status) }, \ + { "total_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_storage_information_t, total_capacity) }, \ + { "used_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_storage_information_t, used_capacity) }, \ + { "available_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_storage_information_t, available_capacity) }, \ + { "read_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_storage_information_t, read_speed) }, \ + { "write_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_storage_information_t, write_speed) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_storage_information_t, type) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 32, 28, offsetof(mavlink_storage_information_t, name) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_STORAGE_INFORMATION { \ + "STORAGE_INFORMATION", \ + 11, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_storage_information_t, time_boot_ms) }, \ + { "storage_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_storage_information_t, storage_id) }, \ + { "storage_count", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_storage_information_t, storage_count) }, \ + { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_storage_information_t, status) }, \ + { "total_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_storage_information_t, total_capacity) }, \ + { "used_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_storage_information_t, used_capacity) }, \ + { "available_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_storage_information_t, available_capacity) }, \ + { "read_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_storage_information_t, read_speed) }, \ + { "write_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_storage_information_t, write_speed) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_storage_information_t, type) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 32, 28, offsetof(mavlink_storage_information_t, name) }, \ + } \ +} +#endif + +/** + * @brief Pack a storage_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param storage_id Storage ID (1 for first, 2 for second, etc.) + * @param storage_count Number of storage devices + * @param status Status of storage + * @param total_capacity [MiB] Total capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param used_capacity [MiB] Used capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param available_capacity [MiB] Available storage capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param read_speed [MiB/s] Read speed. + * @param write_speed [MiB/s] Write speed. + * @param type Type of storage + * @param name Textual storage name to be used in UI (microSD 1, Internal Memory, etc.) This is a NULL terminated string. If it is exactly 32 characters long, add a terminating NULL. If this string is empty, the generic type is shown to the user. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_storage_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t storage_id, uint8_t storage_count, uint8_t status, float total_capacity, float used_capacity, float available_capacity, float read_speed, float write_speed, uint8_t type, const char *name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, total_capacity); + _mav_put_float(buf, 8, used_capacity); + _mav_put_float(buf, 12, available_capacity); + _mav_put_float(buf, 16, read_speed); + _mav_put_float(buf, 20, write_speed); + _mav_put_uint8_t(buf, 24, storage_id); + _mav_put_uint8_t(buf, 25, storage_count); + _mav_put_uint8_t(buf, 26, status); + _mav_put_uint8_t(buf, 27, type); + _mav_put_char_array(buf, 28, name, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); +#else + mavlink_storage_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.total_capacity = total_capacity; + packet.used_capacity = used_capacity; + packet.available_capacity = available_capacity; + packet.read_speed = read_speed; + packet.write_speed = write_speed; + packet.storage_id = storage_id; + packet.storage_count = storage_count; + packet.status = status; + packet.type = type; + mav_array_memcpy(packet.name, name, sizeof(char)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_STORAGE_INFORMATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); +} + +/** + * @brief Pack a storage_information message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param storage_id Storage ID (1 for first, 2 for second, etc.) + * @param storage_count Number of storage devices + * @param status Status of storage + * @param total_capacity [MiB] Total capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param used_capacity [MiB] Used capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param available_capacity [MiB] Available storage capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param read_speed [MiB/s] Read speed. + * @param write_speed [MiB/s] Write speed. + * @param type Type of storage + * @param name Textual storage name to be used in UI (microSD 1, Internal Memory, etc.) This is a NULL terminated string. If it is exactly 32 characters long, add a terminating NULL. If this string is empty, the generic type is shown to the user. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_storage_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t storage_id,uint8_t storage_count,uint8_t status,float total_capacity,float used_capacity,float available_capacity,float read_speed,float write_speed,uint8_t type,const char *name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, total_capacity); + _mav_put_float(buf, 8, used_capacity); + _mav_put_float(buf, 12, available_capacity); + _mav_put_float(buf, 16, read_speed); + _mav_put_float(buf, 20, write_speed); + _mav_put_uint8_t(buf, 24, storage_id); + _mav_put_uint8_t(buf, 25, storage_count); + _mav_put_uint8_t(buf, 26, status); + _mav_put_uint8_t(buf, 27, type); + _mav_put_char_array(buf, 28, name, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); +#else + mavlink_storage_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.total_capacity = total_capacity; + packet.used_capacity = used_capacity; + packet.available_capacity = available_capacity; + packet.read_speed = read_speed; + packet.write_speed = write_speed; + packet.storage_id = storage_id; + packet.storage_count = storage_count; + packet.status = status; + packet.type = type; + mav_array_memcpy(packet.name, name, sizeof(char)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_STORAGE_INFORMATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); +} + +/** + * @brief Encode a storage_information struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param storage_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_storage_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_storage_information_t* storage_information) +{ + return mavlink_msg_storage_information_pack(system_id, component_id, msg, storage_information->time_boot_ms, storage_information->storage_id, storage_information->storage_count, storage_information->status, storage_information->total_capacity, storage_information->used_capacity, storage_information->available_capacity, storage_information->read_speed, storage_information->write_speed, storage_information->type, storage_information->name); +} + +/** + * @brief Encode a storage_information struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param storage_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_storage_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_storage_information_t* storage_information) +{ + return mavlink_msg_storage_information_pack_chan(system_id, component_id, chan, msg, storage_information->time_boot_ms, storage_information->storage_id, storage_information->storage_count, storage_information->status, storage_information->total_capacity, storage_information->used_capacity, storage_information->available_capacity, storage_information->read_speed, storage_information->write_speed, storage_information->type, storage_information->name); +} + +/** + * @brief Send a storage_information message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param storage_id Storage ID (1 for first, 2 for second, etc.) + * @param storage_count Number of storage devices + * @param status Status of storage + * @param total_capacity [MiB] Total capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param used_capacity [MiB] Used capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param available_capacity [MiB] Available storage capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param read_speed [MiB/s] Read speed. + * @param write_speed [MiB/s] Write speed. + * @param type Type of storage + * @param name Textual storage name to be used in UI (microSD 1, Internal Memory, etc.) This is a NULL terminated string. If it is exactly 32 characters long, add a terminating NULL. If this string is empty, the generic type is shown to the user. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_storage_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t storage_id, uint8_t storage_count, uint8_t status, float total_capacity, float used_capacity, float available_capacity, float read_speed, float write_speed, uint8_t type, const char *name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, total_capacity); + _mav_put_float(buf, 8, used_capacity); + _mav_put_float(buf, 12, available_capacity); + _mav_put_float(buf, 16, read_speed); + _mav_put_float(buf, 20, write_speed); + _mav_put_uint8_t(buf, 24, storage_id); + _mav_put_uint8_t(buf, 25, storage_count); + _mav_put_uint8_t(buf, 26, status); + _mav_put_uint8_t(buf, 27, type); + _mav_put_char_array(buf, 28, name, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STORAGE_INFORMATION, buf, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); +#else + mavlink_storage_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.total_capacity = total_capacity; + packet.used_capacity = used_capacity; + packet.available_capacity = available_capacity; + packet.read_speed = read_speed; + packet.write_speed = write_speed; + packet.storage_id = storage_id; + packet.storage_count = storage_count; + packet.status = status; + packet.type = type; + mav_array_memcpy(packet.name, name, sizeof(char)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STORAGE_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); +#endif +} + +/** + * @brief Send a storage_information message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_storage_information_send_struct(mavlink_channel_t chan, const mavlink_storage_information_t* storage_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_storage_information_send(chan, storage_information->time_boot_ms, storage_information->storage_id, storage_information->storage_count, storage_information->status, storage_information->total_capacity, storage_information->used_capacity, storage_information->available_capacity, storage_information->read_speed, storage_information->write_speed, storage_information->type, storage_information->name); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STORAGE_INFORMATION, (const char *)storage_information, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_storage_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t storage_id, uint8_t storage_count, uint8_t status, float total_capacity, float used_capacity, float available_capacity, float read_speed, float write_speed, uint8_t type, const char *name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, total_capacity); + _mav_put_float(buf, 8, used_capacity); + _mav_put_float(buf, 12, available_capacity); + _mav_put_float(buf, 16, read_speed); + _mav_put_float(buf, 20, write_speed); + _mav_put_uint8_t(buf, 24, storage_id); + _mav_put_uint8_t(buf, 25, storage_count); + _mav_put_uint8_t(buf, 26, status); + _mav_put_uint8_t(buf, 27, type); + _mav_put_char_array(buf, 28, name, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STORAGE_INFORMATION, buf, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); +#else + mavlink_storage_information_t *packet = (mavlink_storage_information_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->total_capacity = total_capacity; + packet->used_capacity = used_capacity; + packet->available_capacity = available_capacity; + packet->read_speed = read_speed; + packet->write_speed = write_speed; + packet->storage_id = storage_id; + packet->storage_count = storage_count; + packet->status = status; + packet->type = type; + mav_array_memcpy(packet->name, name, sizeof(char)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STORAGE_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE STORAGE_INFORMATION UNPACKING + + +/** + * @brief Get field time_boot_ms from storage_information message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_storage_information_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field storage_id from storage_information message + * + * @return Storage ID (1 for first, 2 for second, etc.) + */ +static inline uint8_t mavlink_msg_storage_information_get_storage_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 24); +} + +/** + * @brief Get field storage_count from storage_information message + * + * @return Number of storage devices + */ +static inline uint8_t mavlink_msg_storage_information_get_storage_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 25); +} + +/** + * @brief Get field status from storage_information message + * + * @return Status of storage + */ +static inline uint8_t mavlink_msg_storage_information_get_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 26); +} + +/** + * @brief Get field total_capacity from storage_information message + * + * @return [MiB] Total capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + */ +static inline float mavlink_msg_storage_information_get_total_capacity(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field used_capacity from storage_information message + * + * @return [MiB] Used capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + */ +static inline float mavlink_msg_storage_information_get_used_capacity(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field available_capacity from storage_information message + * + * @return [MiB] Available storage capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + */ +static inline float mavlink_msg_storage_information_get_available_capacity(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field read_speed from storage_information message + * + * @return [MiB/s] Read speed. + */ +static inline float mavlink_msg_storage_information_get_read_speed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field write_speed from storage_information message + * + * @return [MiB/s] Write speed. + */ +static inline float mavlink_msg_storage_information_get_write_speed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field type from storage_information message + * + * @return Type of storage + */ +static inline uint8_t mavlink_msg_storage_information_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 27); +} + +/** + * @brief Get field name from storage_information message + * + * @return Textual storage name to be used in UI (microSD 1, Internal Memory, etc.) This is a NULL terminated string. If it is exactly 32 characters long, add a terminating NULL. If this string is empty, the generic type is shown to the user. + */ +static inline uint16_t mavlink_msg_storage_information_get_name(const mavlink_message_t* msg, char *name) +{ + return _MAV_RETURN_char_array(msg, name, 32, 28); +} + +/** + * @brief Decode a storage_information message into a struct + * + * @param msg The message to decode + * @param storage_information C-struct to decode the message contents into + */ +static inline void mavlink_msg_storage_information_decode(const mavlink_message_t* msg, mavlink_storage_information_t* storage_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + storage_information->time_boot_ms = mavlink_msg_storage_information_get_time_boot_ms(msg); + storage_information->total_capacity = mavlink_msg_storage_information_get_total_capacity(msg); + storage_information->used_capacity = mavlink_msg_storage_information_get_used_capacity(msg); + storage_information->available_capacity = mavlink_msg_storage_information_get_available_capacity(msg); + storage_information->read_speed = mavlink_msg_storage_information_get_read_speed(msg); + storage_information->write_speed = mavlink_msg_storage_information_get_write_speed(msg); + storage_information->storage_id = mavlink_msg_storage_information_get_storage_id(msg); + storage_information->storage_count = mavlink_msg_storage_information_get_storage_count(msg); + storage_information->status = mavlink_msg_storage_information_get_status(msg); + storage_information->type = mavlink_msg_storage_information_get_type(msg); + mavlink_msg_storage_information_get_name(msg, storage_information->name); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN; + memset(storage_information, 0, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); + memcpy(storage_information, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_supported_tunes.h b/lib/main/MAVLink/common/mavlink_msg_supported_tunes.h new file mode 100644 index 0000000000..a8fb9a3abf --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_supported_tunes.h @@ -0,0 +1,263 @@ +#pragma once +// MESSAGE SUPPORTED_TUNES PACKING + +#define MAVLINK_MSG_ID_SUPPORTED_TUNES 401 + + +typedef struct __mavlink_supported_tunes_t { + uint32_t format; /*< Bitfield of supported tune formats.*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ +} mavlink_supported_tunes_t; + +#define MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN 6 +#define MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN 6 +#define MAVLINK_MSG_ID_401_LEN 6 +#define MAVLINK_MSG_ID_401_MIN_LEN 6 + +#define MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC 183 +#define MAVLINK_MSG_ID_401_CRC 183 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_SUPPORTED_TUNES { \ + 401, \ + "SUPPORTED_TUNES", \ + 3, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_supported_tunes_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_supported_tunes_t, target_component) }, \ + { "format", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_supported_tunes_t, format) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_SUPPORTED_TUNES { \ + "SUPPORTED_TUNES", \ + 3, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_supported_tunes_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_supported_tunes_t, target_component) }, \ + { "format", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_supported_tunes_t, format) }, \ + } \ +} +#endif + +/** + * @brief Pack a supported_tunes message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param format Bitfield of supported tune formats. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_supported_tunes_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t format) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN]; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN); +#else + mavlink_supported_tunes_t packet; + packet.format = format; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SUPPORTED_TUNES; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC); +} + +/** + * @brief Pack a supported_tunes message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param format Bitfield of supported tune formats. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_supported_tunes_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint32_t format) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN]; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN); +#else + mavlink_supported_tunes_t packet; + packet.format = format; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SUPPORTED_TUNES; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC); +} + +/** + * @brief Encode a supported_tunes struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param supported_tunes C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_supported_tunes_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_supported_tunes_t* supported_tunes) +{ + return mavlink_msg_supported_tunes_pack(system_id, component_id, msg, supported_tunes->target_system, supported_tunes->target_component, supported_tunes->format); +} + +/** + * @brief Encode a supported_tunes struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param supported_tunes C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_supported_tunes_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_supported_tunes_t* supported_tunes) +{ + return mavlink_msg_supported_tunes_pack_chan(system_id, component_id, chan, msg, supported_tunes->target_system, supported_tunes->target_component, supported_tunes->format); +} + +/** + * @brief Send a supported_tunes message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param format Bitfield of supported tune formats. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_supported_tunes_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t format) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN]; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SUPPORTED_TUNES, buf, MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC); +#else + mavlink_supported_tunes_t packet; + packet.format = format; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SUPPORTED_TUNES, (const char *)&packet, MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC); +#endif +} + +/** + * @brief Send a supported_tunes message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_supported_tunes_send_struct(mavlink_channel_t chan, const mavlink_supported_tunes_t* supported_tunes) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_supported_tunes_send(chan, supported_tunes->target_system, supported_tunes->target_component, supported_tunes->format); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SUPPORTED_TUNES, (const char *)supported_tunes, MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC); +#endif +} + +#if MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_supported_tunes_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t format) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SUPPORTED_TUNES, buf, MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC); +#else + mavlink_supported_tunes_t *packet = (mavlink_supported_tunes_t *)msgbuf; + packet->format = format; + packet->target_system = target_system; + packet->target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SUPPORTED_TUNES, (const char *)packet, MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC); +#endif +} +#endif + +#endif + +// MESSAGE SUPPORTED_TUNES UNPACKING + + +/** + * @brief Get field target_system from supported_tunes message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_supported_tunes_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from supported_tunes message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_supported_tunes_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field format from supported_tunes message + * + * @return Bitfield of supported tune formats. + */ +static inline uint32_t mavlink_msg_supported_tunes_get_format(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Decode a supported_tunes message into a struct + * + * @param msg The message to decode + * @param supported_tunes C-struct to decode the message contents into + */ +static inline void mavlink_msg_supported_tunes_decode(const mavlink_message_t* msg, mavlink_supported_tunes_t* supported_tunes) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + supported_tunes->format = mavlink_msg_supported_tunes_get_format(msg); + supported_tunes->target_system = mavlink_msg_supported_tunes_get_target_system(msg); + supported_tunes->target_component = mavlink_msg_supported_tunes_get_target_component(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN? msg->len : MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN; + memset(supported_tunes, 0, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN); + memcpy(supported_tunes, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_time_estimate_to_target.h b/lib/main/MAVLink/common/mavlink_msg_time_estimate_to_target.h new file mode 100644 index 0000000000..29d03d5946 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_time_estimate_to_target.h @@ -0,0 +1,313 @@ +#pragma once +// MESSAGE TIME_ESTIMATE_TO_TARGET PACKING + +#define MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET 380 + + +typedef struct __mavlink_time_estimate_to_target_t { + int32_t safe_return; /*< [s] Estimated time to complete the vehicle's configured "safe return" action from its current position (e.g. RTL, Smart RTL, etc.). -1 indicates that the vehicle is landed, or that no time estimate available.*/ + int32_t land; /*< [s] Estimated time for vehicle to complete the LAND action from its current position. -1 indicates that the vehicle is landed, or that no time estimate available.*/ + int32_t mission_next_item; /*< [s] Estimated time for reaching/completing the currently active mission item. -1 means no time estimate available.*/ + int32_t mission_end; /*< [s] Estimated time for completing the current mission. -1 means no mission active and/or no estimate available.*/ + int32_t commanded_action; /*< [s] Estimated time for completing the current commanded action (i.e. Go To, Takeoff, Land, etc.). -1 means no action active and/or no estimate available.*/ +} mavlink_time_estimate_to_target_t; + +#define MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN 20 +#define MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN 20 +#define MAVLINK_MSG_ID_380_LEN 20 +#define MAVLINK_MSG_ID_380_MIN_LEN 20 + +#define MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC 232 +#define MAVLINK_MSG_ID_380_CRC 232 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_TIME_ESTIMATE_TO_TARGET { \ + 380, \ + "TIME_ESTIMATE_TO_TARGET", \ + 5, \ + { { "safe_return", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_time_estimate_to_target_t, safe_return) }, \ + { "land", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_time_estimate_to_target_t, land) }, \ + { "mission_next_item", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_time_estimate_to_target_t, mission_next_item) }, \ + { "mission_end", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_time_estimate_to_target_t, mission_end) }, \ + { "commanded_action", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_time_estimate_to_target_t, commanded_action) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_TIME_ESTIMATE_TO_TARGET { \ + "TIME_ESTIMATE_TO_TARGET", \ + 5, \ + { { "safe_return", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_time_estimate_to_target_t, safe_return) }, \ + { "land", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_time_estimate_to_target_t, land) }, \ + { "mission_next_item", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_time_estimate_to_target_t, mission_next_item) }, \ + { "mission_end", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_time_estimate_to_target_t, mission_end) }, \ + { "commanded_action", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_time_estimate_to_target_t, commanded_action) }, \ + } \ +} +#endif + +/** + * @brief Pack a time_estimate_to_target message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param safe_return [s] Estimated time to complete the vehicle's configured "safe return" action from its current position (e.g. RTL, Smart RTL, etc.). -1 indicates that the vehicle is landed, or that no time estimate available. + * @param land [s] Estimated time for vehicle to complete the LAND action from its current position. -1 indicates that the vehicle is landed, or that no time estimate available. + * @param mission_next_item [s] Estimated time for reaching/completing the currently active mission item. -1 means no time estimate available. + * @param mission_end [s] Estimated time for completing the current mission. -1 means no mission active and/or no estimate available. + * @param commanded_action [s] Estimated time for completing the current commanded action (i.e. Go To, Takeoff, Land, etc.). -1 means no action active and/or no estimate available. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_time_estimate_to_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int32_t safe_return, int32_t land, int32_t mission_next_item, int32_t mission_end, int32_t commanded_action) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN]; + _mav_put_int32_t(buf, 0, safe_return); + _mav_put_int32_t(buf, 4, land); + _mav_put_int32_t(buf, 8, mission_next_item); + _mav_put_int32_t(buf, 12, mission_end); + _mav_put_int32_t(buf, 16, commanded_action); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN); +#else + mavlink_time_estimate_to_target_t packet; + packet.safe_return = safe_return; + packet.land = land; + packet.mission_next_item = mission_next_item; + packet.mission_end = mission_end; + packet.commanded_action = commanded_action; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC); +} + +/** + * @brief Pack a time_estimate_to_target message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param safe_return [s] Estimated time to complete the vehicle's configured "safe return" action from its current position (e.g. RTL, Smart RTL, etc.). -1 indicates that the vehicle is landed, or that no time estimate available. + * @param land [s] Estimated time for vehicle to complete the LAND action from its current position. -1 indicates that the vehicle is landed, or that no time estimate available. + * @param mission_next_item [s] Estimated time for reaching/completing the currently active mission item. -1 means no time estimate available. + * @param mission_end [s] Estimated time for completing the current mission. -1 means no mission active and/or no estimate available. + * @param commanded_action [s] Estimated time for completing the current commanded action (i.e. Go To, Takeoff, Land, etc.). -1 means no action active and/or no estimate available. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_time_estimate_to_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + int32_t safe_return,int32_t land,int32_t mission_next_item,int32_t mission_end,int32_t commanded_action) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN]; + _mav_put_int32_t(buf, 0, safe_return); + _mav_put_int32_t(buf, 4, land); + _mav_put_int32_t(buf, 8, mission_next_item); + _mav_put_int32_t(buf, 12, mission_end); + _mav_put_int32_t(buf, 16, commanded_action); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN); +#else + mavlink_time_estimate_to_target_t packet; + packet.safe_return = safe_return; + packet.land = land; + packet.mission_next_item = mission_next_item; + packet.mission_end = mission_end; + packet.commanded_action = commanded_action; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC); +} + +/** + * @brief Encode a time_estimate_to_target struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param time_estimate_to_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_time_estimate_to_target_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_time_estimate_to_target_t* time_estimate_to_target) +{ + return mavlink_msg_time_estimate_to_target_pack(system_id, component_id, msg, time_estimate_to_target->safe_return, time_estimate_to_target->land, time_estimate_to_target->mission_next_item, time_estimate_to_target->mission_end, time_estimate_to_target->commanded_action); +} + +/** + * @brief Encode a time_estimate_to_target struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_estimate_to_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_time_estimate_to_target_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_time_estimate_to_target_t* time_estimate_to_target) +{ + return mavlink_msg_time_estimate_to_target_pack_chan(system_id, component_id, chan, msg, time_estimate_to_target->safe_return, time_estimate_to_target->land, time_estimate_to_target->mission_next_item, time_estimate_to_target->mission_end, time_estimate_to_target->commanded_action); +} + +/** + * @brief Send a time_estimate_to_target message + * @param chan MAVLink channel to send the message + * + * @param safe_return [s] Estimated time to complete the vehicle's configured "safe return" action from its current position (e.g. RTL, Smart RTL, etc.). -1 indicates that the vehicle is landed, or that no time estimate available. + * @param land [s] Estimated time for vehicle to complete the LAND action from its current position. -1 indicates that the vehicle is landed, or that no time estimate available. + * @param mission_next_item [s] Estimated time for reaching/completing the currently active mission item. -1 means no time estimate available. + * @param mission_end [s] Estimated time for completing the current mission. -1 means no mission active and/or no estimate available. + * @param commanded_action [s] Estimated time for completing the current commanded action (i.e. Go To, Takeoff, Land, etc.). -1 means no action active and/or no estimate available. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_time_estimate_to_target_send(mavlink_channel_t chan, int32_t safe_return, int32_t land, int32_t mission_next_item, int32_t mission_end, int32_t commanded_action) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN]; + _mav_put_int32_t(buf, 0, safe_return); + _mav_put_int32_t(buf, 4, land); + _mav_put_int32_t(buf, 8, mission_next_item); + _mav_put_int32_t(buf, 12, mission_end); + _mav_put_int32_t(buf, 16, commanded_action); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET, buf, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC); +#else + mavlink_time_estimate_to_target_t packet; + packet.safe_return = safe_return; + packet.land = land; + packet.mission_next_item = mission_next_item; + packet.mission_end = mission_end; + packet.commanded_action = commanded_action; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET, (const char *)&packet, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC); +#endif +} + +/** + * @brief Send a time_estimate_to_target message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_time_estimate_to_target_send_struct(mavlink_channel_t chan, const mavlink_time_estimate_to_target_t* time_estimate_to_target) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_time_estimate_to_target_send(chan, time_estimate_to_target->safe_return, time_estimate_to_target->land, time_estimate_to_target->mission_next_item, time_estimate_to_target->mission_end, time_estimate_to_target->commanded_action); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET, (const char *)time_estimate_to_target, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC); +#endif +} + +#if MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_time_estimate_to_target_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t safe_return, int32_t land, int32_t mission_next_item, int32_t mission_end, int32_t commanded_action) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, safe_return); + _mav_put_int32_t(buf, 4, land); + _mav_put_int32_t(buf, 8, mission_next_item); + _mav_put_int32_t(buf, 12, mission_end); + _mav_put_int32_t(buf, 16, commanded_action); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET, buf, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC); +#else + mavlink_time_estimate_to_target_t *packet = (mavlink_time_estimate_to_target_t *)msgbuf; + packet->safe_return = safe_return; + packet->land = land; + packet->mission_next_item = mission_next_item; + packet->mission_end = mission_end; + packet->commanded_action = commanded_action; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET, (const char *)packet, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC); +#endif +} +#endif + +#endif + +// MESSAGE TIME_ESTIMATE_TO_TARGET UNPACKING + + +/** + * @brief Get field safe_return from time_estimate_to_target message + * + * @return [s] Estimated time to complete the vehicle's configured "safe return" action from its current position (e.g. RTL, Smart RTL, etc.). -1 indicates that the vehicle is landed, or that no time estimate available. + */ +static inline int32_t mavlink_msg_time_estimate_to_target_get_safe_return(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field land from time_estimate_to_target message + * + * @return [s] Estimated time for vehicle to complete the LAND action from its current position. -1 indicates that the vehicle is landed, or that no time estimate available. + */ +static inline int32_t mavlink_msg_time_estimate_to_target_get_land(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field mission_next_item from time_estimate_to_target message + * + * @return [s] Estimated time for reaching/completing the currently active mission item. -1 means no time estimate available. + */ +static inline int32_t mavlink_msg_time_estimate_to_target_get_mission_next_item(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field mission_end from time_estimate_to_target message + * + * @return [s] Estimated time for completing the current mission. -1 means no mission active and/or no estimate available. + */ +static inline int32_t mavlink_msg_time_estimate_to_target_get_mission_end(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field commanded_action from time_estimate_to_target message + * + * @return [s] Estimated time for completing the current commanded action (i.e. Go To, Takeoff, Land, etc.). -1 means no action active and/or no estimate available. + */ +static inline int32_t mavlink_msg_time_estimate_to_target_get_commanded_action(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Decode a time_estimate_to_target message into a struct + * + * @param msg The message to decode + * @param time_estimate_to_target C-struct to decode the message contents into + */ +static inline void mavlink_msg_time_estimate_to_target_decode(const mavlink_message_t* msg, mavlink_time_estimate_to_target_t* time_estimate_to_target) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + time_estimate_to_target->safe_return = mavlink_msg_time_estimate_to_target_get_safe_return(msg); + time_estimate_to_target->land = mavlink_msg_time_estimate_to_target_get_land(msg); + time_estimate_to_target->mission_next_item = mavlink_msg_time_estimate_to_target_get_mission_next_item(msg); + time_estimate_to_target->mission_end = mavlink_msg_time_estimate_to_target_get_mission_end(msg); + time_estimate_to_target->commanded_action = mavlink_msg_time_estimate_to_target_get_commanded_action(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN? msg->len : MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN; + memset(time_estimate_to_target, 0, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN); + memcpy(time_estimate_to_target, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_bezier.h b/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_bezier.h new file mode 100644 index 0000000000..c535fd4416 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_bezier.h @@ -0,0 +1,359 @@ +#pragma once +// MESSAGE TRAJECTORY_REPRESENTATION_BEZIER PACKING + +#define MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER 333 + + +typedef struct __mavlink_trajectory_representation_bezier_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + float pos_x[5]; /*< [m] X-coordinate of bezier control points. Set to NaN if not being used*/ + float pos_y[5]; /*< [m] Y-coordinate of bezier control points. Set to NaN if not being used*/ + float pos_z[5]; /*< [m] Z-coordinate of bezier control points. Set to NaN if not being used*/ + float delta[5]; /*< [s] Bezier time horizon. Set to NaN if velocity/acceleration should not be incorporated*/ + float pos_yaw[5]; /*< [rad] Yaw. Set to NaN for unchanged*/ + uint8_t valid_points; /*< Number of valid control points (up-to 5 points are possible)*/ +} mavlink_trajectory_representation_bezier_t; + +#define MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN 109 +#define MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN 109 +#define MAVLINK_MSG_ID_333_LEN 109 +#define MAVLINK_MSG_ID_333_MIN_LEN 109 + +#define MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC 231 +#define MAVLINK_MSG_ID_333_CRC 231 + +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_BEZIER_FIELD_POS_X_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_BEZIER_FIELD_POS_Y_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_BEZIER_FIELD_POS_Z_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_BEZIER_FIELD_DELTA_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_BEZIER_FIELD_POS_YAW_LEN 5 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_BEZIER { \ + 333, \ + "TRAJECTORY_REPRESENTATION_BEZIER", \ + 7, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_trajectory_representation_bezier_t, time_usec) }, \ + { "valid_points", NULL, MAVLINK_TYPE_UINT8_T, 0, 108, offsetof(mavlink_trajectory_representation_bezier_t, valid_points) }, \ + { "pos_x", NULL, MAVLINK_TYPE_FLOAT, 5, 8, offsetof(mavlink_trajectory_representation_bezier_t, pos_x) }, \ + { "pos_y", NULL, MAVLINK_TYPE_FLOAT, 5, 28, offsetof(mavlink_trajectory_representation_bezier_t, pos_y) }, \ + { "pos_z", NULL, MAVLINK_TYPE_FLOAT, 5, 48, offsetof(mavlink_trajectory_representation_bezier_t, pos_z) }, \ + { "delta", NULL, MAVLINK_TYPE_FLOAT, 5, 68, offsetof(mavlink_trajectory_representation_bezier_t, delta) }, \ + { "pos_yaw", NULL, MAVLINK_TYPE_FLOAT, 5, 88, offsetof(mavlink_trajectory_representation_bezier_t, pos_yaw) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_BEZIER { \ + "TRAJECTORY_REPRESENTATION_BEZIER", \ + 7, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_trajectory_representation_bezier_t, time_usec) }, \ + { "valid_points", NULL, MAVLINK_TYPE_UINT8_T, 0, 108, offsetof(mavlink_trajectory_representation_bezier_t, valid_points) }, \ + { "pos_x", NULL, MAVLINK_TYPE_FLOAT, 5, 8, offsetof(mavlink_trajectory_representation_bezier_t, pos_x) }, \ + { "pos_y", NULL, MAVLINK_TYPE_FLOAT, 5, 28, offsetof(mavlink_trajectory_representation_bezier_t, pos_y) }, \ + { "pos_z", NULL, MAVLINK_TYPE_FLOAT, 5, 48, offsetof(mavlink_trajectory_representation_bezier_t, pos_z) }, \ + { "delta", NULL, MAVLINK_TYPE_FLOAT, 5, 68, offsetof(mavlink_trajectory_representation_bezier_t, delta) }, \ + { "pos_yaw", NULL, MAVLINK_TYPE_FLOAT, 5, 88, offsetof(mavlink_trajectory_representation_bezier_t, pos_yaw) }, \ + } \ +} +#endif + +/** + * @brief Pack a trajectory_representation_bezier message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param valid_points Number of valid control points (up-to 5 points are possible) + * @param pos_x [m] X-coordinate of bezier control points. Set to NaN if not being used + * @param pos_y [m] Y-coordinate of bezier control points. Set to NaN if not being used + * @param pos_z [m] Z-coordinate of bezier control points. Set to NaN if not being used + * @param delta [s] Bezier time horizon. Set to NaN if velocity/acceleration should not be incorporated + * @param pos_yaw [rad] Yaw. Set to NaN for unchanged + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t valid_points, const float *pos_x, const float *pos_y, const float *pos_z, const float *delta, const float *pos_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 108, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, delta, 5); + _mav_put_float_array(buf, 88, pos_yaw, 5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN); +#else + mavlink_trajectory_representation_bezier_t packet; + packet.time_usec = time_usec; + packet.valid_points = valid_points; + mav_array_memcpy(packet.pos_x, pos_x, sizeof(float)*5); + mav_array_memcpy(packet.pos_y, pos_y, sizeof(float)*5); + mav_array_memcpy(packet.pos_z, pos_z, sizeof(float)*5); + mav_array_memcpy(packet.delta, delta, sizeof(float)*5); + mav_array_memcpy(packet.pos_yaw, pos_yaw, sizeof(float)*5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); +} + +/** + * @brief Pack a trajectory_representation_bezier message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param valid_points Number of valid control points (up-to 5 points are possible) + * @param pos_x [m] X-coordinate of bezier control points. Set to NaN if not being used + * @param pos_y [m] Y-coordinate of bezier control points. Set to NaN if not being used + * @param pos_z [m] Z-coordinate of bezier control points. Set to NaN if not being used + * @param delta [s] Bezier time horizon. Set to NaN if velocity/acceleration should not be incorporated + * @param pos_yaw [rad] Yaw. Set to NaN for unchanged + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t valid_points,const float *pos_x,const float *pos_y,const float *pos_z,const float *delta,const float *pos_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 108, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, delta, 5); + _mav_put_float_array(buf, 88, pos_yaw, 5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN); +#else + mavlink_trajectory_representation_bezier_t packet; + packet.time_usec = time_usec; + packet.valid_points = valid_points; + mav_array_memcpy(packet.pos_x, pos_x, sizeof(float)*5); + mav_array_memcpy(packet.pos_y, pos_y, sizeof(float)*5); + mav_array_memcpy(packet.pos_z, pos_z, sizeof(float)*5); + mav_array_memcpy(packet.delta, delta, sizeof(float)*5); + mav_array_memcpy(packet.pos_yaw, pos_yaw, sizeof(float)*5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); +} + +/** + * @brief Encode a trajectory_representation_bezier struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param trajectory_representation_bezier C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_trajectory_representation_bezier_t* trajectory_representation_bezier) +{ + return mavlink_msg_trajectory_representation_bezier_pack(system_id, component_id, msg, trajectory_representation_bezier->time_usec, trajectory_representation_bezier->valid_points, trajectory_representation_bezier->pos_x, trajectory_representation_bezier->pos_y, trajectory_representation_bezier->pos_z, trajectory_representation_bezier->delta, trajectory_representation_bezier->pos_yaw); +} + +/** + * @brief Encode a trajectory_representation_bezier struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param trajectory_representation_bezier C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_trajectory_representation_bezier_t* trajectory_representation_bezier) +{ + return mavlink_msg_trajectory_representation_bezier_pack_chan(system_id, component_id, chan, msg, trajectory_representation_bezier->time_usec, trajectory_representation_bezier->valid_points, trajectory_representation_bezier->pos_x, trajectory_representation_bezier->pos_y, trajectory_representation_bezier->pos_z, trajectory_representation_bezier->delta, trajectory_representation_bezier->pos_yaw); +} + +/** + * @brief Send a trajectory_representation_bezier message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param valid_points Number of valid control points (up-to 5 points are possible) + * @param pos_x [m] X-coordinate of bezier control points. Set to NaN if not being used + * @param pos_y [m] Y-coordinate of bezier control points. Set to NaN if not being used + * @param pos_z [m] Z-coordinate of bezier control points. Set to NaN if not being used + * @param delta [s] Bezier time horizon. Set to NaN if velocity/acceleration should not be incorporated + * @param pos_yaw [rad] Yaw. Set to NaN for unchanged + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_trajectory_representation_bezier_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t valid_points, const float *pos_x, const float *pos_y, const float *pos_z, const float *delta, const float *pos_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 108, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, delta, 5); + _mav_put_float_array(buf, 88, pos_yaw, 5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER, buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); +#else + mavlink_trajectory_representation_bezier_t packet; + packet.time_usec = time_usec; + packet.valid_points = valid_points; + mav_array_memcpy(packet.pos_x, pos_x, sizeof(float)*5); + mav_array_memcpy(packet.pos_y, pos_y, sizeof(float)*5); + mav_array_memcpy(packet.pos_z, pos_z, sizeof(float)*5); + mav_array_memcpy(packet.delta, delta, sizeof(float)*5); + mav_array_memcpy(packet.pos_yaw, pos_yaw, sizeof(float)*5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER, (const char *)&packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); +#endif +} + +/** + * @brief Send a trajectory_representation_bezier message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_trajectory_representation_bezier_send_struct(mavlink_channel_t chan, const mavlink_trajectory_representation_bezier_t* trajectory_representation_bezier) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_trajectory_representation_bezier_send(chan, trajectory_representation_bezier->time_usec, trajectory_representation_bezier->valid_points, trajectory_representation_bezier->pos_x, trajectory_representation_bezier->pos_y, trajectory_representation_bezier->pos_z, trajectory_representation_bezier->delta, trajectory_representation_bezier->pos_yaw); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER, (const char *)trajectory_representation_bezier, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); +#endif +} + +#if MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_trajectory_representation_bezier_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t valid_points, const float *pos_x, const float *pos_y, const float *pos_z, const float *delta, const float *pos_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 108, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, delta, 5); + _mav_put_float_array(buf, 88, pos_yaw, 5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER, buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); +#else + mavlink_trajectory_representation_bezier_t *packet = (mavlink_trajectory_representation_bezier_t *)msgbuf; + packet->time_usec = time_usec; + packet->valid_points = valid_points; + mav_array_memcpy(packet->pos_x, pos_x, sizeof(float)*5); + mav_array_memcpy(packet->pos_y, pos_y, sizeof(float)*5); + mav_array_memcpy(packet->pos_z, pos_z, sizeof(float)*5); + mav_array_memcpy(packet->delta, delta, sizeof(float)*5); + mav_array_memcpy(packet->pos_yaw, pos_yaw, sizeof(float)*5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER, (const char *)packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); +#endif +} +#endif + +#endif + +// MESSAGE TRAJECTORY_REPRESENTATION_BEZIER UNPACKING + + +/** + * @brief Get field time_usec from trajectory_representation_bezier message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_trajectory_representation_bezier_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field valid_points from trajectory_representation_bezier message + * + * @return Number of valid control points (up-to 5 points are possible) + */ +static inline uint8_t mavlink_msg_trajectory_representation_bezier_get_valid_points(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 108); +} + +/** + * @brief Get field pos_x from trajectory_representation_bezier message + * + * @return [m] X-coordinate of bezier control points. Set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_get_pos_x(const mavlink_message_t* msg, float *pos_x) +{ + return _MAV_RETURN_float_array(msg, pos_x, 5, 8); +} + +/** + * @brief Get field pos_y from trajectory_representation_bezier message + * + * @return [m] Y-coordinate of bezier control points. Set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_get_pos_y(const mavlink_message_t* msg, float *pos_y) +{ + return _MAV_RETURN_float_array(msg, pos_y, 5, 28); +} + +/** + * @brief Get field pos_z from trajectory_representation_bezier message + * + * @return [m] Z-coordinate of bezier control points. Set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_get_pos_z(const mavlink_message_t* msg, float *pos_z) +{ + return _MAV_RETURN_float_array(msg, pos_z, 5, 48); +} + +/** + * @brief Get field delta from trajectory_representation_bezier message + * + * @return [s] Bezier time horizon. Set to NaN if velocity/acceleration should not be incorporated + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_get_delta(const mavlink_message_t* msg, float *delta) +{ + return _MAV_RETURN_float_array(msg, delta, 5, 68); +} + +/** + * @brief Get field pos_yaw from trajectory_representation_bezier message + * + * @return [rad] Yaw. Set to NaN for unchanged + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_get_pos_yaw(const mavlink_message_t* msg, float *pos_yaw) +{ + return _MAV_RETURN_float_array(msg, pos_yaw, 5, 88); +} + +/** + * @brief Decode a trajectory_representation_bezier message into a struct + * + * @param msg The message to decode + * @param trajectory_representation_bezier C-struct to decode the message contents into + */ +static inline void mavlink_msg_trajectory_representation_bezier_decode(const mavlink_message_t* msg, mavlink_trajectory_representation_bezier_t* trajectory_representation_bezier) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + trajectory_representation_bezier->time_usec = mavlink_msg_trajectory_representation_bezier_get_time_usec(msg); + mavlink_msg_trajectory_representation_bezier_get_pos_x(msg, trajectory_representation_bezier->pos_x); + mavlink_msg_trajectory_representation_bezier_get_pos_y(msg, trajectory_representation_bezier->pos_y); + mavlink_msg_trajectory_representation_bezier_get_pos_z(msg, trajectory_representation_bezier->pos_z); + mavlink_msg_trajectory_representation_bezier_get_delta(msg, trajectory_representation_bezier->delta); + mavlink_msg_trajectory_representation_bezier_get_pos_yaw(msg, trajectory_representation_bezier->pos_yaw); + trajectory_representation_bezier->valid_points = mavlink_msg_trajectory_representation_bezier_get_valid_points(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN? msg->len : MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN; + memset(trajectory_representation_bezier, 0, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN); + memcpy(trajectory_representation_bezier, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_waypoints.h b/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_waypoints.h new file mode 100644 index 0000000000..470c4626fa --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_waypoints.h @@ -0,0 +1,541 @@ +#pragma once +// MESSAGE TRAJECTORY_REPRESENTATION_WAYPOINTS PACKING + +#define MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS 332 + + +typedef struct __mavlink_trajectory_representation_waypoints_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + float pos_x[5]; /*< [m] X-coordinate of waypoint, set to NaN if not being used*/ + float pos_y[5]; /*< [m] Y-coordinate of waypoint, set to NaN if not being used*/ + float pos_z[5]; /*< [m] Z-coordinate of waypoint, set to NaN if not being used*/ + float vel_x[5]; /*< [m/s] X-velocity of waypoint, set to NaN if not being used*/ + float vel_y[5]; /*< [m/s] Y-velocity of waypoint, set to NaN if not being used*/ + float vel_z[5]; /*< [m/s] Z-velocity of waypoint, set to NaN if not being used*/ + float acc_x[5]; /*< [m/s/s] X-acceleration of waypoint, set to NaN if not being used*/ + float acc_y[5]; /*< [m/s/s] Y-acceleration of waypoint, set to NaN if not being used*/ + float acc_z[5]; /*< [m/s/s] Z-acceleration of waypoint, set to NaN if not being used*/ + float pos_yaw[5]; /*< [rad] Yaw angle, set to NaN if not being used*/ + float vel_yaw[5]; /*< [rad/s] Yaw rate, set to NaN if not being used*/ + uint16_t command[5]; /*< Scheduled action for each waypoint, UINT16_MAX if not being used.*/ + uint8_t valid_points; /*< Number of valid points (up-to 5 waypoints are possible)*/ +} mavlink_trajectory_representation_waypoints_t; + +#define MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN 239 +#define MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN 239 +#define MAVLINK_MSG_ID_332_LEN 239 +#define MAVLINK_MSG_ID_332_MIN_LEN 239 + +#define MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC 236 +#define MAVLINK_MSG_ID_332_CRC 236 + +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_POS_X_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_POS_Y_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_POS_Z_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_VEL_X_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_VEL_Y_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_VEL_Z_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_ACC_X_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_ACC_Y_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_ACC_Z_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_POS_YAW_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_VEL_YAW_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_COMMAND_LEN 5 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_WAYPOINTS { \ + 332, \ + "TRAJECTORY_REPRESENTATION_WAYPOINTS", \ + 14, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_trajectory_representation_waypoints_t, time_usec) }, \ + { "valid_points", NULL, MAVLINK_TYPE_UINT8_T, 0, 238, offsetof(mavlink_trajectory_representation_waypoints_t, valid_points) }, \ + { "pos_x", NULL, MAVLINK_TYPE_FLOAT, 5, 8, offsetof(mavlink_trajectory_representation_waypoints_t, pos_x) }, \ + { "pos_y", NULL, MAVLINK_TYPE_FLOAT, 5, 28, offsetof(mavlink_trajectory_representation_waypoints_t, pos_y) }, \ + { "pos_z", NULL, MAVLINK_TYPE_FLOAT, 5, 48, offsetof(mavlink_trajectory_representation_waypoints_t, pos_z) }, \ + { "vel_x", NULL, MAVLINK_TYPE_FLOAT, 5, 68, offsetof(mavlink_trajectory_representation_waypoints_t, vel_x) }, \ + { "vel_y", NULL, MAVLINK_TYPE_FLOAT, 5, 88, offsetof(mavlink_trajectory_representation_waypoints_t, vel_y) }, \ + { "vel_z", NULL, MAVLINK_TYPE_FLOAT, 5, 108, offsetof(mavlink_trajectory_representation_waypoints_t, vel_z) }, \ + { "acc_x", NULL, MAVLINK_TYPE_FLOAT, 5, 128, offsetof(mavlink_trajectory_representation_waypoints_t, acc_x) }, \ + { "acc_y", NULL, MAVLINK_TYPE_FLOAT, 5, 148, offsetof(mavlink_trajectory_representation_waypoints_t, acc_y) }, \ + { "acc_z", NULL, MAVLINK_TYPE_FLOAT, 5, 168, offsetof(mavlink_trajectory_representation_waypoints_t, acc_z) }, \ + { "pos_yaw", NULL, MAVLINK_TYPE_FLOAT, 5, 188, offsetof(mavlink_trajectory_representation_waypoints_t, pos_yaw) }, \ + { "vel_yaw", NULL, MAVLINK_TYPE_FLOAT, 5, 208, offsetof(mavlink_trajectory_representation_waypoints_t, vel_yaw) }, \ + { "command", NULL, MAVLINK_TYPE_UINT16_T, 5, 228, offsetof(mavlink_trajectory_representation_waypoints_t, command) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_WAYPOINTS { \ + "TRAJECTORY_REPRESENTATION_WAYPOINTS", \ + 14, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_trajectory_representation_waypoints_t, time_usec) }, \ + { "valid_points", NULL, MAVLINK_TYPE_UINT8_T, 0, 238, offsetof(mavlink_trajectory_representation_waypoints_t, valid_points) }, \ + { "pos_x", NULL, MAVLINK_TYPE_FLOAT, 5, 8, offsetof(mavlink_trajectory_representation_waypoints_t, pos_x) }, \ + { "pos_y", NULL, MAVLINK_TYPE_FLOAT, 5, 28, offsetof(mavlink_trajectory_representation_waypoints_t, pos_y) }, \ + { "pos_z", NULL, MAVLINK_TYPE_FLOAT, 5, 48, offsetof(mavlink_trajectory_representation_waypoints_t, pos_z) }, \ + { "vel_x", NULL, MAVLINK_TYPE_FLOAT, 5, 68, offsetof(mavlink_trajectory_representation_waypoints_t, vel_x) }, \ + { "vel_y", NULL, MAVLINK_TYPE_FLOAT, 5, 88, offsetof(mavlink_trajectory_representation_waypoints_t, vel_y) }, \ + { "vel_z", NULL, MAVLINK_TYPE_FLOAT, 5, 108, offsetof(mavlink_trajectory_representation_waypoints_t, vel_z) }, \ + { "acc_x", NULL, MAVLINK_TYPE_FLOAT, 5, 128, offsetof(mavlink_trajectory_representation_waypoints_t, acc_x) }, \ + { "acc_y", NULL, MAVLINK_TYPE_FLOAT, 5, 148, offsetof(mavlink_trajectory_representation_waypoints_t, acc_y) }, \ + { "acc_z", NULL, MAVLINK_TYPE_FLOAT, 5, 168, offsetof(mavlink_trajectory_representation_waypoints_t, acc_z) }, \ + { "pos_yaw", NULL, MAVLINK_TYPE_FLOAT, 5, 188, offsetof(mavlink_trajectory_representation_waypoints_t, pos_yaw) }, \ + { "vel_yaw", NULL, MAVLINK_TYPE_FLOAT, 5, 208, offsetof(mavlink_trajectory_representation_waypoints_t, vel_yaw) }, \ + { "command", NULL, MAVLINK_TYPE_UINT16_T, 5, 228, offsetof(mavlink_trajectory_representation_waypoints_t, command) }, \ + } \ +} +#endif + +/** + * @brief Pack a trajectory_representation_waypoints message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param valid_points Number of valid points (up-to 5 waypoints are possible) + * @param pos_x [m] X-coordinate of waypoint, set to NaN if not being used + * @param pos_y [m] Y-coordinate of waypoint, set to NaN if not being used + * @param pos_z [m] Z-coordinate of waypoint, set to NaN if not being used + * @param vel_x [m/s] X-velocity of waypoint, set to NaN if not being used + * @param vel_y [m/s] Y-velocity of waypoint, set to NaN if not being used + * @param vel_z [m/s] Z-velocity of waypoint, set to NaN if not being used + * @param acc_x [m/s/s] X-acceleration of waypoint, set to NaN if not being used + * @param acc_y [m/s/s] Y-acceleration of waypoint, set to NaN if not being used + * @param acc_z [m/s/s] Z-acceleration of waypoint, set to NaN if not being used + * @param pos_yaw [rad] Yaw angle, set to NaN if not being used + * @param vel_yaw [rad/s] Yaw rate, set to NaN if not being used + * @param command Scheduled action for each waypoint, UINT16_MAX if not being used. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t valid_points, const float *pos_x, const float *pos_y, const float *pos_z, const float *vel_x, const float *vel_y, const float *vel_z, const float *acc_x, const float *acc_y, const float *acc_z, const float *pos_yaw, const float *vel_yaw, const uint16_t *command) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 238, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, vel_x, 5); + _mav_put_float_array(buf, 88, vel_y, 5); + _mav_put_float_array(buf, 108, vel_z, 5); + _mav_put_float_array(buf, 128, acc_x, 5); + _mav_put_float_array(buf, 148, acc_y, 5); + _mav_put_float_array(buf, 168, acc_z, 5); + _mav_put_float_array(buf, 188, pos_yaw, 5); + _mav_put_float_array(buf, 208, vel_yaw, 5); + _mav_put_uint16_t_array(buf, 228, command, 5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN); +#else + mavlink_trajectory_representation_waypoints_t packet; + packet.time_usec = time_usec; + packet.valid_points = valid_points; + mav_array_memcpy(packet.pos_x, pos_x, sizeof(float)*5); + mav_array_memcpy(packet.pos_y, pos_y, sizeof(float)*5); + mav_array_memcpy(packet.pos_z, pos_z, sizeof(float)*5); + mav_array_memcpy(packet.vel_x, vel_x, sizeof(float)*5); + mav_array_memcpy(packet.vel_y, vel_y, sizeof(float)*5); + mav_array_memcpy(packet.vel_z, vel_z, sizeof(float)*5); + mav_array_memcpy(packet.acc_x, acc_x, sizeof(float)*5); + mav_array_memcpy(packet.acc_y, acc_y, sizeof(float)*5); + mav_array_memcpy(packet.acc_z, acc_z, sizeof(float)*5); + mav_array_memcpy(packet.pos_yaw, pos_yaw, sizeof(float)*5); + mav_array_memcpy(packet.vel_yaw, vel_yaw, sizeof(float)*5); + mav_array_memcpy(packet.command, command, sizeof(uint16_t)*5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); +} + +/** + * @brief Pack a trajectory_representation_waypoints message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param valid_points Number of valid points (up-to 5 waypoints are possible) + * @param pos_x [m] X-coordinate of waypoint, set to NaN if not being used + * @param pos_y [m] Y-coordinate of waypoint, set to NaN if not being used + * @param pos_z [m] Z-coordinate of waypoint, set to NaN if not being used + * @param vel_x [m/s] X-velocity of waypoint, set to NaN if not being used + * @param vel_y [m/s] Y-velocity of waypoint, set to NaN if not being used + * @param vel_z [m/s] Z-velocity of waypoint, set to NaN if not being used + * @param acc_x [m/s/s] X-acceleration of waypoint, set to NaN if not being used + * @param acc_y [m/s/s] Y-acceleration of waypoint, set to NaN if not being used + * @param acc_z [m/s/s] Z-acceleration of waypoint, set to NaN if not being used + * @param pos_yaw [rad] Yaw angle, set to NaN if not being used + * @param vel_yaw [rad/s] Yaw rate, set to NaN if not being used + * @param command Scheduled action for each waypoint, UINT16_MAX if not being used. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t valid_points,const float *pos_x,const float *pos_y,const float *pos_z,const float *vel_x,const float *vel_y,const float *vel_z,const float *acc_x,const float *acc_y,const float *acc_z,const float *pos_yaw,const float *vel_yaw,const uint16_t *command) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 238, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, vel_x, 5); + _mav_put_float_array(buf, 88, vel_y, 5); + _mav_put_float_array(buf, 108, vel_z, 5); + _mav_put_float_array(buf, 128, acc_x, 5); + _mav_put_float_array(buf, 148, acc_y, 5); + _mav_put_float_array(buf, 168, acc_z, 5); + _mav_put_float_array(buf, 188, pos_yaw, 5); + _mav_put_float_array(buf, 208, vel_yaw, 5); + _mav_put_uint16_t_array(buf, 228, command, 5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN); +#else + mavlink_trajectory_representation_waypoints_t packet; + packet.time_usec = time_usec; + packet.valid_points = valid_points; + mav_array_memcpy(packet.pos_x, pos_x, sizeof(float)*5); + mav_array_memcpy(packet.pos_y, pos_y, sizeof(float)*5); + mav_array_memcpy(packet.pos_z, pos_z, sizeof(float)*5); + mav_array_memcpy(packet.vel_x, vel_x, sizeof(float)*5); + mav_array_memcpy(packet.vel_y, vel_y, sizeof(float)*5); + mav_array_memcpy(packet.vel_z, vel_z, sizeof(float)*5); + mav_array_memcpy(packet.acc_x, acc_x, sizeof(float)*5); + mav_array_memcpy(packet.acc_y, acc_y, sizeof(float)*5); + mav_array_memcpy(packet.acc_z, acc_z, sizeof(float)*5); + mav_array_memcpy(packet.pos_yaw, pos_yaw, sizeof(float)*5); + mav_array_memcpy(packet.vel_yaw, vel_yaw, sizeof(float)*5); + mav_array_memcpy(packet.command, command, sizeof(uint16_t)*5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); +} + +/** + * @brief Encode a trajectory_representation_waypoints struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param trajectory_representation_waypoints C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_trajectory_representation_waypoints_t* trajectory_representation_waypoints) +{ + return mavlink_msg_trajectory_representation_waypoints_pack(system_id, component_id, msg, trajectory_representation_waypoints->time_usec, trajectory_representation_waypoints->valid_points, trajectory_representation_waypoints->pos_x, trajectory_representation_waypoints->pos_y, trajectory_representation_waypoints->pos_z, trajectory_representation_waypoints->vel_x, trajectory_representation_waypoints->vel_y, trajectory_representation_waypoints->vel_z, trajectory_representation_waypoints->acc_x, trajectory_representation_waypoints->acc_y, trajectory_representation_waypoints->acc_z, trajectory_representation_waypoints->pos_yaw, trajectory_representation_waypoints->vel_yaw, trajectory_representation_waypoints->command); +} + +/** + * @brief Encode a trajectory_representation_waypoints struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param trajectory_representation_waypoints C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_trajectory_representation_waypoints_t* trajectory_representation_waypoints) +{ + return mavlink_msg_trajectory_representation_waypoints_pack_chan(system_id, component_id, chan, msg, trajectory_representation_waypoints->time_usec, trajectory_representation_waypoints->valid_points, trajectory_representation_waypoints->pos_x, trajectory_representation_waypoints->pos_y, trajectory_representation_waypoints->pos_z, trajectory_representation_waypoints->vel_x, trajectory_representation_waypoints->vel_y, trajectory_representation_waypoints->vel_z, trajectory_representation_waypoints->acc_x, trajectory_representation_waypoints->acc_y, trajectory_representation_waypoints->acc_z, trajectory_representation_waypoints->pos_yaw, trajectory_representation_waypoints->vel_yaw, trajectory_representation_waypoints->command); +} + +/** + * @brief Send a trajectory_representation_waypoints message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param valid_points Number of valid points (up-to 5 waypoints are possible) + * @param pos_x [m] X-coordinate of waypoint, set to NaN if not being used + * @param pos_y [m] Y-coordinate of waypoint, set to NaN if not being used + * @param pos_z [m] Z-coordinate of waypoint, set to NaN if not being used + * @param vel_x [m/s] X-velocity of waypoint, set to NaN if not being used + * @param vel_y [m/s] Y-velocity of waypoint, set to NaN if not being used + * @param vel_z [m/s] Z-velocity of waypoint, set to NaN if not being used + * @param acc_x [m/s/s] X-acceleration of waypoint, set to NaN if not being used + * @param acc_y [m/s/s] Y-acceleration of waypoint, set to NaN if not being used + * @param acc_z [m/s/s] Z-acceleration of waypoint, set to NaN if not being used + * @param pos_yaw [rad] Yaw angle, set to NaN if not being used + * @param vel_yaw [rad/s] Yaw rate, set to NaN if not being used + * @param command Scheduled action for each waypoint, UINT16_MAX if not being used. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_trajectory_representation_waypoints_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t valid_points, const float *pos_x, const float *pos_y, const float *pos_z, const float *vel_x, const float *vel_y, const float *vel_z, const float *acc_x, const float *acc_y, const float *acc_z, const float *pos_yaw, const float *vel_yaw, const uint16_t *command) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 238, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, vel_x, 5); + _mav_put_float_array(buf, 88, vel_y, 5); + _mav_put_float_array(buf, 108, vel_z, 5); + _mav_put_float_array(buf, 128, acc_x, 5); + _mav_put_float_array(buf, 148, acc_y, 5); + _mav_put_float_array(buf, 168, acc_z, 5); + _mav_put_float_array(buf, 188, pos_yaw, 5); + _mav_put_float_array(buf, 208, vel_yaw, 5); + _mav_put_uint16_t_array(buf, 228, command, 5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS, buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); +#else + mavlink_trajectory_representation_waypoints_t packet; + packet.time_usec = time_usec; + packet.valid_points = valid_points; + mav_array_memcpy(packet.pos_x, pos_x, sizeof(float)*5); + mav_array_memcpy(packet.pos_y, pos_y, sizeof(float)*5); + mav_array_memcpy(packet.pos_z, pos_z, sizeof(float)*5); + mav_array_memcpy(packet.vel_x, vel_x, sizeof(float)*5); + mav_array_memcpy(packet.vel_y, vel_y, sizeof(float)*5); + mav_array_memcpy(packet.vel_z, vel_z, sizeof(float)*5); + mav_array_memcpy(packet.acc_x, acc_x, sizeof(float)*5); + mav_array_memcpy(packet.acc_y, acc_y, sizeof(float)*5); + mav_array_memcpy(packet.acc_z, acc_z, sizeof(float)*5); + mav_array_memcpy(packet.pos_yaw, pos_yaw, sizeof(float)*5); + mav_array_memcpy(packet.vel_yaw, vel_yaw, sizeof(float)*5); + mav_array_memcpy(packet.command, command, sizeof(uint16_t)*5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS, (const char *)&packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); +#endif +} + +/** + * @brief Send a trajectory_representation_waypoints message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_trajectory_representation_waypoints_send_struct(mavlink_channel_t chan, const mavlink_trajectory_representation_waypoints_t* trajectory_representation_waypoints) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_trajectory_representation_waypoints_send(chan, trajectory_representation_waypoints->time_usec, trajectory_representation_waypoints->valid_points, trajectory_representation_waypoints->pos_x, trajectory_representation_waypoints->pos_y, trajectory_representation_waypoints->pos_z, trajectory_representation_waypoints->vel_x, trajectory_representation_waypoints->vel_y, trajectory_representation_waypoints->vel_z, trajectory_representation_waypoints->acc_x, trajectory_representation_waypoints->acc_y, trajectory_representation_waypoints->acc_z, trajectory_representation_waypoints->pos_yaw, trajectory_representation_waypoints->vel_yaw, trajectory_representation_waypoints->command); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS, (const char *)trajectory_representation_waypoints, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_trajectory_representation_waypoints_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t valid_points, const float *pos_x, const float *pos_y, const float *pos_z, const float *vel_x, const float *vel_y, const float *vel_z, const float *acc_x, const float *acc_y, const float *acc_z, const float *pos_yaw, const float *vel_yaw, const uint16_t *command) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 238, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, vel_x, 5); + _mav_put_float_array(buf, 88, vel_y, 5); + _mav_put_float_array(buf, 108, vel_z, 5); + _mav_put_float_array(buf, 128, acc_x, 5); + _mav_put_float_array(buf, 148, acc_y, 5); + _mav_put_float_array(buf, 168, acc_z, 5); + _mav_put_float_array(buf, 188, pos_yaw, 5); + _mav_put_float_array(buf, 208, vel_yaw, 5); + _mav_put_uint16_t_array(buf, 228, command, 5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS, buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); +#else + mavlink_trajectory_representation_waypoints_t *packet = (mavlink_trajectory_representation_waypoints_t *)msgbuf; + packet->time_usec = time_usec; + packet->valid_points = valid_points; + mav_array_memcpy(packet->pos_x, pos_x, sizeof(float)*5); + mav_array_memcpy(packet->pos_y, pos_y, sizeof(float)*5); + mav_array_memcpy(packet->pos_z, pos_z, sizeof(float)*5); + mav_array_memcpy(packet->vel_x, vel_x, sizeof(float)*5); + mav_array_memcpy(packet->vel_y, vel_y, sizeof(float)*5); + mav_array_memcpy(packet->vel_z, vel_z, sizeof(float)*5); + mav_array_memcpy(packet->acc_x, acc_x, sizeof(float)*5); + mav_array_memcpy(packet->acc_y, acc_y, sizeof(float)*5); + mav_array_memcpy(packet->acc_z, acc_z, sizeof(float)*5); + mav_array_memcpy(packet->pos_yaw, pos_yaw, sizeof(float)*5); + mav_array_memcpy(packet->vel_yaw, vel_yaw, sizeof(float)*5); + mav_array_memcpy(packet->command, command, sizeof(uint16_t)*5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS, (const char *)packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE TRAJECTORY_REPRESENTATION_WAYPOINTS UNPACKING + + +/** + * @brief Get field time_usec from trajectory_representation_waypoints message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_trajectory_representation_waypoints_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field valid_points from trajectory_representation_waypoints message + * + * @return Number of valid points (up-to 5 waypoints are possible) + */ +static inline uint8_t mavlink_msg_trajectory_representation_waypoints_get_valid_points(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 238); +} + +/** + * @brief Get field pos_x from trajectory_representation_waypoints message + * + * @return [m] X-coordinate of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_pos_x(const mavlink_message_t* msg, float *pos_x) +{ + return _MAV_RETURN_float_array(msg, pos_x, 5, 8); +} + +/** + * @brief Get field pos_y from trajectory_representation_waypoints message + * + * @return [m] Y-coordinate of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_pos_y(const mavlink_message_t* msg, float *pos_y) +{ + return _MAV_RETURN_float_array(msg, pos_y, 5, 28); +} + +/** + * @brief Get field pos_z from trajectory_representation_waypoints message + * + * @return [m] Z-coordinate of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_pos_z(const mavlink_message_t* msg, float *pos_z) +{ + return _MAV_RETURN_float_array(msg, pos_z, 5, 48); +} + +/** + * @brief Get field vel_x from trajectory_representation_waypoints message + * + * @return [m/s] X-velocity of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_vel_x(const mavlink_message_t* msg, float *vel_x) +{ + return _MAV_RETURN_float_array(msg, vel_x, 5, 68); +} + +/** + * @brief Get field vel_y from trajectory_representation_waypoints message + * + * @return [m/s] Y-velocity of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_vel_y(const mavlink_message_t* msg, float *vel_y) +{ + return _MAV_RETURN_float_array(msg, vel_y, 5, 88); +} + +/** + * @brief Get field vel_z from trajectory_representation_waypoints message + * + * @return [m/s] Z-velocity of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_vel_z(const mavlink_message_t* msg, float *vel_z) +{ + return _MAV_RETURN_float_array(msg, vel_z, 5, 108); +} + +/** + * @brief Get field acc_x from trajectory_representation_waypoints message + * + * @return [m/s/s] X-acceleration of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_acc_x(const mavlink_message_t* msg, float *acc_x) +{ + return _MAV_RETURN_float_array(msg, acc_x, 5, 128); +} + +/** + * @brief Get field acc_y from trajectory_representation_waypoints message + * + * @return [m/s/s] Y-acceleration of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_acc_y(const mavlink_message_t* msg, float *acc_y) +{ + return _MAV_RETURN_float_array(msg, acc_y, 5, 148); +} + +/** + * @brief Get field acc_z from trajectory_representation_waypoints message + * + * @return [m/s/s] Z-acceleration of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_acc_z(const mavlink_message_t* msg, float *acc_z) +{ + return _MAV_RETURN_float_array(msg, acc_z, 5, 168); +} + +/** + * @brief Get field pos_yaw from trajectory_representation_waypoints message + * + * @return [rad] Yaw angle, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_pos_yaw(const mavlink_message_t* msg, float *pos_yaw) +{ + return _MAV_RETURN_float_array(msg, pos_yaw, 5, 188); +} + +/** + * @brief Get field vel_yaw from trajectory_representation_waypoints message + * + * @return [rad/s] Yaw rate, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_vel_yaw(const mavlink_message_t* msg, float *vel_yaw) +{ + return _MAV_RETURN_float_array(msg, vel_yaw, 5, 208); +} + +/** + * @brief Get field command from trajectory_representation_waypoints message + * + * @return Scheduled action for each waypoint, UINT16_MAX if not being used. + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_command(const mavlink_message_t* msg, uint16_t *command) +{ + return _MAV_RETURN_uint16_t_array(msg, command, 5, 228); +} + +/** + * @brief Decode a trajectory_representation_waypoints message into a struct + * + * @param msg The message to decode + * @param trajectory_representation_waypoints C-struct to decode the message contents into + */ +static inline void mavlink_msg_trajectory_representation_waypoints_decode(const mavlink_message_t* msg, mavlink_trajectory_representation_waypoints_t* trajectory_representation_waypoints) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + trajectory_representation_waypoints->time_usec = mavlink_msg_trajectory_representation_waypoints_get_time_usec(msg); + mavlink_msg_trajectory_representation_waypoints_get_pos_x(msg, trajectory_representation_waypoints->pos_x); + mavlink_msg_trajectory_representation_waypoints_get_pos_y(msg, trajectory_representation_waypoints->pos_y); + mavlink_msg_trajectory_representation_waypoints_get_pos_z(msg, trajectory_representation_waypoints->pos_z); + mavlink_msg_trajectory_representation_waypoints_get_vel_x(msg, trajectory_representation_waypoints->vel_x); + mavlink_msg_trajectory_representation_waypoints_get_vel_y(msg, trajectory_representation_waypoints->vel_y); + mavlink_msg_trajectory_representation_waypoints_get_vel_z(msg, trajectory_representation_waypoints->vel_z); + mavlink_msg_trajectory_representation_waypoints_get_acc_x(msg, trajectory_representation_waypoints->acc_x); + mavlink_msg_trajectory_representation_waypoints_get_acc_y(msg, trajectory_representation_waypoints->acc_y); + mavlink_msg_trajectory_representation_waypoints_get_acc_z(msg, trajectory_representation_waypoints->acc_z); + mavlink_msg_trajectory_representation_waypoints_get_pos_yaw(msg, trajectory_representation_waypoints->pos_yaw); + mavlink_msg_trajectory_representation_waypoints_get_vel_yaw(msg, trajectory_representation_waypoints->vel_yaw); + mavlink_msg_trajectory_representation_waypoints_get_command(msg, trajectory_representation_waypoints->command); + trajectory_representation_waypoints->valid_points = mavlink_msg_trajectory_representation_waypoints_get_valid_points(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN? msg->len : MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN; + memset(trajectory_representation_waypoints, 0, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN); + memcpy(trajectory_representation_waypoints, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_tunnel.h b/lib/main/MAVLink/common/mavlink_msg_tunnel.h new file mode 100644 index 0000000000..871315882d --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_tunnel.h @@ -0,0 +1,305 @@ +#pragma once +// MESSAGE TUNNEL PACKING + +#define MAVLINK_MSG_ID_TUNNEL 385 + + +typedef struct __mavlink_tunnel_t { + uint16_t payload_type; /*< A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase.*/ + uint8_t target_system; /*< System ID (can be 0 for broadcast, but this is discouraged)*/ + uint8_t target_component; /*< Component ID (can be 0 for broadcast, but this is discouraged)*/ + uint8_t payload_length; /*< Length of the data transported in payload*/ + uint8_t payload[128]; /*< Variable length payload. The payload length is defined by payload_length. The entire content of this block is opaque unless you understand the encoding specified by payload_type.*/ +} mavlink_tunnel_t; + +#define MAVLINK_MSG_ID_TUNNEL_LEN 133 +#define MAVLINK_MSG_ID_TUNNEL_MIN_LEN 133 +#define MAVLINK_MSG_ID_385_LEN 133 +#define MAVLINK_MSG_ID_385_MIN_LEN 133 + +#define MAVLINK_MSG_ID_TUNNEL_CRC 147 +#define MAVLINK_MSG_ID_385_CRC 147 + +#define MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN 128 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_TUNNEL { \ + 385, \ + "TUNNEL", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_tunnel_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_tunnel_t, target_component) }, \ + { "payload_type", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_tunnel_t, payload_type) }, \ + { "payload_length", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_tunnel_t, payload_length) }, \ + { "payload", NULL, MAVLINK_TYPE_UINT8_T, 128, 5, offsetof(mavlink_tunnel_t, payload) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_TUNNEL { \ + "TUNNEL", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_tunnel_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_tunnel_t, target_component) }, \ + { "payload_type", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_tunnel_t, payload_type) }, \ + { "payload_length", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_tunnel_t, payload_length) }, \ + { "payload", NULL, MAVLINK_TYPE_UINT8_T, 128, 5, offsetof(mavlink_tunnel_t, payload) }, \ + } \ +} +#endif + +/** + * @brief Pack a tunnel message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (can be 0 for broadcast, but this is discouraged) + * @param target_component Component ID (can be 0 for broadcast, but this is discouraged) + * @param payload_type A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + * @param payload_length Length of the data transported in payload + * @param payload Variable length payload. The payload length is defined by payload_length. The entire content of this block is opaque unless you understand the encoding specified by payload_type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_tunnel_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t payload_type, uint8_t payload_length, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TUNNEL_LEN]; + _mav_put_uint16_t(buf, 0, payload_type); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, payload_length); + _mav_put_uint8_t_array(buf, 5, payload, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TUNNEL_LEN); +#else + mavlink_tunnel_t packet; + packet.payload_type = payload_type; + packet.target_system = target_system; + packet.target_component = target_component; + packet.payload_length = payload_length; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TUNNEL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TUNNEL; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); +} + +/** + * @brief Pack a tunnel message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID (can be 0 for broadcast, but this is discouraged) + * @param target_component Component ID (can be 0 for broadcast, but this is discouraged) + * @param payload_type A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + * @param payload_length Length of the data transported in payload + * @param payload Variable length payload. The payload length is defined by payload_length. The entire content of this block is opaque unless you understand the encoding specified by payload_type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_tunnel_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t payload_type,uint8_t payload_length,const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TUNNEL_LEN]; + _mav_put_uint16_t(buf, 0, payload_type); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, payload_length); + _mav_put_uint8_t_array(buf, 5, payload, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TUNNEL_LEN); +#else + mavlink_tunnel_t packet; + packet.payload_type = payload_type; + packet.target_system = target_system; + packet.target_component = target_component; + packet.payload_length = payload_length; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TUNNEL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TUNNEL; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); +} + +/** + * @brief Encode a tunnel struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param tunnel C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_tunnel_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_tunnel_t* tunnel) +{ + return mavlink_msg_tunnel_pack(system_id, component_id, msg, tunnel->target_system, tunnel->target_component, tunnel->payload_type, tunnel->payload_length, tunnel->payload); +} + +/** + * @brief Encode a tunnel struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param tunnel C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_tunnel_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_tunnel_t* tunnel) +{ + return mavlink_msg_tunnel_pack_chan(system_id, component_id, chan, msg, tunnel->target_system, tunnel->target_component, tunnel->payload_type, tunnel->payload_length, tunnel->payload); +} + +/** + * @brief Send a tunnel message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID (can be 0 for broadcast, but this is discouraged) + * @param target_component Component ID (can be 0 for broadcast, but this is discouraged) + * @param payload_type A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + * @param payload_length Length of the data transported in payload + * @param payload Variable length payload. The payload length is defined by payload_length. The entire content of this block is opaque unless you understand the encoding specified by payload_type. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_tunnel_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t payload_type, uint8_t payload_length, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TUNNEL_LEN]; + _mav_put_uint16_t(buf, 0, payload_type); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, payload_length); + _mav_put_uint8_t_array(buf, 5, payload, 128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TUNNEL, buf, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); +#else + mavlink_tunnel_t packet; + packet.payload_type = payload_type; + packet.target_system = target_system; + packet.target_component = target_component; + packet.payload_length = payload_length; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TUNNEL, (const char *)&packet, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); +#endif +} + +/** + * @brief Send a tunnel message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_tunnel_send_struct(mavlink_channel_t chan, const mavlink_tunnel_t* tunnel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_tunnel_send(chan, tunnel->target_system, tunnel->target_component, tunnel->payload_type, tunnel->payload_length, tunnel->payload); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TUNNEL, (const char *)tunnel, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); +#endif +} + +#if MAVLINK_MSG_ID_TUNNEL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_tunnel_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t payload_type, uint8_t payload_length, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, payload_type); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, payload_length); + _mav_put_uint8_t_array(buf, 5, payload, 128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TUNNEL, buf, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); +#else + mavlink_tunnel_t *packet = (mavlink_tunnel_t *)msgbuf; + packet->payload_type = payload_type; + packet->target_system = target_system; + packet->target_component = target_component; + packet->payload_length = payload_length; + mav_array_memcpy(packet->payload, payload, sizeof(uint8_t)*128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TUNNEL, (const char *)packet, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); +#endif +} +#endif + +#endif + +// MESSAGE TUNNEL UNPACKING + + +/** + * @brief Get field target_system from tunnel message + * + * @return System ID (can be 0 for broadcast, but this is discouraged) + */ +static inline uint8_t mavlink_msg_tunnel_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field target_component from tunnel message + * + * @return Component ID (can be 0 for broadcast, but this is discouraged) + */ +static inline uint8_t mavlink_msg_tunnel_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field payload_type from tunnel message + * + * @return A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + */ +static inline uint16_t mavlink_msg_tunnel_get_payload_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field payload_length from tunnel message + * + * @return Length of the data transported in payload + */ +static inline uint8_t mavlink_msg_tunnel_get_payload_length(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field payload from tunnel message + * + * @return Variable length payload. The payload length is defined by payload_length. The entire content of this block is opaque unless you understand the encoding specified by payload_type. + */ +static inline uint16_t mavlink_msg_tunnel_get_payload(const mavlink_message_t* msg, uint8_t *payload) +{ + return _MAV_RETURN_uint8_t_array(msg, payload, 128, 5); +} + +/** + * @brief Decode a tunnel message into a struct + * + * @param msg The message to decode + * @param tunnel C-struct to decode the message contents into + */ +static inline void mavlink_msg_tunnel_decode(const mavlink_message_t* msg, mavlink_tunnel_t* tunnel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + tunnel->payload_type = mavlink_msg_tunnel_get_payload_type(msg); + tunnel->target_system = mavlink_msg_tunnel_get_target_system(msg); + tunnel->target_component = mavlink_msg_tunnel_get_target_component(msg); + tunnel->payload_length = mavlink_msg_tunnel_get_payload_length(msg); + mavlink_msg_tunnel_get_payload(msg, tunnel->payload); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_TUNNEL_LEN? msg->len : MAVLINK_MSG_ID_TUNNEL_LEN; + memset(tunnel, 0, MAVLINK_MSG_ID_TUNNEL_LEN); + memcpy(tunnel, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_uavcan_node_info.h b/lib/main/MAVLink/common/mavlink_msg_uavcan_node_info.h new file mode 100644 index 0000000000..a042c5c4e5 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_uavcan_node_info.h @@ -0,0 +1,406 @@ +#pragma once +// MESSAGE UAVCAN_NODE_INFO PACKING + +#define MAVLINK_MSG_ID_UAVCAN_NODE_INFO 311 + + +typedef struct __mavlink_uavcan_node_info_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + uint32_t uptime_sec; /*< [s] Time since the start-up of the node.*/ + uint32_t sw_vcs_commit; /*< Version control system (VCS) revision identifier (e.g. git short commit hash). Zero if unknown.*/ + char name[80]; /*< Node name string. For example, "sapog.px4.io".*/ + uint8_t hw_version_major; /*< Hardware major version number.*/ + uint8_t hw_version_minor; /*< Hardware minor version number.*/ + uint8_t hw_unique_id[16]; /*< Hardware unique 128-bit ID.*/ + uint8_t sw_version_major; /*< Software major version number.*/ + uint8_t sw_version_minor; /*< Software minor version number.*/ +} mavlink_uavcan_node_info_t; + +#define MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN 116 +#define MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN 116 +#define MAVLINK_MSG_ID_311_LEN 116 +#define MAVLINK_MSG_ID_311_MIN_LEN 116 + +#define MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC 95 +#define MAVLINK_MSG_ID_311_CRC 95 + +#define MAVLINK_MSG_UAVCAN_NODE_INFO_FIELD_NAME_LEN 80 +#define MAVLINK_MSG_UAVCAN_NODE_INFO_FIELD_HW_UNIQUE_ID_LEN 16 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_UAVCAN_NODE_INFO { \ + 311, \ + "UAVCAN_NODE_INFO", \ + 9, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_uavcan_node_info_t, time_usec) }, \ + { "uptime_sec", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_uavcan_node_info_t, uptime_sec) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 80, 16, offsetof(mavlink_uavcan_node_info_t, name) }, \ + { "hw_version_major", NULL, MAVLINK_TYPE_UINT8_T, 0, 96, offsetof(mavlink_uavcan_node_info_t, hw_version_major) }, \ + { "hw_version_minor", NULL, MAVLINK_TYPE_UINT8_T, 0, 97, offsetof(mavlink_uavcan_node_info_t, hw_version_minor) }, \ + { "hw_unique_id", NULL, MAVLINK_TYPE_UINT8_T, 16, 98, offsetof(mavlink_uavcan_node_info_t, hw_unique_id) }, \ + { "sw_version_major", NULL, MAVLINK_TYPE_UINT8_T, 0, 114, offsetof(mavlink_uavcan_node_info_t, sw_version_major) }, \ + { "sw_version_minor", NULL, MAVLINK_TYPE_UINT8_T, 0, 115, offsetof(mavlink_uavcan_node_info_t, sw_version_minor) }, \ + { "sw_vcs_commit", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_uavcan_node_info_t, sw_vcs_commit) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_UAVCAN_NODE_INFO { \ + "UAVCAN_NODE_INFO", \ + 9, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_uavcan_node_info_t, time_usec) }, \ + { "uptime_sec", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_uavcan_node_info_t, uptime_sec) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 80, 16, offsetof(mavlink_uavcan_node_info_t, name) }, \ + { "hw_version_major", NULL, MAVLINK_TYPE_UINT8_T, 0, 96, offsetof(mavlink_uavcan_node_info_t, hw_version_major) }, \ + { "hw_version_minor", NULL, MAVLINK_TYPE_UINT8_T, 0, 97, offsetof(mavlink_uavcan_node_info_t, hw_version_minor) }, \ + { "hw_unique_id", NULL, MAVLINK_TYPE_UINT8_T, 16, 98, offsetof(mavlink_uavcan_node_info_t, hw_unique_id) }, \ + { "sw_version_major", NULL, MAVLINK_TYPE_UINT8_T, 0, 114, offsetof(mavlink_uavcan_node_info_t, sw_version_major) }, \ + { "sw_version_minor", NULL, MAVLINK_TYPE_UINT8_T, 0, 115, offsetof(mavlink_uavcan_node_info_t, sw_version_minor) }, \ + { "sw_vcs_commit", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_uavcan_node_info_t, sw_vcs_commit) }, \ + } \ +} +#endif + +/** + * @brief Pack a uavcan_node_info message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime_sec [s] Time since the start-up of the node. + * @param name Node name string. For example, "sapog.px4.io". + * @param hw_version_major Hardware major version number. + * @param hw_version_minor Hardware minor version number. + * @param hw_unique_id Hardware unique 128-bit ID. + * @param sw_version_major Software major version number. + * @param sw_version_minor Software minor version number. + * @param sw_vcs_commit Version control system (VCS) revision identifier (e.g. git short commit hash). Zero if unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_uavcan_node_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint32_t uptime_sec, const char *name, uint8_t hw_version_major, uint8_t hw_version_minor, const uint8_t *hw_unique_id, uint8_t sw_version_major, uint8_t sw_version_minor, uint32_t sw_vcs_commit) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint32_t(buf, 12, sw_vcs_commit); + _mav_put_uint8_t(buf, 96, hw_version_major); + _mav_put_uint8_t(buf, 97, hw_version_minor); + _mav_put_uint8_t(buf, 114, sw_version_major); + _mav_put_uint8_t(buf, 115, sw_version_minor); + _mav_put_char_array(buf, 16, name, 80); + _mav_put_uint8_t_array(buf, 98, hw_unique_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN); +#else + mavlink_uavcan_node_info_t packet; + packet.time_usec = time_usec; + packet.uptime_sec = uptime_sec; + packet.sw_vcs_commit = sw_vcs_commit; + packet.hw_version_major = hw_version_major; + packet.hw_version_minor = hw_version_minor; + packet.sw_version_major = sw_version_major; + packet.sw_version_minor = sw_version_minor; + mav_array_memcpy(packet.name, name, sizeof(char)*80); + mav_array_memcpy(packet.hw_unique_id, hw_unique_id, sizeof(uint8_t)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_UAVCAN_NODE_INFO; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); +} + +/** + * @brief Pack a uavcan_node_info message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime_sec [s] Time since the start-up of the node. + * @param name Node name string. For example, "sapog.px4.io". + * @param hw_version_major Hardware major version number. + * @param hw_version_minor Hardware minor version number. + * @param hw_unique_id Hardware unique 128-bit ID. + * @param sw_version_major Software major version number. + * @param sw_version_minor Software minor version number. + * @param sw_vcs_commit Version control system (VCS) revision identifier (e.g. git short commit hash). Zero if unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_uavcan_node_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint32_t uptime_sec,const char *name,uint8_t hw_version_major,uint8_t hw_version_minor,const uint8_t *hw_unique_id,uint8_t sw_version_major,uint8_t sw_version_minor,uint32_t sw_vcs_commit) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint32_t(buf, 12, sw_vcs_commit); + _mav_put_uint8_t(buf, 96, hw_version_major); + _mav_put_uint8_t(buf, 97, hw_version_minor); + _mav_put_uint8_t(buf, 114, sw_version_major); + _mav_put_uint8_t(buf, 115, sw_version_minor); + _mav_put_char_array(buf, 16, name, 80); + _mav_put_uint8_t_array(buf, 98, hw_unique_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN); +#else + mavlink_uavcan_node_info_t packet; + packet.time_usec = time_usec; + packet.uptime_sec = uptime_sec; + packet.sw_vcs_commit = sw_vcs_commit; + packet.hw_version_major = hw_version_major; + packet.hw_version_minor = hw_version_minor; + packet.sw_version_major = sw_version_major; + packet.sw_version_minor = sw_version_minor; + mav_array_memcpy(packet.name, name, sizeof(char)*80); + mav_array_memcpy(packet.hw_unique_id, hw_unique_id, sizeof(uint8_t)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_UAVCAN_NODE_INFO; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); +} + +/** + * @brief Encode a uavcan_node_info struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param uavcan_node_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_uavcan_node_info_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_uavcan_node_info_t* uavcan_node_info) +{ + return mavlink_msg_uavcan_node_info_pack(system_id, component_id, msg, uavcan_node_info->time_usec, uavcan_node_info->uptime_sec, uavcan_node_info->name, uavcan_node_info->hw_version_major, uavcan_node_info->hw_version_minor, uavcan_node_info->hw_unique_id, uavcan_node_info->sw_version_major, uavcan_node_info->sw_version_minor, uavcan_node_info->sw_vcs_commit); +} + +/** + * @brief Encode a uavcan_node_info struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param uavcan_node_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_uavcan_node_info_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_uavcan_node_info_t* uavcan_node_info) +{ + return mavlink_msg_uavcan_node_info_pack_chan(system_id, component_id, chan, msg, uavcan_node_info->time_usec, uavcan_node_info->uptime_sec, uavcan_node_info->name, uavcan_node_info->hw_version_major, uavcan_node_info->hw_version_minor, uavcan_node_info->hw_unique_id, uavcan_node_info->sw_version_major, uavcan_node_info->sw_version_minor, uavcan_node_info->sw_vcs_commit); +} + +/** + * @brief Send a uavcan_node_info message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime_sec [s] Time since the start-up of the node. + * @param name Node name string. For example, "sapog.px4.io". + * @param hw_version_major Hardware major version number. + * @param hw_version_minor Hardware minor version number. + * @param hw_unique_id Hardware unique 128-bit ID. + * @param sw_version_major Software major version number. + * @param sw_version_minor Software minor version number. + * @param sw_vcs_commit Version control system (VCS) revision identifier (e.g. git short commit hash). Zero if unknown. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_uavcan_node_info_send(mavlink_channel_t chan, uint64_t time_usec, uint32_t uptime_sec, const char *name, uint8_t hw_version_major, uint8_t hw_version_minor, const uint8_t *hw_unique_id, uint8_t sw_version_major, uint8_t sw_version_minor, uint32_t sw_vcs_commit) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint32_t(buf, 12, sw_vcs_commit); + _mav_put_uint8_t(buf, 96, hw_version_major); + _mav_put_uint8_t(buf, 97, hw_version_minor); + _mav_put_uint8_t(buf, 114, sw_version_major); + _mav_put_uint8_t(buf, 115, sw_version_minor); + _mav_put_char_array(buf, 16, name, 80); + _mav_put_uint8_t_array(buf, 98, hw_unique_id, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_INFO, buf, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); +#else + mavlink_uavcan_node_info_t packet; + packet.time_usec = time_usec; + packet.uptime_sec = uptime_sec; + packet.sw_vcs_commit = sw_vcs_commit; + packet.hw_version_major = hw_version_major; + packet.hw_version_minor = hw_version_minor; + packet.sw_version_major = sw_version_major; + packet.sw_version_minor = sw_version_minor; + mav_array_memcpy(packet.name, name, sizeof(char)*80); + mav_array_memcpy(packet.hw_unique_id, hw_unique_id, sizeof(uint8_t)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_INFO, (const char *)&packet, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); +#endif +} + +/** + * @brief Send a uavcan_node_info message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_uavcan_node_info_send_struct(mavlink_channel_t chan, const mavlink_uavcan_node_info_t* uavcan_node_info) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_uavcan_node_info_send(chan, uavcan_node_info->time_usec, uavcan_node_info->uptime_sec, uavcan_node_info->name, uavcan_node_info->hw_version_major, uavcan_node_info->hw_version_minor, uavcan_node_info->hw_unique_id, uavcan_node_info->sw_version_major, uavcan_node_info->sw_version_minor, uavcan_node_info->sw_vcs_commit); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_INFO, (const char *)uavcan_node_info, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); +#endif +} + +#if MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_uavcan_node_info_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint32_t uptime_sec, const char *name, uint8_t hw_version_major, uint8_t hw_version_minor, const uint8_t *hw_unique_id, uint8_t sw_version_major, uint8_t sw_version_minor, uint32_t sw_vcs_commit) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint32_t(buf, 12, sw_vcs_commit); + _mav_put_uint8_t(buf, 96, hw_version_major); + _mav_put_uint8_t(buf, 97, hw_version_minor); + _mav_put_uint8_t(buf, 114, sw_version_major); + _mav_put_uint8_t(buf, 115, sw_version_minor); + _mav_put_char_array(buf, 16, name, 80); + _mav_put_uint8_t_array(buf, 98, hw_unique_id, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_INFO, buf, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); +#else + mavlink_uavcan_node_info_t *packet = (mavlink_uavcan_node_info_t *)msgbuf; + packet->time_usec = time_usec; + packet->uptime_sec = uptime_sec; + packet->sw_vcs_commit = sw_vcs_commit; + packet->hw_version_major = hw_version_major; + packet->hw_version_minor = hw_version_minor; + packet->sw_version_major = sw_version_major; + packet->sw_version_minor = sw_version_minor; + mav_array_memcpy(packet->name, name, sizeof(char)*80); + mav_array_memcpy(packet->hw_unique_id, hw_unique_id, sizeof(uint8_t)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_INFO, (const char *)packet, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); +#endif +} +#endif + +#endif + +// MESSAGE UAVCAN_NODE_INFO UNPACKING + + +/** + * @brief Get field time_usec from uavcan_node_info message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_uavcan_node_info_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field uptime_sec from uavcan_node_info message + * + * @return [s] Time since the start-up of the node. + */ +static inline uint32_t mavlink_msg_uavcan_node_info_get_uptime_sec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field name from uavcan_node_info message + * + * @return Node name string. For example, "sapog.px4.io". + */ +static inline uint16_t mavlink_msg_uavcan_node_info_get_name(const mavlink_message_t* msg, char *name) +{ + return _MAV_RETURN_char_array(msg, name, 80, 16); +} + +/** + * @brief Get field hw_version_major from uavcan_node_info message + * + * @return Hardware major version number. + */ +static inline uint8_t mavlink_msg_uavcan_node_info_get_hw_version_major(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 96); +} + +/** + * @brief Get field hw_version_minor from uavcan_node_info message + * + * @return Hardware minor version number. + */ +static inline uint8_t mavlink_msg_uavcan_node_info_get_hw_version_minor(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 97); +} + +/** + * @brief Get field hw_unique_id from uavcan_node_info message + * + * @return Hardware unique 128-bit ID. + */ +static inline uint16_t mavlink_msg_uavcan_node_info_get_hw_unique_id(const mavlink_message_t* msg, uint8_t *hw_unique_id) +{ + return _MAV_RETURN_uint8_t_array(msg, hw_unique_id, 16, 98); +} + +/** + * @brief Get field sw_version_major from uavcan_node_info message + * + * @return Software major version number. + */ +static inline uint8_t mavlink_msg_uavcan_node_info_get_sw_version_major(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 114); +} + +/** + * @brief Get field sw_version_minor from uavcan_node_info message + * + * @return Software minor version number. + */ +static inline uint8_t mavlink_msg_uavcan_node_info_get_sw_version_minor(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 115); +} + +/** + * @brief Get field sw_vcs_commit from uavcan_node_info message + * + * @return Version control system (VCS) revision identifier (e.g. git short commit hash). Zero if unknown. + */ +static inline uint32_t mavlink_msg_uavcan_node_info_get_sw_vcs_commit(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 12); +} + +/** + * @brief Decode a uavcan_node_info message into a struct + * + * @param msg The message to decode + * @param uavcan_node_info C-struct to decode the message contents into + */ +static inline void mavlink_msg_uavcan_node_info_decode(const mavlink_message_t* msg, mavlink_uavcan_node_info_t* uavcan_node_info) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + uavcan_node_info->time_usec = mavlink_msg_uavcan_node_info_get_time_usec(msg); + uavcan_node_info->uptime_sec = mavlink_msg_uavcan_node_info_get_uptime_sec(msg); + uavcan_node_info->sw_vcs_commit = mavlink_msg_uavcan_node_info_get_sw_vcs_commit(msg); + mavlink_msg_uavcan_node_info_get_name(msg, uavcan_node_info->name); + uavcan_node_info->hw_version_major = mavlink_msg_uavcan_node_info_get_hw_version_major(msg); + uavcan_node_info->hw_version_minor = mavlink_msg_uavcan_node_info_get_hw_version_minor(msg); + mavlink_msg_uavcan_node_info_get_hw_unique_id(msg, uavcan_node_info->hw_unique_id); + uavcan_node_info->sw_version_major = mavlink_msg_uavcan_node_info_get_sw_version_major(msg); + uavcan_node_info->sw_version_minor = mavlink_msg_uavcan_node_info_get_sw_version_minor(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN? msg->len : MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN; + memset(uavcan_node_info, 0, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN); + memcpy(uavcan_node_info, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_uavcan_node_status.h b/lib/main/MAVLink/common/mavlink_msg_uavcan_node_status.h new file mode 100644 index 0000000000..6f1d8d8fd3 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_uavcan_node_status.h @@ -0,0 +1,338 @@ +#pragma once +// MESSAGE UAVCAN_NODE_STATUS PACKING + +#define MAVLINK_MSG_ID_UAVCAN_NODE_STATUS 310 + + +typedef struct __mavlink_uavcan_node_status_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + uint32_t uptime_sec; /*< [s] Time since the start-up of the node.*/ + uint16_t vendor_specific_status_code; /*< Vendor-specific status information.*/ + uint8_t health; /*< Generalized node health status.*/ + uint8_t mode; /*< Generalized operating mode.*/ + uint8_t sub_mode; /*< Not used currently.*/ +} mavlink_uavcan_node_status_t; + +#define MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN 17 +#define MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN 17 +#define MAVLINK_MSG_ID_310_LEN 17 +#define MAVLINK_MSG_ID_310_MIN_LEN 17 + +#define MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC 28 +#define MAVLINK_MSG_ID_310_CRC 28 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_UAVCAN_NODE_STATUS { \ + 310, \ + "UAVCAN_NODE_STATUS", \ + 6, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_uavcan_node_status_t, time_usec) }, \ + { "uptime_sec", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_uavcan_node_status_t, uptime_sec) }, \ + { "health", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_uavcan_node_status_t, health) }, \ + { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_uavcan_node_status_t, mode) }, \ + { "sub_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_uavcan_node_status_t, sub_mode) }, \ + { "vendor_specific_status_code", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_uavcan_node_status_t, vendor_specific_status_code) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_UAVCAN_NODE_STATUS { \ + "UAVCAN_NODE_STATUS", \ + 6, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_uavcan_node_status_t, time_usec) }, \ + { "uptime_sec", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_uavcan_node_status_t, uptime_sec) }, \ + { "health", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_uavcan_node_status_t, health) }, \ + { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_uavcan_node_status_t, mode) }, \ + { "sub_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_uavcan_node_status_t, sub_mode) }, \ + { "vendor_specific_status_code", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_uavcan_node_status_t, vendor_specific_status_code) }, \ + } \ +} +#endif + +/** + * @brief Pack a uavcan_node_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime_sec [s] Time since the start-up of the node. + * @param health Generalized node health status. + * @param mode Generalized operating mode. + * @param sub_mode Not used currently. + * @param vendor_specific_status_code Vendor-specific status information. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_uavcan_node_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint32_t uptime_sec, uint8_t health, uint8_t mode, uint8_t sub_mode, uint16_t vendor_specific_status_code) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint16_t(buf, 12, vendor_specific_status_code); + _mav_put_uint8_t(buf, 14, health); + _mav_put_uint8_t(buf, 15, mode); + _mav_put_uint8_t(buf, 16, sub_mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN); +#else + mavlink_uavcan_node_status_t packet; + packet.time_usec = time_usec; + packet.uptime_sec = uptime_sec; + packet.vendor_specific_status_code = vendor_specific_status_code; + packet.health = health; + packet.mode = mode; + packet.sub_mode = sub_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_UAVCAN_NODE_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC); +} + +/** + * @brief Pack a uavcan_node_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime_sec [s] Time since the start-up of the node. + * @param health Generalized node health status. + * @param mode Generalized operating mode. + * @param sub_mode Not used currently. + * @param vendor_specific_status_code Vendor-specific status information. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_uavcan_node_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint32_t uptime_sec,uint8_t health,uint8_t mode,uint8_t sub_mode,uint16_t vendor_specific_status_code) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint16_t(buf, 12, vendor_specific_status_code); + _mav_put_uint8_t(buf, 14, health); + _mav_put_uint8_t(buf, 15, mode); + _mav_put_uint8_t(buf, 16, sub_mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN); +#else + mavlink_uavcan_node_status_t packet; + packet.time_usec = time_usec; + packet.uptime_sec = uptime_sec; + packet.vendor_specific_status_code = vendor_specific_status_code; + packet.health = health; + packet.mode = mode; + packet.sub_mode = sub_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_UAVCAN_NODE_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC); +} + +/** + * @brief Encode a uavcan_node_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param uavcan_node_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_uavcan_node_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_uavcan_node_status_t* uavcan_node_status) +{ + return mavlink_msg_uavcan_node_status_pack(system_id, component_id, msg, uavcan_node_status->time_usec, uavcan_node_status->uptime_sec, uavcan_node_status->health, uavcan_node_status->mode, uavcan_node_status->sub_mode, uavcan_node_status->vendor_specific_status_code); +} + +/** + * @brief Encode a uavcan_node_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param uavcan_node_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_uavcan_node_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_uavcan_node_status_t* uavcan_node_status) +{ + return mavlink_msg_uavcan_node_status_pack_chan(system_id, component_id, chan, msg, uavcan_node_status->time_usec, uavcan_node_status->uptime_sec, uavcan_node_status->health, uavcan_node_status->mode, uavcan_node_status->sub_mode, uavcan_node_status->vendor_specific_status_code); +} + +/** + * @brief Send a uavcan_node_status message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime_sec [s] Time since the start-up of the node. + * @param health Generalized node health status. + * @param mode Generalized operating mode. + * @param sub_mode Not used currently. + * @param vendor_specific_status_code Vendor-specific status information. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_uavcan_node_status_send(mavlink_channel_t chan, uint64_t time_usec, uint32_t uptime_sec, uint8_t health, uint8_t mode, uint8_t sub_mode, uint16_t vendor_specific_status_code) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint16_t(buf, 12, vendor_specific_status_code); + _mav_put_uint8_t(buf, 14, health); + _mav_put_uint8_t(buf, 15, mode); + _mav_put_uint8_t(buf, 16, sub_mode); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS, buf, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC); +#else + mavlink_uavcan_node_status_t packet; + packet.time_usec = time_usec; + packet.uptime_sec = uptime_sec; + packet.vendor_specific_status_code = vendor_specific_status_code; + packet.health = health; + packet.mode = mode; + packet.sub_mode = sub_mode; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC); +#endif +} + +/** + * @brief Send a uavcan_node_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_uavcan_node_status_send_struct(mavlink_channel_t chan, const mavlink_uavcan_node_status_t* uavcan_node_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_uavcan_node_status_send(chan, uavcan_node_status->time_usec, uavcan_node_status->uptime_sec, uavcan_node_status->health, uavcan_node_status->mode, uavcan_node_status->sub_mode, uavcan_node_status->vendor_specific_status_code); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS, (const char *)uavcan_node_status, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_uavcan_node_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint32_t uptime_sec, uint8_t health, uint8_t mode, uint8_t sub_mode, uint16_t vendor_specific_status_code) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint16_t(buf, 12, vendor_specific_status_code); + _mav_put_uint8_t(buf, 14, health); + _mav_put_uint8_t(buf, 15, mode); + _mav_put_uint8_t(buf, 16, sub_mode); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS, buf, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC); +#else + mavlink_uavcan_node_status_t *packet = (mavlink_uavcan_node_status_t *)msgbuf; + packet->time_usec = time_usec; + packet->uptime_sec = uptime_sec; + packet->vendor_specific_status_code = vendor_specific_status_code; + packet->health = health; + packet->mode = mode; + packet->sub_mode = sub_mode; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS, (const char *)packet, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE UAVCAN_NODE_STATUS UNPACKING + + +/** + * @brief Get field time_usec from uavcan_node_status message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_uavcan_node_status_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field uptime_sec from uavcan_node_status message + * + * @return [s] Time since the start-up of the node. + */ +static inline uint32_t mavlink_msg_uavcan_node_status_get_uptime_sec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field health from uavcan_node_status message + * + * @return Generalized node health status. + */ +static inline uint8_t mavlink_msg_uavcan_node_status_get_health(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 14); +} + +/** + * @brief Get field mode from uavcan_node_status message + * + * @return Generalized operating mode. + */ +static inline uint8_t mavlink_msg_uavcan_node_status_get_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 15); +} + +/** + * @brief Get field sub_mode from uavcan_node_status message + * + * @return Not used currently. + */ +static inline uint8_t mavlink_msg_uavcan_node_status_get_sub_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 16); +} + +/** + * @brief Get field vendor_specific_status_code from uavcan_node_status message + * + * @return Vendor-specific status information. + */ +static inline uint16_t mavlink_msg_uavcan_node_status_get_vendor_specific_status_code(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Decode a uavcan_node_status message into a struct + * + * @param msg The message to decode + * @param uavcan_node_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_uavcan_node_status_decode(const mavlink_message_t* msg, mavlink_uavcan_node_status_t* uavcan_node_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + uavcan_node_status->time_usec = mavlink_msg_uavcan_node_status_get_time_usec(msg); + uavcan_node_status->uptime_sec = mavlink_msg_uavcan_node_status_get_uptime_sec(msg); + uavcan_node_status->vendor_specific_status_code = mavlink_msg_uavcan_node_status_get_vendor_specific_status_code(msg); + uavcan_node_status->health = mavlink_msg_uavcan_node_status_get_health(msg); + uavcan_node_status->mode = mavlink_msg_uavcan_node_status_get_mode(msg); + uavcan_node_status->sub_mode = mavlink_msg_uavcan_node_status_get_sub_mode(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN? msg->len : MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN; + memset(uavcan_node_status, 0, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN); + memcpy(uavcan_node_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_utm_global_position.h b/lib/main/MAVLink/common/mavlink_msg_utm_global_position.h new file mode 100644 index 0000000000..0c8ee72263 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_utm_global_position.h @@ -0,0 +1,630 @@ +#pragma once +// MESSAGE UTM_GLOBAL_POSITION PACKING + +#define MAVLINK_MSG_ID_UTM_GLOBAL_POSITION 340 + + +typedef struct __mavlink_utm_global_position_t { + uint64_t time; /*< [us] Time of applicability of position (microseconds since UNIX epoch).*/ + int32_t lat; /*< [degE7] Latitude (WGS84)*/ + int32_t lon; /*< [degE7] Longitude (WGS84)*/ + int32_t alt; /*< [mm] Altitude (WGS84)*/ + int32_t relative_alt; /*< [mm] Altitude above ground*/ + int32_t next_lat; /*< [degE7] Next waypoint, latitude (WGS84)*/ + int32_t next_lon; /*< [degE7] Next waypoint, longitude (WGS84)*/ + int32_t next_alt; /*< [mm] Next waypoint, altitude (WGS84)*/ + int16_t vx; /*< [cm/s] Ground X speed (latitude, positive north)*/ + int16_t vy; /*< [cm/s] Ground Y speed (longitude, positive east)*/ + int16_t vz; /*< [cm/s] Ground Z speed (altitude, positive down)*/ + uint16_t h_acc; /*< [mm] Horizontal position uncertainty (standard deviation)*/ + uint16_t v_acc; /*< [mm] Altitude uncertainty (standard deviation)*/ + uint16_t vel_acc; /*< [cm/s] Speed uncertainty (standard deviation)*/ + uint16_t update_rate; /*< [cs] Time until next update. Set to 0 if unknown or in data driven mode.*/ + uint8_t uas_id[18]; /*< Unique UAS ID.*/ + uint8_t flight_state; /*< Flight state*/ + uint8_t flags; /*< Bitwise OR combination of the data available flags.*/ +} mavlink_utm_global_position_t; + +#define MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN 70 +#define MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN 70 +#define MAVLINK_MSG_ID_340_LEN 70 +#define MAVLINK_MSG_ID_340_MIN_LEN 70 + +#define MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC 99 +#define MAVLINK_MSG_ID_340_CRC 99 + +#define MAVLINK_MSG_UTM_GLOBAL_POSITION_FIELD_UAS_ID_LEN 18 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_UTM_GLOBAL_POSITION { \ + 340, \ + "UTM_GLOBAL_POSITION", \ + 18, \ + { { "time", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_utm_global_position_t, time) }, \ + { "uas_id", NULL, MAVLINK_TYPE_UINT8_T, 18, 50, offsetof(mavlink_utm_global_position_t, uas_id) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_utm_global_position_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_utm_global_position_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_utm_global_position_t, alt) }, \ + { "relative_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_utm_global_position_t, relative_alt) }, \ + { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_utm_global_position_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_utm_global_position_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_utm_global_position_t, vz) }, \ + { "h_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 42, offsetof(mavlink_utm_global_position_t, h_acc) }, \ + { "v_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 44, offsetof(mavlink_utm_global_position_t, v_acc) }, \ + { "vel_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 46, offsetof(mavlink_utm_global_position_t, vel_acc) }, \ + { "next_lat", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_utm_global_position_t, next_lat) }, \ + { "next_lon", NULL, MAVLINK_TYPE_INT32_T, 0, 28, offsetof(mavlink_utm_global_position_t, next_lon) }, \ + { "next_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_utm_global_position_t, next_alt) }, \ + { "update_rate", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_utm_global_position_t, update_rate) }, \ + { "flight_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 68, offsetof(mavlink_utm_global_position_t, flight_state) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 69, offsetof(mavlink_utm_global_position_t, flags) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_UTM_GLOBAL_POSITION { \ + "UTM_GLOBAL_POSITION", \ + 18, \ + { { "time", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_utm_global_position_t, time) }, \ + { "uas_id", NULL, MAVLINK_TYPE_UINT8_T, 18, 50, offsetof(mavlink_utm_global_position_t, uas_id) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_utm_global_position_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_utm_global_position_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_utm_global_position_t, alt) }, \ + { "relative_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_utm_global_position_t, relative_alt) }, \ + { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_utm_global_position_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_utm_global_position_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_utm_global_position_t, vz) }, \ + { "h_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 42, offsetof(mavlink_utm_global_position_t, h_acc) }, \ + { "v_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 44, offsetof(mavlink_utm_global_position_t, v_acc) }, \ + { "vel_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 46, offsetof(mavlink_utm_global_position_t, vel_acc) }, \ + { "next_lat", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_utm_global_position_t, next_lat) }, \ + { "next_lon", NULL, MAVLINK_TYPE_INT32_T, 0, 28, offsetof(mavlink_utm_global_position_t, next_lon) }, \ + { "next_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_utm_global_position_t, next_alt) }, \ + { "update_rate", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_utm_global_position_t, update_rate) }, \ + { "flight_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 68, offsetof(mavlink_utm_global_position_t, flight_state) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 69, offsetof(mavlink_utm_global_position_t, flags) }, \ + } \ +} +#endif + +/** + * @brief Pack a utm_global_position message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time [us] Time of applicability of position (microseconds since UNIX epoch). + * @param uas_id Unique UAS ID. + * @param lat [degE7] Latitude (WGS84) + * @param lon [degE7] Longitude (WGS84) + * @param alt [mm] Altitude (WGS84) + * @param relative_alt [mm] Altitude above ground + * @param vx [cm/s] Ground X speed (latitude, positive north) + * @param vy [cm/s] Ground Y speed (longitude, positive east) + * @param vz [cm/s] Ground Z speed (altitude, positive down) + * @param h_acc [mm] Horizontal position uncertainty (standard deviation) + * @param v_acc [mm] Altitude uncertainty (standard deviation) + * @param vel_acc [cm/s] Speed uncertainty (standard deviation) + * @param next_lat [degE7] Next waypoint, latitude (WGS84) + * @param next_lon [degE7] Next waypoint, longitude (WGS84) + * @param next_alt [mm] Next waypoint, altitude (WGS84) + * @param update_rate [cs] Time until next update. Set to 0 if unknown or in data driven mode. + * @param flight_state Flight state + * @param flags Bitwise OR combination of the data available flags. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_utm_global_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time, const uint8_t *uas_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t h_acc, uint16_t v_acc, uint16_t vel_acc, int32_t next_lat, int32_t next_lon, int32_t next_alt, uint16_t update_rate, uint8_t flight_state, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN]; + _mav_put_uint64_t(buf, 0, time); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_int32_t(buf, 20, relative_alt); + _mav_put_int32_t(buf, 24, next_lat); + _mav_put_int32_t(buf, 28, next_lon); + _mav_put_int32_t(buf, 32, next_alt); + _mav_put_int16_t(buf, 36, vx); + _mav_put_int16_t(buf, 38, vy); + _mav_put_int16_t(buf, 40, vz); + _mav_put_uint16_t(buf, 42, h_acc); + _mav_put_uint16_t(buf, 44, v_acc); + _mav_put_uint16_t(buf, 46, vel_acc); + _mav_put_uint16_t(buf, 48, update_rate); + _mav_put_uint8_t(buf, 68, flight_state); + _mav_put_uint8_t(buf, 69, flags); + _mav_put_uint8_t_array(buf, 50, uas_id, 18); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN); +#else + mavlink_utm_global_position_t packet; + packet.time = time; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.next_lat = next_lat; + packet.next_lon = next_lon; + packet.next_alt = next_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.update_rate = update_rate; + packet.flight_state = flight_state; + packet.flags = flags; + mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*18); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_UTM_GLOBAL_POSITION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); +} + +/** + * @brief Pack a utm_global_position message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time [us] Time of applicability of position (microseconds since UNIX epoch). + * @param uas_id Unique UAS ID. + * @param lat [degE7] Latitude (WGS84) + * @param lon [degE7] Longitude (WGS84) + * @param alt [mm] Altitude (WGS84) + * @param relative_alt [mm] Altitude above ground + * @param vx [cm/s] Ground X speed (latitude, positive north) + * @param vy [cm/s] Ground Y speed (longitude, positive east) + * @param vz [cm/s] Ground Z speed (altitude, positive down) + * @param h_acc [mm] Horizontal position uncertainty (standard deviation) + * @param v_acc [mm] Altitude uncertainty (standard deviation) + * @param vel_acc [cm/s] Speed uncertainty (standard deviation) + * @param next_lat [degE7] Next waypoint, latitude (WGS84) + * @param next_lon [degE7] Next waypoint, longitude (WGS84) + * @param next_alt [mm] Next waypoint, altitude (WGS84) + * @param update_rate [cs] Time until next update. Set to 0 if unknown or in data driven mode. + * @param flight_state Flight state + * @param flags Bitwise OR combination of the data available flags. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_utm_global_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time,const uint8_t *uas_id,int32_t lat,int32_t lon,int32_t alt,int32_t relative_alt,int16_t vx,int16_t vy,int16_t vz,uint16_t h_acc,uint16_t v_acc,uint16_t vel_acc,int32_t next_lat,int32_t next_lon,int32_t next_alt,uint16_t update_rate,uint8_t flight_state,uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN]; + _mav_put_uint64_t(buf, 0, time); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_int32_t(buf, 20, relative_alt); + _mav_put_int32_t(buf, 24, next_lat); + _mav_put_int32_t(buf, 28, next_lon); + _mav_put_int32_t(buf, 32, next_alt); + _mav_put_int16_t(buf, 36, vx); + _mav_put_int16_t(buf, 38, vy); + _mav_put_int16_t(buf, 40, vz); + _mav_put_uint16_t(buf, 42, h_acc); + _mav_put_uint16_t(buf, 44, v_acc); + _mav_put_uint16_t(buf, 46, vel_acc); + _mav_put_uint16_t(buf, 48, update_rate); + _mav_put_uint8_t(buf, 68, flight_state); + _mav_put_uint8_t(buf, 69, flags); + _mav_put_uint8_t_array(buf, 50, uas_id, 18); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN); +#else + mavlink_utm_global_position_t packet; + packet.time = time; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.next_lat = next_lat; + packet.next_lon = next_lon; + packet.next_alt = next_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.update_rate = update_rate; + packet.flight_state = flight_state; + packet.flags = flags; + mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*18); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_UTM_GLOBAL_POSITION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); +} + +/** + * @brief Encode a utm_global_position struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param utm_global_position C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_utm_global_position_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_utm_global_position_t* utm_global_position) +{ + return mavlink_msg_utm_global_position_pack(system_id, component_id, msg, utm_global_position->time, utm_global_position->uas_id, utm_global_position->lat, utm_global_position->lon, utm_global_position->alt, utm_global_position->relative_alt, utm_global_position->vx, utm_global_position->vy, utm_global_position->vz, utm_global_position->h_acc, utm_global_position->v_acc, utm_global_position->vel_acc, utm_global_position->next_lat, utm_global_position->next_lon, utm_global_position->next_alt, utm_global_position->update_rate, utm_global_position->flight_state, utm_global_position->flags); +} + +/** + * @brief Encode a utm_global_position struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param utm_global_position C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_utm_global_position_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_utm_global_position_t* utm_global_position) +{ + return mavlink_msg_utm_global_position_pack_chan(system_id, component_id, chan, msg, utm_global_position->time, utm_global_position->uas_id, utm_global_position->lat, utm_global_position->lon, utm_global_position->alt, utm_global_position->relative_alt, utm_global_position->vx, utm_global_position->vy, utm_global_position->vz, utm_global_position->h_acc, utm_global_position->v_acc, utm_global_position->vel_acc, utm_global_position->next_lat, utm_global_position->next_lon, utm_global_position->next_alt, utm_global_position->update_rate, utm_global_position->flight_state, utm_global_position->flags); +} + +/** + * @brief Send a utm_global_position message + * @param chan MAVLink channel to send the message + * + * @param time [us] Time of applicability of position (microseconds since UNIX epoch). + * @param uas_id Unique UAS ID. + * @param lat [degE7] Latitude (WGS84) + * @param lon [degE7] Longitude (WGS84) + * @param alt [mm] Altitude (WGS84) + * @param relative_alt [mm] Altitude above ground + * @param vx [cm/s] Ground X speed (latitude, positive north) + * @param vy [cm/s] Ground Y speed (longitude, positive east) + * @param vz [cm/s] Ground Z speed (altitude, positive down) + * @param h_acc [mm] Horizontal position uncertainty (standard deviation) + * @param v_acc [mm] Altitude uncertainty (standard deviation) + * @param vel_acc [cm/s] Speed uncertainty (standard deviation) + * @param next_lat [degE7] Next waypoint, latitude (WGS84) + * @param next_lon [degE7] Next waypoint, longitude (WGS84) + * @param next_alt [mm] Next waypoint, altitude (WGS84) + * @param update_rate [cs] Time until next update. Set to 0 if unknown or in data driven mode. + * @param flight_state Flight state + * @param flags Bitwise OR combination of the data available flags. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_utm_global_position_send(mavlink_channel_t chan, uint64_t time, const uint8_t *uas_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t h_acc, uint16_t v_acc, uint16_t vel_acc, int32_t next_lat, int32_t next_lon, int32_t next_alt, uint16_t update_rate, uint8_t flight_state, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN]; + _mav_put_uint64_t(buf, 0, time); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_int32_t(buf, 20, relative_alt); + _mav_put_int32_t(buf, 24, next_lat); + _mav_put_int32_t(buf, 28, next_lon); + _mav_put_int32_t(buf, 32, next_alt); + _mav_put_int16_t(buf, 36, vx); + _mav_put_int16_t(buf, 38, vy); + _mav_put_int16_t(buf, 40, vz); + _mav_put_uint16_t(buf, 42, h_acc); + _mav_put_uint16_t(buf, 44, v_acc); + _mav_put_uint16_t(buf, 46, vel_acc); + _mav_put_uint16_t(buf, 48, update_rate); + _mav_put_uint8_t(buf, 68, flight_state); + _mav_put_uint8_t(buf, 69, flags); + _mav_put_uint8_t_array(buf, 50, uas_id, 18); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, buf, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); +#else + mavlink_utm_global_position_t packet; + packet.time = time; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.next_lat = next_lat; + packet.next_lon = next_lon; + packet.next_alt = next_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.update_rate = update_rate; + packet.flight_state = flight_state; + packet.flags = flags; + mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*18); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, (const char *)&packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); +#endif +} + +/** + * @brief Send a utm_global_position message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_utm_global_position_send_struct(mavlink_channel_t chan, const mavlink_utm_global_position_t* utm_global_position) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_utm_global_position_send(chan, utm_global_position->time, utm_global_position->uas_id, utm_global_position->lat, utm_global_position->lon, utm_global_position->alt, utm_global_position->relative_alt, utm_global_position->vx, utm_global_position->vy, utm_global_position->vz, utm_global_position->h_acc, utm_global_position->v_acc, utm_global_position->vel_acc, utm_global_position->next_lat, utm_global_position->next_lon, utm_global_position->next_alt, utm_global_position->update_rate, utm_global_position->flight_state, utm_global_position->flags); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, (const char *)utm_global_position, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_utm_global_position_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time, const uint8_t *uas_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t h_acc, uint16_t v_acc, uint16_t vel_acc, int32_t next_lat, int32_t next_lon, int32_t next_alt, uint16_t update_rate, uint8_t flight_state, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_int32_t(buf, 20, relative_alt); + _mav_put_int32_t(buf, 24, next_lat); + _mav_put_int32_t(buf, 28, next_lon); + _mav_put_int32_t(buf, 32, next_alt); + _mav_put_int16_t(buf, 36, vx); + _mav_put_int16_t(buf, 38, vy); + _mav_put_int16_t(buf, 40, vz); + _mav_put_uint16_t(buf, 42, h_acc); + _mav_put_uint16_t(buf, 44, v_acc); + _mav_put_uint16_t(buf, 46, vel_acc); + _mav_put_uint16_t(buf, 48, update_rate); + _mav_put_uint8_t(buf, 68, flight_state); + _mav_put_uint8_t(buf, 69, flags); + _mav_put_uint8_t_array(buf, 50, uas_id, 18); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, buf, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); +#else + mavlink_utm_global_position_t *packet = (mavlink_utm_global_position_t *)msgbuf; + packet->time = time; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->relative_alt = relative_alt; + packet->next_lat = next_lat; + packet->next_lon = next_lon; + packet->next_alt = next_alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->h_acc = h_acc; + packet->v_acc = v_acc; + packet->vel_acc = vel_acc; + packet->update_rate = update_rate; + packet->flight_state = flight_state; + packet->flags = flags; + mav_array_memcpy(packet->uas_id, uas_id, sizeof(uint8_t)*18); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, (const char *)packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE UTM_GLOBAL_POSITION UNPACKING + + +/** + * @brief Get field time from utm_global_position message + * + * @return [us] Time of applicability of position (microseconds since UNIX epoch). + */ +static inline uint64_t mavlink_msg_utm_global_position_get_time(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field uas_id from utm_global_position message + * + * @return Unique UAS ID. + */ +static inline uint16_t mavlink_msg_utm_global_position_get_uas_id(const mavlink_message_t* msg, uint8_t *uas_id) +{ + return _MAV_RETURN_uint8_t_array(msg, uas_id, 18, 50); +} + +/** + * @brief Get field lat from utm_global_position message + * + * @return [degE7] Latitude (WGS84) + */ +static inline int32_t mavlink_msg_utm_global_position_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field lon from utm_global_position message + * + * @return [degE7] Longitude (WGS84) + */ +static inline int32_t mavlink_msg_utm_global_position_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field alt from utm_global_position message + * + * @return [mm] Altitude (WGS84) + */ +static inline int32_t mavlink_msg_utm_global_position_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field relative_alt from utm_global_position message + * + * @return [mm] Altitude above ground + */ +static inline int32_t mavlink_msg_utm_global_position_get_relative_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 20); +} + +/** + * @brief Get field vx from utm_global_position message + * + * @return [cm/s] Ground X speed (latitude, positive north) + */ +static inline int16_t mavlink_msg_utm_global_position_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 36); +} + +/** + * @brief Get field vy from utm_global_position message + * + * @return [cm/s] Ground Y speed (longitude, positive east) + */ +static inline int16_t mavlink_msg_utm_global_position_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 38); +} + +/** + * @brief Get field vz from utm_global_position message + * + * @return [cm/s] Ground Z speed (altitude, positive down) + */ +static inline int16_t mavlink_msg_utm_global_position_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 40); +} + +/** + * @brief Get field h_acc from utm_global_position message + * + * @return [mm] Horizontal position uncertainty (standard deviation) + */ +static inline uint16_t mavlink_msg_utm_global_position_get_h_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 42); +} + +/** + * @brief Get field v_acc from utm_global_position message + * + * @return [mm] Altitude uncertainty (standard deviation) + */ +static inline uint16_t mavlink_msg_utm_global_position_get_v_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 44); +} + +/** + * @brief Get field vel_acc from utm_global_position message + * + * @return [cm/s] Speed uncertainty (standard deviation) + */ +static inline uint16_t mavlink_msg_utm_global_position_get_vel_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 46); +} + +/** + * @brief Get field next_lat from utm_global_position message + * + * @return [degE7] Next waypoint, latitude (WGS84) + */ +static inline int32_t mavlink_msg_utm_global_position_get_next_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 24); +} + +/** + * @brief Get field next_lon from utm_global_position message + * + * @return [degE7] Next waypoint, longitude (WGS84) + */ +static inline int32_t mavlink_msg_utm_global_position_get_next_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 28); +} + +/** + * @brief Get field next_alt from utm_global_position message + * + * @return [mm] Next waypoint, altitude (WGS84) + */ +static inline int32_t mavlink_msg_utm_global_position_get_next_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 32); +} + +/** + * @brief Get field update_rate from utm_global_position message + * + * @return [cs] Time until next update. Set to 0 if unknown or in data driven mode. + */ +static inline uint16_t mavlink_msg_utm_global_position_get_update_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 48); +} + +/** + * @brief Get field flight_state from utm_global_position message + * + * @return Flight state + */ +static inline uint8_t mavlink_msg_utm_global_position_get_flight_state(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 68); +} + +/** + * @brief Get field flags from utm_global_position message + * + * @return Bitwise OR combination of the data available flags. + */ +static inline uint8_t mavlink_msg_utm_global_position_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 69); +} + +/** + * @brief Decode a utm_global_position message into a struct + * + * @param msg The message to decode + * @param utm_global_position C-struct to decode the message contents into + */ +static inline void mavlink_msg_utm_global_position_decode(const mavlink_message_t* msg, mavlink_utm_global_position_t* utm_global_position) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + utm_global_position->time = mavlink_msg_utm_global_position_get_time(msg); + utm_global_position->lat = mavlink_msg_utm_global_position_get_lat(msg); + utm_global_position->lon = mavlink_msg_utm_global_position_get_lon(msg); + utm_global_position->alt = mavlink_msg_utm_global_position_get_alt(msg); + utm_global_position->relative_alt = mavlink_msg_utm_global_position_get_relative_alt(msg); + utm_global_position->next_lat = mavlink_msg_utm_global_position_get_next_lat(msg); + utm_global_position->next_lon = mavlink_msg_utm_global_position_get_next_lon(msg); + utm_global_position->next_alt = mavlink_msg_utm_global_position_get_next_alt(msg); + utm_global_position->vx = mavlink_msg_utm_global_position_get_vx(msg); + utm_global_position->vy = mavlink_msg_utm_global_position_get_vy(msg); + utm_global_position->vz = mavlink_msg_utm_global_position_get_vz(msg); + utm_global_position->h_acc = mavlink_msg_utm_global_position_get_h_acc(msg); + utm_global_position->v_acc = mavlink_msg_utm_global_position_get_v_acc(msg); + utm_global_position->vel_acc = mavlink_msg_utm_global_position_get_vel_acc(msg); + utm_global_position->update_rate = mavlink_msg_utm_global_position_get_update_rate(msg); + mavlink_msg_utm_global_position_get_uas_id(msg, utm_global_position->uas_id); + utm_global_position->flight_state = mavlink_msg_utm_global_position_get_flight_state(msg); + utm_global_position->flags = mavlink_msg_utm_global_position_get_flags(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN? msg->len : MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN; + memset(utm_global_position, 0, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN); + memcpy(utm_global_position, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_vicon_position_estimate.h b/lib/main/MAVLink/common/mavlink_msg_vicon_position_estimate.h index 4ad2a73885..753f526037 100755 --- a/lib/main/MAVLink/common/mavlink_msg_vicon_position_estimate.h +++ b/lib/main/MAVLink/common/mavlink_msg_vicon_position_estimate.h @@ -12,23 +12,24 @@ typedef struct __mavlink_vicon_position_estimate_t { float roll; /*< [rad] Roll angle*/ float pitch; /*< [rad] Pitch angle*/ float yaw; /*< [rad] Yaw angle*/ + float covariance[21]; /*< Row-major representation of 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.*/ } mavlink_vicon_position_estimate_t; -#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN 32 +#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN 116 #define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_MIN_LEN 32 -#define MAVLINK_MSG_ID_104_LEN 32 +#define MAVLINK_MSG_ID_104_LEN 116 #define MAVLINK_MSG_ID_104_MIN_LEN 32 #define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC 56 #define MAVLINK_MSG_ID_104_CRC 56 - +#define MAVLINK_MSG_VICON_POSITION_ESTIMATE_FIELD_COVARIANCE_LEN 21 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE { \ 104, \ "VICON_POSITION_ESTIMATE", \ - 7, \ + 8, \ { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vicon_position_estimate_t, usec) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vicon_position_estimate_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vicon_position_estimate_t, y) }, \ @@ -36,12 +37,13 @@ typedef struct __mavlink_vicon_position_estimate_t { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vicon_position_estimate_t, roll) }, \ { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vicon_position_estimate_t, pitch) }, \ { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vicon_position_estimate_t, yaw) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 32, offsetof(mavlink_vicon_position_estimate_t, covariance) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE { \ "VICON_POSITION_ESTIMATE", \ - 7, \ + 8, \ { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vicon_position_estimate_t, usec) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vicon_position_estimate_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vicon_position_estimate_t, y) }, \ @@ -49,6 +51,7 @@ typedef struct __mavlink_vicon_position_estimate_t { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vicon_position_estimate_t, roll) }, \ { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vicon_position_estimate_t, pitch) }, \ { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vicon_position_estimate_t, yaw) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 32, offsetof(mavlink_vicon_position_estimate_t, covariance) }, \ } \ } #endif @@ -66,10 +69,11 @@ typedef struct __mavlink_vicon_position_estimate_t { * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) + uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN]; @@ -80,7 +84,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_float_array(buf, 32, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); #else mavlink_vicon_position_estimate_t packet; @@ -91,7 +95,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); #endif @@ -112,11 +116,12 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) + uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw,const float *covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN]; @@ -127,7 +132,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t sys _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_float_array(buf, 32, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); #else mavlink_vicon_position_estimate_t packet; @@ -138,7 +143,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t sys packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); #endif @@ -156,7 +161,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t sys */ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vicon_position_estimate_t* vicon_position_estimate) { - return mavlink_msg_vicon_position_estimate_pack(system_id, component_id, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); + return mavlink_msg_vicon_position_estimate_pack(system_id, component_id, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw, vicon_position_estimate->covariance); } /** @@ -170,7 +175,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system */ static inline uint16_t mavlink_msg_vicon_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vicon_position_estimate_t* vicon_position_estimate) { - return mavlink_msg_vicon_position_estimate_pack_chan(system_id, component_id, chan, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); + return mavlink_msg_vicon_position_estimate_pack_chan(system_id, component_id, chan, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw, vicon_position_estimate->covariance); } /** @@ -184,10 +189,11 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode_chan(uint8_t s * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN]; @@ -198,7 +204,7 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_float_array(buf, 32, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); #else mavlink_vicon_position_estimate_t packet; @@ -209,7 +215,7 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); #endif } @@ -222,7 +228,7 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch static inline void mavlink_msg_vicon_position_estimate_send_struct(mavlink_channel_t chan, const mavlink_vicon_position_estimate_t* vicon_position_estimate) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_vicon_position_estimate_send(chan, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); + mavlink_msg_vicon_position_estimate_send(chan, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw, vicon_position_estimate->covariance); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)vicon_position_estimate, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); #endif @@ -236,7 +242,7 @@ static inline void mavlink_msg_vicon_position_estimate_send_struct(mavlink_chann is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_vicon_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +static inline void mavlink_msg_vicon_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -247,7 +253,7 @@ static inline void mavlink_msg_vicon_position_estimate_send_buf(mavlink_message_ _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_float_array(buf, 32, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); #else mavlink_vicon_position_estimate_t *packet = (mavlink_vicon_position_estimate_t *)msgbuf; @@ -258,7 +264,7 @@ static inline void mavlink_msg_vicon_position_estimate_send_buf(mavlink_message_ packet->roll = roll; packet->pitch = pitch; packet->yaw = yaw; - + mav_array_memcpy(packet->covariance, covariance, sizeof(float)*21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); #endif } @@ -339,6 +345,16 @@ static inline float mavlink_msg_vicon_position_estimate_get_yaw(const mavlink_me return _MAV_RETURN_float(msg, 28); } +/** + * @brief Get field covariance from vicon_position_estimate message + * + * @return Row-major representation of 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + */ +static inline uint16_t mavlink_msg_vicon_position_estimate_get_covariance(const mavlink_message_t* msg, float *covariance) +{ + return _MAV_RETURN_float_array(msg, covariance, 21, 32); +} + /** * @brief Decode a vicon_position_estimate message into a struct * @@ -355,6 +371,7 @@ static inline void mavlink_msg_vicon_position_estimate_decode(const mavlink_mess vicon_position_estimate->roll = mavlink_msg_vicon_position_estimate_get_roll(msg); vicon_position_estimate->pitch = mavlink_msg_vicon_position_estimate_get_pitch(msg); vicon_position_estimate->yaw = mavlink_msg_vicon_position_estimate_get_yaw(msg); + mavlink_msg_vicon_position_estimate_get_covariance(msg, vicon_position_estimate->covariance); #else uint8_t len = msg->len < MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN? msg->len : MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN; memset(vicon_position_estimate, 0, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_video_stream_information.h b/lib/main/MAVLink/common/mavlink_msg_video_stream_information.h new file mode 100644 index 0000000000..aa6d84b743 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_video_stream_information.h @@ -0,0 +1,481 @@ +#pragma once +// MESSAGE VIDEO_STREAM_INFORMATION PACKING + +#define MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION 269 + + +typedef struct __mavlink_video_stream_information_t { + float framerate; /*< [Hz] Frame rate.*/ + uint32_t bitrate; /*< [bits/s] Bit rate.*/ + uint16_t flags; /*< Bitmap of stream status flags.*/ + uint16_t resolution_h; /*< [pix] Horizontal resolution.*/ + uint16_t resolution_v; /*< [pix] Vertical resolution.*/ + uint16_t rotation; /*< [deg] Video image rotation clockwise.*/ + uint16_t hfov; /*< [deg] Horizontal Field of view.*/ + uint8_t stream_id; /*< Video Stream ID (1 for first, 2 for second, etc.)*/ + uint8_t count; /*< Number of streams available.*/ + uint8_t type; /*< Type of stream.*/ + char name[32]; /*< Stream name.*/ + char uri[160]; /*< Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to).*/ +} mavlink_video_stream_information_t; + +#define MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN 213 +#define MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN 213 +#define MAVLINK_MSG_ID_269_LEN 213 +#define MAVLINK_MSG_ID_269_MIN_LEN 213 + +#define MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC 109 +#define MAVLINK_MSG_ID_269_CRC 109 + +#define MAVLINK_MSG_VIDEO_STREAM_INFORMATION_FIELD_NAME_LEN 32 +#define MAVLINK_MSG_VIDEO_STREAM_INFORMATION_FIELD_URI_LEN 160 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_VIDEO_STREAM_INFORMATION { \ + 269, \ + "VIDEO_STREAM_INFORMATION", \ + 12, \ + { { "stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_video_stream_information_t, stream_id) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_video_stream_information_t, count) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_video_stream_information_t, type) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_video_stream_information_t, flags) }, \ + { "framerate", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_video_stream_information_t, framerate) }, \ + { "resolution_h", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_video_stream_information_t, resolution_h) }, \ + { "resolution_v", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_video_stream_information_t, resolution_v) }, \ + { "bitrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_video_stream_information_t, bitrate) }, \ + { "rotation", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_video_stream_information_t, rotation) }, \ + { "hfov", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_video_stream_information_t, hfov) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 32, 21, offsetof(mavlink_video_stream_information_t, name) }, \ + { "uri", NULL, MAVLINK_TYPE_CHAR, 160, 53, offsetof(mavlink_video_stream_information_t, uri) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_VIDEO_STREAM_INFORMATION { \ + "VIDEO_STREAM_INFORMATION", \ + 12, \ + { { "stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_video_stream_information_t, stream_id) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_video_stream_information_t, count) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_video_stream_information_t, type) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_video_stream_information_t, flags) }, \ + { "framerate", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_video_stream_information_t, framerate) }, \ + { "resolution_h", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_video_stream_information_t, resolution_h) }, \ + { "resolution_v", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_video_stream_information_t, resolution_v) }, \ + { "bitrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_video_stream_information_t, bitrate) }, \ + { "rotation", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_video_stream_information_t, rotation) }, \ + { "hfov", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_video_stream_information_t, hfov) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 32, 21, offsetof(mavlink_video_stream_information_t, name) }, \ + { "uri", NULL, MAVLINK_TYPE_CHAR, 160, 53, offsetof(mavlink_video_stream_information_t, uri) }, \ + } \ +} +#endif + +/** + * @brief Pack a video_stream_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param count Number of streams available. + * @param type Type of stream. + * @param flags Bitmap of stream status flags. + * @param framerate [Hz] Frame rate. + * @param resolution_h [pix] Horizontal resolution. + * @param resolution_v [pix] Vertical resolution. + * @param bitrate [bits/s] Bit rate. + * @param rotation [deg] Video image rotation clockwise. + * @param hfov [deg] Horizontal Field of view. + * @param name Stream name. + * @param uri Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_video_stream_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t stream_id, uint8_t count, uint8_t type, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov, const char *name, const char *uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN]; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + _mav_put_uint8_t(buf, 19, count); + _mav_put_uint8_t(buf, 20, type); + _mav_put_char_array(buf, 21, name, 32); + _mav_put_char_array(buf, 53, uri, 160); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); +#else + mavlink_video_stream_information_t packet; + packet.framerate = framerate; + packet.bitrate = bitrate; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.rotation = rotation; + packet.hfov = hfov; + packet.stream_id = stream_id; + packet.count = count; + packet.type = type; + mav_array_memcpy(packet.name, name, sizeof(char)*32); + mav_array_memcpy(packet.uri, uri, sizeof(char)*160); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); +} + +/** + * @brief Pack a video_stream_information message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param count Number of streams available. + * @param type Type of stream. + * @param flags Bitmap of stream status flags. + * @param framerate [Hz] Frame rate. + * @param resolution_h [pix] Horizontal resolution. + * @param resolution_v [pix] Vertical resolution. + * @param bitrate [bits/s] Bit rate. + * @param rotation [deg] Video image rotation clockwise. + * @param hfov [deg] Horizontal Field of view. + * @param name Stream name. + * @param uri Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_video_stream_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t stream_id,uint8_t count,uint8_t type,uint16_t flags,float framerate,uint16_t resolution_h,uint16_t resolution_v,uint32_t bitrate,uint16_t rotation,uint16_t hfov,const char *name,const char *uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN]; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + _mav_put_uint8_t(buf, 19, count); + _mav_put_uint8_t(buf, 20, type); + _mav_put_char_array(buf, 21, name, 32); + _mav_put_char_array(buf, 53, uri, 160); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); +#else + mavlink_video_stream_information_t packet; + packet.framerate = framerate; + packet.bitrate = bitrate; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.rotation = rotation; + packet.hfov = hfov; + packet.stream_id = stream_id; + packet.count = count; + packet.type = type; + mav_array_memcpy(packet.name, name, sizeof(char)*32); + mav_array_memcpy(packet.uri, uri, sizeof(char)*160); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); +} + +/** + * @brief Encode a video_stream_information struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param video_stream_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_video_stream_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_video_stream_information_t* video_stream_information) +{ + return mavlink_msg_video_stream_information_pack(system_id, component_id, msg, video_stream_information->stream_id, video_stream_information->count, video_stream_information->type, video_stream_information->flags, video_stream_information->framerate, video_stream_information->resolution_h, video_stream_information->resolution_v, video_stream_information->bitrate, video_stream_information->rotation, video_stream_information->hfov, video_stream_information->name, video_stream_information->uri); +} + +/** + * @brief Encode a video_stream_information struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param video_stream_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_video_stream_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_video_stream_information_t* video_stream_information) +{ + return mavlink_msg_video_stream_information_pack_chan(system_id, component_id, chan, msg, video_stream_information->stream_id, video_stream_information->count, video_stream_information->type, video_stream_information->flags, video_stream_information->framerate, video_stream_information->resolution_h, video_stream_information->resolution_v, video_stream_information->bitrate, video_stream_information->rotation, video_stream_information->hfov, video_stream_information->name, video_stream_information->uri); +} + +/** + * @brief Send a video_stream_information message + * @param chan MAVLink channel to send the message + * + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param count Number of streams available. + * @param type Type of stream. + * @param flags Bitmap of stream status flags. + * @param framerate [Hz] Frame rate. + * @param resolution_h [pix] Horizontal resolution. + * @param resolution_v [pix] Vertical resolution. + * @param bitrate [bits/s] Bit rate. + * @param rotation [deg] Video image rotation clockwise. + * @param hfov [deg] Horizontal Field of view. + * @param name Stream name. + * @param uri Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_video_stream_information_send(mavlink_channel_t chan, uint8_t stream_id, uint8_t count, uint8_t type, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov, const char *name, const char *uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN]; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + _mav_put_uint8_t(buf, 19, count); + _mav_put_uint8_t(buf, 20, type); + _mav_put_char_array(buf, 21, name, 32); + _mav_put_char_array(buf, 53, uri, 160); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, buf, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); +#else + mavlink_video_stream_information_t packet; + packet.framerate = framerate; + packet.bitrate = bitrate; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.rotation = rotation; + packet.hfov = hfov; + packet.stream_id = stream_id; + packet.count = count; + packet.type = type; + mav_array_memcpy(packet.name, name, sizeof(char)*32); + mav_array_memcpy(packet.uri, uri, sizeof(char)*160); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); +#endif +} + +/** + * @brief Send a video_stream_information message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_video_stream_information_send_struct(mavlink_channel_t chan, const mavlink_video_stream_information_t* video_stream_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_video_stream_information_send(chan, video_stream_information->stream_id, video_stream_information->count, video_stream_information->type, video_stream_information->flags, video_stream_information->framerate, video_stream_information->resolution_h, video_stream_information->resolution_v, video_stream_information->bitrate, video_stream_information->rotation, video_stream_information->hfov, video_stream_information->name, video_stream_information->uri); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, (const char *)video_stream_information, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_video_stream_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t stream_id, uint8_t count, uint8_t type, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov, const char *name, const char *uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + _mav_put_uint8_t(buf, 19, count); + _mav_put_uint8_t(buf, 20, type); + _mav_put_char_array(buf, 21, name, 32); + _mav_put_char_array(buf, 53, uri, 160); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, buf, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); +#else + mavlink_video_stream_information_t *packet = (mavlink_video_stream_information_t *)msgbuf; + packet->framerate = framerate; + packet->bitrate = bitrate; + packet->flags = flags; + packet->resolution_h = resolution_h; + packet->resolution_v = resolution_v; + packet->rotation = rotation; + packet->hfov = hfov; + packet->stream_id = stream_id; + packet->count = count; + packet->type = type; + mav_array_memcpy(packet->name, name, sizeof(char)*32); + mav_array_memcpy(packet->uri, uri, sizeof(char)*160); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE VIDEO_STREAM_INFORMATION UNPACKING + + +/** + * @brief Get field stream_id from video_stream_information message + * + * @return Video Stream ID (1 for first, 2 for second, etc.) + */ +static inline uint8_t mavlink_msg_video_stream_information_get_stream_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 18); +} + +/** + * @brief Get field count from video_stream_information message + * + * @return Number of streams available. + */ +static inline uint8_t mavlink_msg_video_stream_information_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 19); +} + +/** + * @brief Get field type from video_stream_information message + * + * @return Type of stream. + */ +static inline uint8_t mavlink_msg_video_stream_information_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field flags from video_stream_information message + * + * @return Bitmap of stream status flags. + */ +static inline uint16_t mavlink_msg_video_stream_information_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field framerate from video_stream_information message + * + * @return [Hz] Frame rate. + */ +static inline float mavlink_msg_video_stream_information_get_framerate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field resolution_h from video_stream_information message + * + * @return [pix] Horizontal resolution. + */ +static inline uint16_t mavlink_msg_video_stream_information_get_resolution_h(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field resolution_v from video_stream_information message + * + * @return [pix] Vertical resolution. + */ +static inline uint16_t mavlink_msg_video_stream_information_get_resolution_v(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field bitrate from video_stream_information message + * + * @return [bits/s] Bit rate. + */ +static inline uint32_t mavlink_msg_video_stream_information_get_bitrate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field rotation from video_stream_information message + * + * @return [deg] Video image rotation clockwise. + */ +static inline uint16_t mavlink_msg_video_stream_information_get_rotation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Get field hfov from video_stream_information message + * + * @return [deg] Horizontal Field of view. + */ +static inline uint16_t mavlink_msg_video_stream_information_get_hfov(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field name from video_stream_information message + * + * @return Stream name. + */ +static inline uint16_t mavlink_msg_video_stream_information_get_name(const mavlink_message_t* msg, char *name) +{ + return _MAV_RETURN_char_array(msg, name, 32, 21); +} + +/** + * @brief Get field uri from video_stream_information message + * + * @return Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to). + */ +static inline uint16_t mavlink_msg_video_stream_information_get_uri(const mavlink_message_t* msg, char *uri) +{ + return _MAV_RETURN_char_array(msg, uri, 160, 53); +} + +/** + * @brief Decode a video_stream_information message into a struct + * + * @param msg The message to decode + * @param video_stream_information C-struct to decode the message contents into + */ +static inline void mavlink_msg_video_stream_information_decode(const mavlink_message_t* msg, mavlink_video_stream_information_t* video_stream_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + video_stream_information->framerate = mavlink_msg_video_stream_information_get_framerate(msg); + video_stream_information->bitrate = mavlink_msg_video_stream_information_get_bitrate(msg); + video_stream_information->flags = mavlink_msg_video_stream_information_get_flags(msg); + video_stream_information->resolution_h = mavlink_msg_video_stream_information_get_resolution_h(msg); + video_stream_information->resolution_v = mavlink_msg_video_stream_information_get_resolution_v(msg); + video_stream_information->rotation = mavlink_msg_video_stream_information_get_rotation(msg); + video_stream_information->hfov = mavlink_msg_video_stream_information_get_hfov(msg); + video_stream_information->stream_id = mavlink_msg_video_stream_information_get_stream_id(msg); + video_stream_information->count = mavlink_msg_video_stream_information_get_count(msg); + video_stream_information->type = mavlink_msg_video_stream_information_get_type(msg); + mavlink_msg_video_stream_information_get_name(msg, video_stream_information->name); + mavlink_msg_video_stream_information_get_uri(msg, video_stream_information->uri); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN; + memset(video_stream_information, 0, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); + memcpy(video_stream_information, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_video_stream_status.h b/lib/main/MAVLink/common/mavlink_msg_video_stream_status.h new file mode 100644 index 0000000000..380941005e --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_video_stream_status.h @@ -0,0 +1,388 @@ +#pragma once +// MESSAGE VIDEO_STREAM_STATUS PACKING + +#define MAVLINK_MSG_ID_VIDEO_STREAM_STATUS 270 + + +typedef struct __mavlink_video_stream_status_t { + float framerate; /*< [Hz] Frame rate*/ + uint32_t bitrate; /*< [bits/s] Bit rate*/ + uint16_t flags; /*< Bitmap of stream status flags*/ + uint16_t resolution_h; /*< [pix] Horizontal resolution*/ + uint16_t resolution_v; /*< [pix] Vertical resolution*/ + uint16_t rotation; /*< [deg] Video image rotation clockwise*/ + uint16_t hfov; /*< [deg] Horizontal Field of view*/ + uint8_t stream_id; /*< Video Stream ID (1 for first, 2 for second, etc.)*/ +} mavlink_video_stream_status_t; + +#define MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN 19 +#define MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN 19 +#define MAVLINK_MSG_ID_270_LEN 19 +#define MAVLINK_MSG_ID_270_MIN_LEN 19 + +#define MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC 59 +#define MAVLINK_MSG_ID_270_CRC 59 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_VIDEO_STREAM_STATUS { \ + 270, \ + "VIDEO_STREAM_STATUS", \ + 8, \ + { { "stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_video_stream_status_t, stream_id) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_video_stream_status_t, flags) }, \ + { "framerate", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_video_stream_status_t, framerate) }, \ + { "resolution_h", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_video_stream_status_t, resolution_h) }, \ + { "resolution_v", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_video_stream_status_t, resolution_v) }, \ + { "bitrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_video_stream_status_t, bitrate) }, \ + { "rotation", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_video_stream_status_t, rotation) }, \ + { "hfov", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_video_stream_status_t, hfov) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_VIDEO_STREAM_STATUS { \ + "VIDEO_STREAM_STATUS", \ + 8, \ + { { "stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_video_stream_status_t, stream_id) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_video_stream_status_t, flags) }, \ + { "framerate", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_video_stream_status_t, framerate) }, \ + { "resolution_h", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_video_stream_status_t, resolution_h) }, \ + { "resolution_v", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_video_stream_status_t, resolution_v) }, \ + { "bitrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_video_stream_status_t, bitrate) }, \ + { "rotation", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_video_stream_status_t, rotation) }, \ + { "hfov", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_video_stream_status_t, hfov) }, \ + } \ +} +#endif + +/** + * @brief Pack a video_stream_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param flags Bitmap of stream status flags + * @param framerate [Hz] Frame rate + * @param resolution_h [pix] Horizontal resolution + * @param resolution_v [pix] Vertical resolution + * @param bitrate [bits/s] Bit rate + * @param rotation [deg] Video image rotation clockwise + * @param hfov [deg] Horizontal Field of view + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_video_stream_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t stream_id, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN]; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); +#else + mavlink_video_stream_status_t packet; + packet.framerate = framerate; + packet.bitrate = bitrate; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.rotation = rotation; + packet.hfov = hfov; + packet.stream_id = stream_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VIDEO_STREAM_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); +} + +/** + * @brief Pack a video_stream_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param flags Bitmap of stream status flags + * @param framerate [Hz] Frame rate + * @param resolution_h [pix] Horizontal resolution + * @param resolution_v [pix] Vertical resolution + * @param bitrate [bits/s] Bit rate + * @param rotation [deg] Video image rotation clockwise + * @param hfov [deg] Horizontal Field of view + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_video_stream_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t stream_id,uint16_t flags,float framerate,uint16_t resolution_h,uint16_t resolution_v,uint32_t bitrate,uint16_t rotation,uint16_t hfov) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN]; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); +#else + mavlink_video_stream_status_t packet; + packet.framerate = framerate; + packet.bitrate = bitrate; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.rotation = rotation; + packet.hfov = hfov; + packet.stream_id = stream_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VIDEO_STREAM_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); +} + +/** + * @brief Encode a video_stream_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param video_stream_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_video_stream_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_video_stream_status_t* video_stream_status) +{ + return mavlink_msg_video_stream_status_pack(system_id, component_id, msg, video_stream_status->stream_id, video_stream_status->flags, video_stream_status->framerate, video_stream_status->resolution_h, video_stream_status->resolution_v, video_stream_status->bitrate, video_stream_status->rotation, video_stream_status->hfov); +} + +/** + * @brief Encode a video_stream_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param video_stream_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_video_stream_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_video_stream_status_t* video_stream_status) +{ + return mavlink_msg_video_stream_status_pack_chan(system_id, component_id, chan, msg, video_stream_status->stream_id, video_stream_status->flags, video_stream_status->framerate, video_stream_status->resolution_h, video_stream_status->resolution_v, video_stream_status->bitrate, video_stream_status->rotation, video_stream_status->hfov); +} + +/** + * @brief Send a video_stream_status message + * @param chan MAVLink channel to send the message + * + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param flags Bitmap of stream status flags + * @param framerate [Hz] Frame rate + * @param resolution_h [pix] Horizontal resolution + * @param resolution_v [pix] Vertical resolution + * @param bitrate [bits/s] Bit rate + * @param rotation [deg] Video image rotation clockwise + * @param hfov [deg] Horizontal Field of view + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_video_stream_status_send(mavlink_channel_t chan, uint8_t stream_id, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN]; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, buf, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); +#else + mavlink_video_stream_status_t packet; + packet.framerate = framerate; + packet.bitrate = bitrate; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.rotation = rotation; + packet.hfov = hfov; + packet.stream_id = stream_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, (const char *)&packet, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); +#endif +} + +/** + * @brief Send a video_stream_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_video_stream_status_send_struct(mavlink_channel_t chan, const mavlink_video_stream_status_t* video_stream_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_video_stream_status_send(chan, video_stream_status->stream_id, video_stream_status->flags, video_stream_status->framerate, video_stream_status->resolution_h, video_stream_status->resolution_v, video_stream_status->bitrate, video_stream_status->rotation, video_stream_status->hfov); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, (const char *)video_stream_status, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_video_stream_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t stream_id, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, buf, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); +#else + mavlink_video_stream_status_t *packet = (mavlink_video_stream_status_t *)msgbuf; + packet->framerate = framerate; + packet->bitrate = bitrate; + packet->flags = flags; + packet->resolution_h = resolution_h; + packet->resolution_v = resolution_v; + packet->rotation = rotation; + packet->hfov = hfov; + packet->stream_id = stream_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, (const char *)packet, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE VIDEO_STREAM_STATUS UNPACKING + + +/** + * @brief Get field stream_id from video_stream_status message + * + * @return Video Stream ID (1 for first, 2 for second, etc.) + */ +static inline uint8_t mavlink_msg_video_stream_status_get_stream_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 18); +} + +/** + * @brief Get field flags from video_stream_status message + * + * @return Bitmap of stream status flags + */ +static inline uint16_t mavlink_msg_video_stream_status_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field framerate from video_stream_status message + * + * @return [Hz] Frame rate + */ +static inline float mavlink_msg_video_stream_status_get_framerate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field resolution_h from video_stream_status message + * + * @return [pix] Horizontal resolution + */ +static inline uint16_t mavlink_msg_video_stream_status_get_resolution_h(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field resolution_v from video_stream_status message + * + * @return [pix] Vertical resolution + */ +static inline uint16_t mavlink_msg_video_stream_status_get_resolution_v(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field bitrate from video_stream_status message + * + * @return [bits/s] Bit rate + */ +static inline uint32_t mavlink_msg_video_stream_status_get_bitrate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field rotation from video_stream_status message + * + * @return [deg] Video image rotation clockwise + */ +static inline uint16_t mavlink_msg_video_stream_status_get_rotation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Get field hfov from video_stream_status message + * + * @return [deg] Horizontal Field of view + */ +static inline uint16_t mavlink_msg_video_stream_status_get_hfov(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Decode a video_stream_status message into a struct + * + * @param msg The message to decode + * @param video_stream_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_video_stream_status_decode(const mavlink_message_t* msg, mavlink_video_stream_status_t* video_stream_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + video_stream_status->framerate = mavlink_msg_video_stream_status_get_framerate(msg); + video_stream_status->bitrate = mavlink_msg_video_stream_status_get_bitrate(msg); + video_stream_status->flags = mavlink_msg_video_stream_status_get_flags(msg); + video_stream_status->resolution_h = mavlink_msg_video_stream_status_get_resolution_h(msg); + video_stream_status->resolution_v = mavlink_msg_video_stream_status_get_resolution_v(msg); + video_stream_status->rotation = mavlink_msg_video_stream_status_get_rotation(msg); + video_stream_status->hfov = mavlink_msg_video_stream_status_get_hfov(msg); + video_stream_status->stream_id = mavlink_msg_video_stream_status_get_stream_id(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN? msg->len : MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN; + memset(video_stream_status, 0, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); + memcpy(video_stream_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_vision_position_estimate.h b/lib/main/MAVLink/common/mavlink_msg_vision_position_estimate.h index 8c5d5d2e96..438b8c5bb3 100755 --- a/lib/main/MAVLink/common/mavlink_msg_vision_position_estimate.h +++ b/lib/main/MAVLink/common/mavlink_msg_vision_position_estimate.h @@ -12,23 +12,25 @@ typedef struct __mavlink_vision_position_estimate_t { float roll; /*< [rad] Roll angle*/ float pitch; /*< [rad] Pitch angle*/ float yaw; /*< [rad] Yaw angle*/ + float covariance[21]; /*< Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.*/ + uint8_t reset_counter; /*< Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps.*/ } mavlink_vision_position_estimate_t; -#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN 32 +#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN 117 #define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_MIN_LEN 32 -#define MAVLINK_MSG_ID_102_LEN 32 +#define MAVLINK_MSG_ID_102_LEN 117 #define MAVLINK_MSG_ID_102_MIN_LEN 32 #define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC 158 #define MAVLINK_MSG_ID_102_CRC 158 - +#define MAVLINK_MSG_VISION_POSITION_ESTIMATE_FIELD_COVARIANCE_LEN 21 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE { \ 102, \ "VISION_POSITION_ESTIMATE", \ - 7, \ + 9, \ { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_position_estimate_t, usec) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_position_estimate_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_position_estimate_t, y) }, \ @@ -36,12 +38,14 @@ typedef struct __mavlink_vision_position_estimate_t { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vision_position_estimate_t, roll) }, \ { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vision_position_estimate_t, pitch) }, \ { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vision_position_estimate_t, yaw) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 32, offsetof(mavlink_vision_position_estimate_t, covariance) }, \ + { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 116, offsetof(mavlink_vision_position_estimate_t, reset_counter) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE { \ "VISION_POSITION_ESTIMATE", \ - 7, \ + 9, \ { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_position_estimate_t, usec) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_position_estimate_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_position_estimate_t, y) }, \ @@ -49,6 +53,8 @@ typedef struct __mavlink_vision_position_estimate_t { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vision_position_estimate_t, roll) }, \ { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vision_position_estimate_t, pitch) }, \ { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vision_position_estimate_t, yaw) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 32, offsetof(mavlink_vision_position_estimate_t, covariance) }, \ + { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 116, offsetof(mavlink_vision_position_estimate_t, reset_counter) }, \ } \ } #endif @@ -66,10 +72,12 @@ typedef struct __mavlink_vision_position_estimate_t { * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) + uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN]; @@ -80,7 +88,8 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_ _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); #else mavlink_vision_position_estimate_t packet; @@ -91,7 +100,8 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_ packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); #endif @@ -112,11 +122,13 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_ * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) + uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw,const float *covariance,uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN]; @@ -127,7 +139,8 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t sy _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); #else mavlink_vision_position_estimate_t packet; @@ -138,7 +151,8 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t sy packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); #endif @@ -156,7 +170,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t sy */ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vision_position_estimate_t* vision_position_estimate) { - return mavlink_msg_vision_position_estimate_pack(system_id, component_id, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); + return mavlink_msg_vision_position_estimate_pack(system_id, component_id, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw, vision_position_estimate->covariance, vision_position_estimate->reset_counter); } /** @@ -170,7 +184,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t syste */ static inline uint16_t mavlink_msg_vision_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vision_position_estimate_t* vision_position_estimate) { - return mavlink_msg_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); + return mavlink_msg_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw, vision_position_estimate->covariance, vision_position_estimate->reset_counter); } /** @@ -184,10 +198,12 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode_chan(uint8_t * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN]; @@ -198,7 +214,8 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); #else mavlink_vision_position_estimate_t packet; @@ -209,7 +226,8 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); #endif } @@ -222,7 +240,7 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c static inline void mavlink_msg_vision_position_estimate_send_struct(mavlink_channel_t chan, const mavlink_vision_position_estimate_t* vision_position_estimate) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_vision_position_estimate_send(chan, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); + mavlink_msg_vision_position_estimate_send(chan, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw, vision_position_estimate->covariance, vision_position_estimate->reset_counter); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)vision_position_estimate, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); #endif @@ -236,7 +254,7 @@ static inline void mavlink_msg_vision_position_estimate_send_struct(mavlink_chan is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +static inline void mavlink_msg_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -247,7 +265,8 @@ static inline void mavlink_msg_vision_position_estimate_send_buf(mavlink_message _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); #else mavlink_vision_position_estimate_t *packet = (mavlink_vision_position_estimate_t *)msgbuf; @@ -258,7 +277,8 @@ static inline void mavlink_msg_vision_position_estimate_send_buf(mavlink_message packet->roll = roll; packet->pitch = pitch; packet->yaw = yaw; - + packet->reset_counter = reset_counter; + mav_array_memcpy(packet->covariance, covariance, sizeof(float)*21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); #endif } @@ -339,6 +359,26 @@ static inline float mavlink_msg_vision_position_estimate_get_yaw(const mavlink_m return _MAV_RETURN_float(msg, 28); } +/** + * @brief Get field covariance from vision_position_estimate message + * + * @return Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + */ +static inline uint16_t mavlink_msg_vision_position_estimate_get_covariance(const mavlink_message_t* msg, float *covariance) +{ + return _MAV_RETURN_float_array(msg, covariance, 21, 32); +} + +/** + * @brief Get field reset_counter from vision_position_estimate message + * + * @return Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + */ +static inline uint8_t mavlink_msg_vision_position_estimate_get_reset_counter(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 116); +} + /** * @brief Decode a vision_position_estimate message into a struct * @@ -355,6 +395,8 @@ static inline void mavlink_msg_vision_position_estimate_decode(const mavlink_mes vision_position_estimate->roll = mavlink_msg_vision_position_estimate_get_roll(msg); vision_position_estimate->pitch = mavlink_msg_vision_position_estimate_get_pitch(msg); vision_position_estimate->yaw = mavlink_msg_vision_position_estimate_get_yaw(msg); + mavlink_msg_vision_position_estimate_get_covariance(msg, vision_position_estimate->covariance); + vision_position_estimate->reset_counter = mavlink_msg_vision_position_estimate_get_reset_counter(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN? msg->len : MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN; memset(vision_position_estimate, 0, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_vision_speed_estimate.h b/lib/main/MAVLink/common/mavlink_msg_vision_speed_estimate.h index b3f3003636..7c993e8fd6 100755 --- a/lib/main/MAVLink/common/mavlink_msg_vision_speed_estimate.h +++ b/lib/main/MAVLink/common/mavlink_msg_vision_speed_estimate.h @@ -9,37 +9,43 @@ typedef struct __mavlink_vision_speed_estimate_t { float x; /*< [m/s] Global X speed*/ float y; /*< [m/s] Global Y speed*/ float z; /*< [m/s] Global Z speed*/ + float covariance[9]; /*< Row-major representation of 3x3 linear velocity covariance matrix (states: vx, vy, vz; 1st three entries - 1st row, etc.). If unknown, assign NaN value to first element in the array.*/ + uint8_t reset_counter; /*< Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps.*/ } mavlink_vision_speed_estimate_t; -#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN 20 +#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN 57 #define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_MIN_LEN 20 -#define MAVLINK_MSG_ID_103_LEN 20 +#define MAVLINK_MSG_ID_103_LEN 57 #define MAVLINK_MSG_ID_103_MIN_LEN 20 #define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC 208 #define MAVLINK_MSG_ID_103_CRC 208 - +#define MAVLINK_MSG_VISION_SPEED_ESTIMATE_FIELD_COVARIANCE_LEN 9 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE { \ 103, \ "VISION_SPEED_ESTIMATE", \ - 4, \ + 6, \ { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_speed_estimate_t, usec) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_speed_estimate_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_speed_estimate_t, y) }, \ { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vision_speed_estimate_t, z) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 9, 20, offsetof(mavlink_vision_speed_estimate_t, covariance) }, \ + { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 56, offsetof(mavlink_vision_speed_estimate_t, reset_counter) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE { \ "VISION_SPEED_ESTIMATE", \ - 4, \ + 6, \ { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_speed_estimate_t, usec) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_speed_estimate_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_speed_estimate_t, y) }, \ { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vision_speed_estimate_t, z) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 9, 20, offsetof(mavlink_vision_speed_estimate_t, covariance) }, \ + { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 56, offsetof(mavlink_vision_speed_estimate_t, reset_counter) }, \ } \ } #endif @@ -54,10 +60,12 @@ typedef struct __mavlink_vision_speed_estimate_t { * @param x [m/s] Global X speed * @param y [m/s] Global Y speed * @param z [m/s] Global Z speed + * @param covariance Row-major representation of 3x3 linear velocity covariance matrix (states: vx, vy, vz; 1st three entries - 1st row, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, float x, float y, float z) + uint64_t usec, float x, float y, float z, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN]; @@ -65,7 +73,8 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, _mav_put_float(buf, 8, x); _mav_put_float(buf, 12, y); _mav_put_float(buf, 16, z); - + _mav_put_uint8_t(buf, 56, reset_counter); + _mav_put_float_array(buf, 20, covariance, 9); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); #else mavlink_vision_speed_estimate_t packet; @@ -73,7 +82,8 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, packet.x = x; packet.y = y; packet.z = z; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); #endif @@ -91,11 +101,13 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, * @param x [m/s] Global X speed * @param y [m/s] Global Y speed * @param z [m/s] Global Z speed + * @param covariance Row-major representation of 3x3 linear velocity covariance matrix (states: vx, vy, vz; 1st three entries - 1st row, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t usec,float x,float y,float z) + uint64_t usec,float x,float y,float z,const float *covariance,uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN]; @@ -103,7 +115,8 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t syste _mav_put_float(buf, 8, x); _mav_put_float(buf, 12, y); _mav_put_float(buf, 16, z); - + _mav_put_uint8_t(buf, 56, reset_counter); + _mav_put_float_array(buf, 20, covariance, 9); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); #else mavlink_vision_speed_estimate_t packet; @@ -111,7 +124,8 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t syste packet.x = x; packet.y = y; packet.z = z; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); #endif @@ -129,7 +143,7 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t syste */ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate) { - return mavlink_msg_vision_speed_estimate_pack(system_id, component_id, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); + return mavlink_msg_vision_speed_estimate_pack(system_id, component_id, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z, vision_speed_estimate->covariance, vision_speed_estimate->reset_counter); } /** @@ -143,7 +157,7 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_i */ static inline uint16_t mavlink_msg_vision_speed_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate) { - return mavlink_msg_vision_speed_estimate_pack_chan(system_id, component_id, chan, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); + return mavlink_msg_vision_speed_estimate_pack_chan(system_id, component_id, chan, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z, vision_speed_estimate->covariance, vision_speed_estimate->reset_counter); } /** @@ -154,10 +168,12 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode_chan(uint8_t sys * @param x [m/s] Global X speed * @param y [m/s] Global Y speed * @param z [m/s] Global Z speed + * @param covariance Row-major representation of 3x3 linear velocity covariance matrix (states: vx, vy, vz; 1st three entries - 1st row, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z) +static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN]; @@ -165,7 +181,8 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan _mav_put_float(buf, 8, x); _mav_put_float(buf, 12, y); _mav_put_float(buf, 16, z); - + _mav_put_uint8_t(buf, 56, reset_counter); + _mav_put_float_array(buf, 20, covariance, 9); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); #else mavlink_vision_speed_estimate_t packet; @@ -173,7 +190,8 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan packet.x = x; packet.y = y; packet.z = z; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); #endif } @@ -186,7 +204,7 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan static inline void mavlink_msg_vision_speed_estimate_send_struct(mavlink_channel_t chan, const mavlink_vision_speed_estimate_t* vision_speed_estimate) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_vision_speed_estimate_send(chan, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); + mavlink_msg_vision_speed_estimate_send(chan, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z, vision_speed_estimate->covariance, vision_speed_estimate->reset_counter); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)vision_speed_estimate, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); #endif @@ -200,7 +218,7 @@ static inline void mavlink_msg_vision_speed_estimate_send_struct(mavlink_channel is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_vision_speed_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z) +static inline void mavlink_msg_vision_speed_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,7 +226,8 @@ static inline void mavlink_msg_vision_speed_estimate_send_buf(mavlink_message_t _mav_put_float(buf, 8, x); _mav_put_float(buf, 12, y); _mav_put_float(buf, 16, z); - + _mav_put_uint8_t(buf, 56, reset_counter); + _mav_put_float_array(buf, 20, covariance, 9); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); #else mavlink_vision_speed_estimate_t *packet = (mavlink_vision_speed_estimate_t *)msgbuf; @@ -216,7 +235,8 @@ static inline void mavlink_msg_vision_speed_estimate_send_buf(mavlink_message_t packet->x = x; packet->y = y; packet->z = z; - + packet->reset_counter = reset_counter; + mav_array_memcpy(packet->covariance, covariance, sizeof(float)*9); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); #endif } @@ -267,6 +287,26 @@ static inline float mavlink_msg_vision_speed_estimate_get_z(const mavlink_messag return _MAV_RETURN_float(msg, 16); } +/** + * @brief Get field covariance from vision_speed_estimate message + * + * @return Row-major representation of 3x3 linear velocity covariance matrix (states: vx, vy, vz; 1st three entries - 1st row, etc.). If unknown, assign NaN value to first element in the array. + */ +static inline uint16_t mavlink_msg_vision_speed_estimate_get_covariance(const mavlink_message_t* msg, float *covariance) +{ + return _MAV_RETURN_float_array(msg, covariance, 9, 20); +} + +/** + * @brief Get field reset_counter from vision_speed_estimate message + * + * @return Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + */ +static inline uint8_t mavlink_msg_vision_speed_estimate_get_reset_counter(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 56); +} + /** * @brief Decode a vision_speed_estimate message into a struct * @@ -280,6 +320,8 @@ static inline void mavlink_msg_vision_speed_estimate_decode(const mavlink_messag vision_speed_estimate->x = mavlink_msg_vision_speed_estimate_get_x(msg); vision_speed_estimate->y = mavlink_msg_vision_speed_estimate_get_y(msg); vision_speed_estimate->z = mavlink_msg_vision_speed_estimate_get_z(msg); + mavlink_msg_vision_speed_estimate_get_covariance(msg, vision_speed_estimate->covariance); + vision_speed_estimate->reset_counter = mavlink_msg_vision_speed_estimate_get_reset_counter(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN? msg->len : MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN; memset(vision_speed_estimate, 0, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_wheel_distance.h b/lib/main/MAVLink/common/mavlink_msg_wheel_distance.h new file mode 100644 index 0000000000..d506f00360 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_wheel_distance.h @@ -0,0 +1,255 @@ +#pragma once +// MESSAGE WHEEL_DISTANCE PACKING + +#define MAVLINK_MSG_ID_WHEEL_DISTANCE 9000 + + +typedef struct __mavlink_wheel_distance_t { + uint64_t time_usec; /*< [us] Timestamp (synced to UNIX time or since system boot).*/ + double distance[16]; /*< [m] Distance reported by individual wheel encoders. Forward rotations increase values, reverse rotations decrease them. Not all wheels will necessarily have wheel encoders; the mapping of encoders to wheel positions must be agreed/understood by the endpoints.*/ + uint8_t count; /*< Number of wheels reported.*/ +} mavlink_wheel_distance_t; + +#define MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN 137 +#define MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN 137 +#define MAVLINK_MSG_ID_9000_LEN 137 +#define MAVLINK_MSG_ID_9000_MIN_LEN 137 + +#define MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC 113 +#define MAVLINK_MSG_ID_9000_CRC 113 + +#define MAVLINK_MSG_WHEEL_DISTANCE_FIELD_DISTANCE_LEN 16 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_WHEEL_DISTANCE { \ + 9000, \ + "WHEEL_DISTANCE", \ + 3, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_wheel_distance_t, time_usec) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 136, offsetof(mavlink_wheel_distance_t, count) }, \ + { "distance", NULL, MAVLINK_TYPE_DOUBLE, 16, 8, offsetof(mavlink_wheel_distance_t, distance) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_WHEEL_DISTANCE { \ + "WHEEL_DISTANCE", \ + 3, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_wheel_distance_t, time_usec) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 136, offsetof(mavlink_wheel_distance_t, count) }, \ + { "distance", NULL, MAVLINK_TYPE_DOUBLE, 16, 8, offsetof(mavlink_wheel_distance_t, distance) }, \ + } \ +} +#endif + +/** + * @brief Pack a wheel_distance message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (synced to UNIX time or since system boot). + * @param count Number of wheels reported. + * @param distance [m] Distance reported by individual wheel encoders. Forward rotations increase values, reverse rotations decrease them. Not all wheels will necessarily have wheel encoders; the mapping of encoders to wheel positions must be agreed/understood by the endpoints. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_wheel_distance_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t count, const double *distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 136, count); + _mav_put_double_array(buf, 8, distance, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN); +#else + mavlink_wheel_distance_t packet; + packet.time_usec = time_usec; + packet.count = count; + mav_array_memcpy(packet.distance, distance, sizeof(double)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WHEEL_DISTANCE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); +} + +/** + * @brief Pack a wheel_distance message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (synced to UNIX time or since system boot). + * @param count Number of wheels reported. + * @param distance [m] Distance reported by individual wheel encoders. Forward rotations increase values, reverse rotations decrease them. Not all wheels will necessarily have wheel encoders; the mapping of encoders to wheel positions must be agreed/understood by the endpoints. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_wheel_distance_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t count,const double *distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 136, count); + _mav_put_double_array(buf, 8, distance, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN); +#else + mavlink_wheel_distance_t packet; + packet.time_usec = time_usec; + packet.count = count; + mav_array_memcpy(packet.distance, distance, sizeof(double)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WHEEL_DISTANCE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); +} + +/** + * @brief Encode a wheel_distance struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param wheel_distance C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_wheel_distance_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_wheel_distance_t* wheel_distance) +{ + return mavlink_msg_wheel_distance_pack(system_id, component_id, msg, wheel_distance->time_usec, wheel_distance->count, wheel_distance->distance); +} + +/** + * @brief Encode a wheel_distance struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param wheel_distance C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_wheel_distance_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_wheel_distance_t* wheel_distance) +{ + return mavlink_msg_wheel_distance_pack_chan(system_id, component_id, chan, msg, wheel_distance->time_usec, wheel_distance->count, wheel_distance->distance); +} + +/** + * @brief Send a wheel_distance message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (synced to UNIX time or since system boot). + * @param count Number of wheels reported. + * @param distance [m] Distance reported by individual wheel encoders. Forward rotations increase values, reverse rotations decrease them. Not all wheels will necessarily have wheel encoders; the mapping of encoders to wheel positions must be agreed/understood by the endpoints. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_wheel_distance_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t count, const double *distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 136, count); + _mav_put_double_array(buf, 8, distance, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WHEEL_DISTANCE, buf, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); +#else + mavlink_wheel_distance_t packet; + packet.time_usec = time_usec; + packet.count = count; + mav_array_memcpy(packet.distance, distance, sizeof(double)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WHEEL_DISTANCE, (const char *)&packet, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); +#endif +} + +/** + * @brief Send a wheel_distance message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_wheel_distance_send_struct(mavlink_channel_t chan, const mavlink_wheel_distance_t* wheel_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_wheel_distance_send(chan, wheel_distance->time_usec, wheel_distance->count, wheel_distance->distance); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WHEEL_DISTANCE, (const char *)wheel_distance, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_wheel_distance_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t count, const double *distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 136, count); + _mav_put_double_array(buf, 8, distance, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WHEEL_DISTANCE, buf, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); +#else + mavlink_wheel_distance_t *packet = (mavlink_wheel_distance_t *)msgbuf; + packet->time_usec = time_usec; + packet->count = count; + mav_array_memcpy(packet->distance, distance, sizeof(double)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WHEEL_DISTANCE, (const char *)packet, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE WHEEL_DISTANCE UNPACKING + + +/** + * @brief Get field time_usec from wheel_distance message + * + * @return [us] Timestamp (synced to UNIX time or since system boot). + */ +static inline uint64_t mavlink_msg_wheel_distance_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field count from wheel_distance message + * + * @return Number of wheels reported. + */ +static inline uint8_t mavlink_msg_wheel_distance_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 136); +} + +/** + * @brief Get field distance from wheel_distance message + * + * @return [m] Distance reported by individual wheel encoders. Forward rotations increase values, reverse rotations decrease them. Not all wheels will necessarily have wheel encoders; the mapping of encoders to wheel positions must be agreed/understood by the endpoints. + */ +static inline uint16_t mavlink_msg_wheel_distance_get_distance(const mavlink_message_t* msg, double *distance) +{ + return _MAV_RETURN_double_array(msg, distance, 16, 8); +} + +/** + * @brief Decode a wheel_distance message into a struct + * + * @param msg The message to decode + * @param wheel_distance C-struct to decode the message contents into + */ +static inline void mavlink_msg_wheel_distance_decode(const mavlink_message_t* msg, mavlink_wheel_distance_t* wheel_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + wheel_distance->time_usec = mavlink_msg_wheel_distance_get_time_usec(msg); + mavlink_msg_wheel_distance_get_distance(msg, wheel_distance->distance); + wheel_distance->count = mavlink_msg_wheel_distance_get_count(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN? msg->len : MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN; + memset(wheel_distance, 0, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN); + memcpy(wheel_distance, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_wifi_config_ap.h b/lib/main/MAVLink/common/mavlink_msg_wifi_config_ap.h new file mode 100644 index 0000000000..f5abc2473b --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_wifi_config_ap.h @@ -0,0 +1,281 @@ +#pragma once +// MESSAGE WIFI_CONFIG_AP PACKING + +#define MAVLINK_MSG_ID_WIFI_CONFIG_AP 299 + + +typedef struct __mavlink_wifi_config_ap_t { + char ssid[32]; /*< Name of Wi-Fi network (SSID). Blank to leave it unchanged when setting. Current SSID when sent back as a response.*/ + char password[64]; /*< Password. Blank for an open AP. MD5 hash when message is sent back as a response.*/ + int8_t mode; /*< WiFi Mode.*/ + int8_t response; /*< Message acceptance response (sent back to GS).*/ +} mavlink_wifi_config_ap_t; + +#define MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN 98 +#define MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN 96 +#define MAVLINK_MSG_ID_299_LEN 98 +#define MAVLINK_MSG_ID_299_MIN_LEN 96 + +#define MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC 19 +#define MAVLINK_MSG_ID_299_CRC 19 + +#define MAVLINK_MSG_WIFI_CONFIG_AP_FIELD_SSID_LEN 32 +#define MAVLINK_MSG_WIFI_CONFIG_AP_FIELD_PASSWORD_LEN 64 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_WIFI_CONFIG_AP { \ + 299, \ + "WIFI_CONFIG_AP", \ + 4, \ + { { "ssid", NULL, MAVLINK_TYPE_CHAR, 32, 0, offsetof(mavlink_wifi_config_ap_t, ssid) }, \ + { "password", NULL, MAVLINK_TYPE_CHAR, 64, 32, offsetof(mavlink_wifi_config_ap_t, password) }, \ + { "mode", NULL, MAVLINK_TYPE_INT8_T, 0, 96, offsetof(mavlink_wifi_config_ap_t, mode) }, \ + { "response", NULL, MAVLINK_TYPE_INT8_T, 0, 97, offsetof(mavlink_wifi_config_ap_t, response) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_WIFI_CONFIG_AP { \ + "WIFI_CONFIG_AP", \ + 4, \ + { { "ssid", NULL, MAVLINK_TYPE_CHAR, 32, 0, offsetof(mavlink_wifi_config_ap_t, ssid) }, \ + { "password", NULL, MAVLINK_TYPE_CHAR, 64, 32, offsetof(mavlink_wifi_config_ap_t, password) }, \ + { "mode", NULL, MAVLINK_TYPE_INT8_T, 0, 96, offsetof(mavlink_wifi_config_ap_t, mode) }, \ + { "response", NULL, MAVLINK_TYPE_INT8_T, 0, 97, offsetof(mavlink_wifi_config_ap_t, response) }, \ + } \ +} +#endif + +/** + * @brief Pack a wifi_config_ap message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param ssid Name of Wi-Fi network (SSID). Blank to leave it unchanged when setting. Current SSID when sent back as a response. + * @param password Password. Blank for an open AP. MD5 hash when message is sent back as a response. + * @param mode WiFi Mode. + * @param response Message acceptance response (sent back to GS). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_wifi_config_ap_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const char *ssid, const char *password, int8_t mode, int8_t response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN]; + _mav_put_int8_t(buf, 96, mode); + _mav_put_int8_t(buf, 97, response); + _mav_put_char_array(buf, 0, ssid, 32); + _mav_put_char_array(buf, 32, password, 64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN); +#else + mavlink_wifi_config_ap_t packet; + packet.mode = mode; + packet.response = response; + mav_array_memcpy(packet.ssid, ssid, sizeof(char)*32); + mav_array_memcpy(packet.password, password, sizeof(char)*64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WIFI_CONFIG_AP; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); +} + +/** + * @brief Pack a wifi_config_ap message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ssid Name of Wi-Fi network (SSID). Blank to leave it unchanged when setting. Current SSID when sent back as a response. + * @param password Password. Blank for an open AP. MD5 hash when message is sent back as a response. + * @param mode WiFi Mode. + * @param response Message acceptance response (sent back to GS). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_wifi_config_ap_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + const char *ssid,const char *password,int8_t mode,int8_t response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN]; + _mav_put_int8_t(buf, 96, mode); + _mav_put_int8_t(buf, 97, response); + _mav_put_char_array(buf, 0, ssid, 32); + _mav_put_char_array(buf, 32, password, 64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN); +#else + mavlink_wifi_config_ap_t packet; + packet.mode = mode; + packet.response = response; + mav_array_memcpy(packet.ssid, ssid, sizeof(char)*32); + mav_array_memcpy(packet.password, password, sizeof(char)*64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WIFI_CONFIG_AP; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); +} + +/** + * @brief Encode a wifi_config_ap struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param wifi_config_ap C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_wifi_config_ap_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_wifi_config_ap_t* wifi_config_ap) +{ + return mavlink_msg_wifi_config_ap_pack(system_id, component_id, msg, wifi_config_ap->ssid, wifi_config_ap->password, wifi_config_ap->mode, wifi_config_ap->response); +} + +/** + * @brief Encode a wifi_config_ap struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param wifi_config_ap C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_wifi_config_ap_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_wifi_config_ap_t* wifi_config_ap) +{ + return mavlink_msg_wifi_config_ap_pack_chan(system_id, component_id, chan, msg, wifi_config_ap->ssid, wifi_config_ap->password, wifi_config_ap->mode, wifi_config_ap->response); +} + +/** + * @brief Send a wifi_config_ap message + * @param chan MAVLink channel to send the message + * + * @param ssid Name of Wi-Fi network (SSID). Blank to leave it unchanged when setting. Current SSID when sent back as a response. + * @param password Password. Blank for an open AP. MD5 hash when message is sent back as a response. + * @param mode WiFi Mode. + * @param response Message acceptance response (sent back to GS). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_wifi_config_ap_send(mavlink_channel_t chan, const char *ssid, const char *password, int8_t mode, int8_t response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN]; + _mav_put_int8_t(buf, 96, mode); + _mav_put_int8_t(buf, 97, response); + _mav_put_char_array(buf, 0, ssid, 32); + _mav_put_char_array(buf, 32, password, 64); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIFI_CONFIG_AP, buf, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); +#else + mavlink_wifi_config_ap_t packet; + packet.mode = mode; + packet.response = response; + mav_array_memcpy(packet.ssid, ssid, sizeof(char)*32); + mav_array_memcpy(packet.password, password, sizeof(char)*64); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIFI_CONFIG_AP, (const char *)&packet, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); +#endif +} + +/** + * @brief Send a wifi_config_ap message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_wifi_config_ap_send_struct(mavlink_channel_t chan, const mavlink_wifi_config_ap_t* wifi_config_ap) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_wifi_config_ap_send(chan, wifi_config_ap->ssid, wifi_config_ap->password, wifi_config_ap->mode, wifi_config_ap->response); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIFI_CONFIG_AP, (const char *)wifi_config_ap, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); +#endif +} + +#if MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_wifi_config_ap_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *ssid, const char *password, int8_t mode, int8_t response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int8_t(buf, 96, mode); + _mav_put_int8_t(buf, 97, response); + _mav_put_char_array(buf, 0, ssid, 32); + _mav_put_char_array(buf, 32, password, 64); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIFI_CONFIG_AP, buf, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); +#else + mavlink_wifi_config_ap_t *packet = (mavlink_wifi_config_ap_t *)msgbuf; + packet->mode = mode; + packet->response = response; + mav_array_memcpy(packet->ssid, ssid, sizeof(char)*32); + mav_array_memcpy(packet->password, password, sizeof(char)*64); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIFI_CONFIG_AP, (const char *)packet, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); +#endif +} +#endif + +#endif + +// MESSAGE WIFI_CONFIG_AP UNPACKING + + +/** + * @brief Get field ssid from wifi_config_ap message + * + * @return Name of Wi-Fi network (SSID). Blank to leave it unchanged when setting. Current SSID when sent back as a response. + */ +static inline uint16_t mavlink_msg_wifi_config_ap_get_ssid(const mavlink_message_t* msg, char *ssid) +{ + return _MAV_RETURN_char_array(msg, ssid, 32, 0); +} + +/** + * @brief Get field password from wifi_config_ap message + * + * @return Password. Blank for an open AP. MD5 hash when message is sent back as a response. + */ +static inline uint16_t mavlink_msg_wifi_config_ap_get_password(const mavlink_message_t* msg, char *password) +{ + return _MAV_RETURN_char_array(msg, password, 64, 32); +} + +/** + * @brief Get field mode from wifi_config_ap message + * + * @return WiFi Mode. + */ +static inline int8_t mavlink_msg_wifi_config_ap_get_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int8_t(msg, 96); +} + +/** + * @brief Get field response from wifi_config_ap message + * + * @return Message acceptance response (sent back to GS). + */ +static inline int8_t mavlink_msg_wifi_config_ap_get_response(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int8_t(msg, 97); +} + +/** + * @brief Decode a wifi_config_ap message into a struct + * + * @param msg The message to decode + * @param wifi_config_ap C-struct to decode the message contents into + */ +static inline void mavlink_msg_wifi_config_ap_decode(const mavlink_message_t* msg, mavlink_wifi_config_ap_t* wifi_config_ap) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_wifi_config_ap_get_ssid(msg, wifi_config_ap->ssid); + mavlink_msg_wifi_config_ap_get_password(msg, wifi_config_ap->password); + wifi_config_ap->mode = mavlink_msg_wifi_config_ap_get_mode(msg); + wifi_config_ap->response = mavlink_msg_wifi_config_ap_get_response(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN? msg->len : MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN; + memset(wifi_config_ap, 0, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN); + memcpy(wifi_config_ap, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_winch_status.h b/lib/main/MAVLink/common/mavlink_msg_winch_status.h new file mode 100644 index 0000000000..62bd61695f --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_winch_status.h @@ -0,0 +1,388 @@ +#pragma once +// MESSAGE WINCH_STATUS PACKING + +#define MAVLINK_MSG_ID_WINCH_STATUS 9005 + + +typedef struct __mavlink_winch_status_t { + uint64_t time_usec; /*< [us] Timestamp (synced to UNIX time or since system boot).*/ + float line_length; /*< [m] Length of line released. NaN if unknown*/ + float speed; /*< [m/s] Speed line is being released or retracted. Positive values if being released, negative values if being retracted, NaN if unknown*/ + float tension; /*< [kg] Tension on the line. NaN if unknown*/ + float voltage; /*< [V] Voltage of the battery supplying the winch. NaN if unknown*/ + float current; /*< [A] Current draw from the winch. NaN if unknown*/ + uint32_t status; /*< Status flags*/ + int16_t temperature; /*< [degC] Temperature of the motor. INT16_MAX if unknown*/ +} mavlink_winch_status_t; + +#define MAVLINK_MSG_ID_WINCH_STATUS_LEN 34 +#define MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN 34 +#define MAVLINK_MSG_ID_9005_LEN 34 +#define MAVLINK_MSG_ID_9005_MIN_LEN 34 + +#define MAVLINK_MSG_ID_WINCH_STATUS_CRC 117 +#define MAVLINK_MSG_ID_9005_CRC 117 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_WINCH_STATUS { \ + 9005, \ + "WINCH_STATUS", \ + 8, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_winch_status_t, time_usec) }, \ + { "line_length", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_winch_status_t, line_length) }, \ + { "speed", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_winch_status_t, speed) }, \ + { "tension", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_winch_status_t, tension) }, \ + { "voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_winch_status_t, voltage) }, \ + { "current", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_winch_status_t, current) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 32, offsetof(mavlink_winch_status_t, temperature) }, \ + { "status", NULL, MAVLINK_TYPE_UINT32_T, 0, 28, offsetof(mavlink_winch_status_t, status) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_WINCH_STATUS { \ + "WINCH_STATUS", \ + 8, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_winch_status_t, time_usec) }, \ + { "line_length", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_winch_status_t, line_length) }, \ + { "speed", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_winch_status_t, speed) }, \ + { "tension", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_winch_status_t, tension) }, \ + { "voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_winch_status_t, voltage) }, \ + { "current", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_winch_status_t, current) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 32, offsetof(mavlink_winch_status_t, temperature) }, \ + { "status", NULL, MAVLINK_TYPE_UINT32_T, 0, 28, offsetof(mavlink_winch_status_t, status) }, \ + } \ +} +#endif + +/** + * @brief Pack a winch_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (synced to UNIX time or since system boot). + * @param line_length [m] Length of line released. NaN if unknown + * @param speed [m/s] Speed line is being released or retracted. Positive values if being released, negative values if being retracted, NaN if unknown + * @param tension [kg] Tension on the line. NaN if unknown + * @param voltage [V] Voltage of the battery supplying the winch. NaN if unknown + * @param current [A] Current draw from the winch. NaN if unknown + * @param temperature [degC] Temperature of the motor. INT16_MAX if unknown + * @param status Status flags + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_winch_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, float line_length, float speed, float tension, float voltage, float current, int16_t temperature, uint32_t status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WINCH_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, line_length); + _mav_put_float(buf, 12, speed); + _mav_put_float(buf, 16, tension); + _mav_put_float(buf, 20, voltage); + _mav_put_float(buf, 24, current); + _mav_put_uint32_t(buf, 28, status); + _mav_put_int16_t(buf, 32, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WINCH_STATUS_LEN); +#else + mavlink_winch_status_t packet; + packet.time_usec = time_usec; + packet.line_length = line_length; + packet.speed = speed; + packet.tension = tension; + packet.voltage = voltage; + packet.current = current; + packet.status = status; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WINCH_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WINCH_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN, MAVLINK_MSG_ID_WINCH_STATUS_LEN, MAVLINK_MSG_ID_WINCH_STATUS_CRC); +} + +/** + * @brief Pack a winch_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (synced to UNIX time or since system boot). + * @param line_length [m] Length of line released. NaN if unknown + * @param speed [m/s] Speed line is being released or retracted. Positive values if being released, negative values if being retracted, NaN if unknown + * @param tension [kg] Tension on the line. NaN if unknown + * @param voltage [V] Voltage of the battery supplying the winch. NaN if unknown + * @param current [A] Current draw from the winch. NaN if unknown + * @param temperature [degC] Temperature of the motor. INT16_MAX if unknown + * @param status Status flags + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_winch_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,float line_length,float speed,float tension,float voltage,float current,int16_t temperature,uint32_t status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WINCH_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, line_length); + _mav_put_float(buf, 12, speed); + _mav_put_float(buf, 16, tension); + _mav_put_float(buf, 20, voltage); + _mav_put_float(buf, 24, current); + _mav_put_uint32_t(buf, 28, status); + _mav_put_int16_t(buf, 32, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WINCH_STATUS_LEN); +#else + mavlink_winch_status_t packet; + packet.time_usec = time_usec; + packet.line_length = line_length; + packet.speed = speed; + packet.tension = tension; + packet.voltage = voltage; + packet.current = current; + packet.status = status; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WINCH_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WINCH_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN, MAVLINK_MSG_ID_WINCH_STATUS_LEN, MAVLINK_MSG_ID_WINCH_STATUS_CRC); +} + +/** + * @brief Encode a winch_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param winch_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_winch_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_winch_status_t* winch_status) +{ + return mavlink_msg_winch_status_pack(system_id, component_id, msg, winch_status->time_usec, winch_status->line_length, winch_status->speed, winch_status->tension, winch_status->voltage, winch_status->current, winch_status->temperature, winch_status->status); +} + +/** + * @brief Encode a winch_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param winch_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_winch_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_winch_status_t* winch_status) +{ + return mavlink_msg_winch_status_pack_chan(system_id, component_id, chan, msg, winch_status->time_usec, winch_status->line_length, winch_status->speed, winch_status->tension, winch_status->voltage, winch_status->current, winch_status->temperature, winch_status->status); +} + +/** + * @brief Send a winch_status message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (synced to UNIX time or since system boot). + * @param line_length [m] Length of line released. NaN if unknown + * @param speed [m/s] Speed line is being released or retracted. Positive values if being released, negative values if being retracted, NaN if unknown + * @param tension [kg] Tension on the line. NaN if unknown + * @param voltage [V] Voltage of the battery supplying the winch. NaN if unknown + * @param current [A] Current draw from the winch. NaN if unknown + * @param temperature [degC] Temperature of the motor. INT16_MAX if unknown + * @param status Status flags + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_winch_status_send(mavlink_channel_t chan, uint64_t time_usec, float line_length, float speed, float tension, float voltage, float current, int16_t temperature, uint32_t status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WINCH_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, line_length); + _mav_put_float(buf, 12, speed); + _mav_put_float(buf, 16, tension); + _mav_put_float(buf, 20, voltage); + _mav_put_float(buf, 24, current); + _mav_put_uint32_t(buf, 28, status); + _mav_put_int16_t(buf, 32, temperature); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WINCH_STATUS, buf, MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN, MAVLINK_MSG_ID_WINCH_STATUS_LEN, MAVLINK_MSG_ID_WINCH_STATUS_CRC); +#else + mavlink_winch_status_t packet; + packet.time_usec = time_usec; + packet.line_length = line_length; + packet.speed = speed; + packet.tension = tension; + packet.voltage = voltage; + packet.current = current; + packet.status = status; + packet.temperature = temperature; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WINCH_STATUS, (const char *)&packet, MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN, MAVLINK_MSG_ID_WINCH_STATUS_LEN, MAVLINK_MSG_ID_WINCH_STATUS_CRC); +#endif +} + +/** + * @brief Send a winch_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_winch_status_send_struct(mavlink_channel_t chan, const mavlink_winch_status_t* winch_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_winch_status_send(chan, winch_status->time_usec, winch_status->line_length, winch_status->speed, winch_status->tension, winch_status->voltage, winch_status->current, winch_status->temperature, winch_status->status); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WINCH_STATUS, (const char *)winch_status, MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN, MAVLINK_MSG_ID_WINCH_STATUS_LEN, MAVLINK_MSG_ID_WINCH_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_WINCH_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_winch_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float line_length, float speed, float tension, float voltage, float current, int16_t temperature, uint32_t status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, line_length); + _mav_put_float(buf, 12, speed); + _mav_put_float(buf, 16, tension); + _mav_put_float(buf, 20, voltage); + _mav_put_float(buf, 24, current); + _mav_put_uint32_t(buf, 28, status); + _mav_put_int16_t(buf, 32, temperature); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WINCH_STATUS, buf, MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN, MAVLINK_MSG_ID_WINCH_STATUS_LEN, MAVLINK_MSG_ID_WINCH_STATUS_CRC); +#else + mavlink_winch_status_t *packet = (mavlink_winch_status_t *)msgbuf; + packet->time_usec = time_usec; + packet->line_length = line_length; + packet->speed = speed; + packet->tension = tension; + packet->voltage = voltage; + packet->current = current; + packet->status = status; + packet->temperature = temperature; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WINCH_STATUS, (const char *)packet, MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN, MAVLINK_MSG_ID_WINCH_STATUS_LEN, MAVLINK_MSG_ID_WINCH_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE WINCH_STATUS UNPACKING + + +/** + * @brief Get field time_usec from winch_status message + * + * @return [us] Timestamp (synced to UNIX time or since system boot). + */ +static inline uint64_t mavlink_msg_winch_status_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field line_length from winch_status message + * + * @return [m] Length of line released. NaN if unknown + */ +static inline float mavlink_msg_winch_status_get_line_length(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field speed from winch_status message + * + * @return [m/s] Speed line is being released or retracted. Positive values if being released, negative values if being retracted, NaN if unknown + */ +static inline float mavlink_msg_winch_status_get_speed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field tension from winch_status message + * + * @return [kg] Tension on the line. NaN if unknown + */ +static inline float mavlink_msg_winch_status_get_tension(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field voltage from winch_status message + * + * @return [V] Voltage of the battery supplying the winch. NaN if unknown + */ +static inline float mavlink_msg_winch_status_get_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field current from winch_status message + * + * @return [A] Current draw from the winch. NaN if unknown + */ +static inline float mavlink_msg_winch_status_get_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field temperature from winch_status message + * + * @return [degC] Temperature of the motor. INT16_MAX if unknown + */ +static inline int16_t mavlink_msg_winch_status_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 32); +} + +/** + * @brief Get field status from winch_status message + * + * @return Status flags + */ +static inline uint32_t mavlink_msg_winch_status_get_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 28); +} + +/** + * @brief Decode a winch_status message into a struct + * + * @param msg The message to decode + * @param winch_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_winch_status_decode(const mavlink_message_t* msg, mavlink_winch_status_t* winch_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + winch_status->time_usec = mavlink_msg_winch_status_get_time_usec(msg); + winch_status->line_length = mavlink_msg_winch_status_get_line_length(msg); + winch_status->speed = mavlink_msg_winch_status_get_speed(msg); + winch_status->tension = mavlink_msg_winch_status_get_tension(msg); + winch_status->voltage = mavlink_msg_winch_status_get_voltage(msg); + winch_status->current = mavlink_msg_winch_status_get_current(msg); + winch_status->status = mavlink_msg_winch_status_get_status(msg); + winch_status->temperature = mavlink_msg_winch_status_get_temperature(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_WINCH_STATUS_LEN? msg->len : MAVLINK_MSG_ID_WINCH_STATUS_LEN; + memset(winch_status, 0, MAVLINK_MSG_ID_WINCH_STATUS_LEN); + memcpy(winch_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/testsuite.h b/lib/main/MAVLink/common/testsuite.h index a450230f3d..8cea375547 100755 --- a/lib/main/MAVLink/common/testsuite.h +++ b/lib/main/MAVLink/common/testsuite.h @@ -12,78 +12,19 @@ extern "C" { #ifndef MAVLINK_TEST_ALL #define MAVLINK_TEST_ALL - +static void mavlink_test_minimal(uint8_t, uint8_t, mavlink_message_t *last_msg); static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { - + mavlink_test_minimal(system_id, component_id, last_msg); mavlink_test_common(system_id, component_id, last_msg); } #endif +#include "../minimal/testsuite.h" - -static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ -#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 - mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0); - if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_HEARTBEAT >= 256) { - return; - } -#endif - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_heartbeat_t packet_in = { - 963497464,17,84,151,218,3 - }; - mavlink_heartbeat_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.custom_mode = packet_in.custom_mode; - packet1.type = packet_in.type; - packet1.autopilot = packet_in.autopilot; - packet1.base_mode = packet_in.base_mode; - packet1.system_status = packet_in.system_status; - packet1.mavlink_version = packet_in.mavlink_version; - - -#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 - if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { - // cope with extensions - memset(MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN); - } -#endif - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_heartbeat_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_heartbeat_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_heartbeat_pack(system_id, component_id, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); - mavlink_msg_heartbeat_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); - mavlink_msg_heartbeat_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -1297,12 +1249,12 @@ static void mavlink_test_attitude_quaternion(uint8_t system_id, uint8_t componen MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_attitude_quaternion_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed ); + mavlink_msg_attitude_quaternion_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.repr_offset_q ); mavlink_msg_attitude_quaternion_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_attitude_quaternion_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed ); + mavlink_msg_attitude_quaternion_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.repr_offset_q ); mavlink_msg_attitude_quaternion_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -1315,7 +1267,7 @@ static void mavlink_test_attitude_quaternion(uint8_t system_id, uint8_t componen MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed ); + mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.repr_offset_q ); mavlink_msg_attitude_quaternion_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -1582,7 +1534,7 @@ static void mavlink_test_servo_output_raw(uint8_t system_id, uint8_t component_i uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_servo_output_raw_t packet_in = { - 963497464,17443,17547,17651,17755,17859,17963,18067,18171,65 + 963497464,17443,17547,17651,17755,17859,17963,18067,18171,65,18327,18431,18535,18639,18743,18847,18951,19055 }; mavlink_servo_output_raw_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1596,6 +1548,14 @@ static void mavlink_test_servo_output_raw(uint8_t system_id, uint8_t component_i packet1.servo7_raw = packet_in.servo7_raw; packet1.servo8_raw = packet_in.servo8_raw; packet1.port = packet_in.port; + packet1.servo9_raw = packet_in.servo9_raw; + packet1.servo10_raw = packet_in.servo10_raw; + packet1.servo11_raw = packet_in.servo11_raw; + packet1.servo12_raw = packet_in.servo12_raw; + packet1.servo13_raw = packet_in.servo13_raw; + packet1.servo14_raw = packet_in.servo14_raw; + packet1.servo15_raw = packet_in.servo15_raw; + packet1.servo16_raw = packet_in.servo16_raw; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -1610,12 +1570,12 @@ static void mavlink_test_servo_output_raw(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_servo_output_raw_pack(system_id, component_id, &msg , packet1.time_usec , packet1.port , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw ); + mavlink_msg_servo_output_raw_pack(system_id, component_id, &msg , packet1.time_usec , packet1.port , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw , packet1.servo9_raw , packet1.servo10_raw , packet1.servo11_raw , packet1.servo12_raw , packet1.servo13_raw , packet1.servo14_raw , packet1.servo15_raw , packet1.servo16_raw ); mavlink_msg_servo_output_raw_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.port , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw ); + mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.port , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw , packet1.servo9_raw , packet1.servo10_raw , packet1.servo11_raw , packet1.servo12_raw , packet1.servo13_raw , packet1.servo14_raw , packet1.servo15_raw , packet1.servo16_raw ); mavlink_msg_servo_output_raw_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -1628,7 +1588,7 @@ static void mavlink_test_servo_output_raw(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.port , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw ); + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.port , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw , packet1.servo9_raw , packet1.servo10_raw , packet1.servo11_raw , packet1.servo12_raw , packet1.servo13_raw , packet1.servo14_raw , packet1.servo15_raw , packet1.servo16_raw ); mavlink_msg_servo_output_raw_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -1645,7 +1605,7 @@ static void mavlink_test_mission_request_partial_list(uint8_t system_id, uint8_t uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_request_partial_list_t packet_in = { - 17235,17339,17,84 + 17235,17339,17,84,151 }; mavlink_mission_request_partial_list_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1653,6 +1613,7 @@ static void mavlink_test_mission_request_partial_list(uint8_t system_id, uint8_t packet1.end_index = packet_in.end_index; packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -1667,12 +1628,12 @@ static void mavlink_test_mission_request_partial_list(uint8_t system_id, uint8_t MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_partial_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index ); + mavlink_msg_mission_request_partial_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index , packet1.mission_type ); mavlink_msg_mission_request_partial_list_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_partial_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index ); + mavlink_msg_mission_request_partial_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index , packet1.mission_type ); mavlink_msg_mission_request_partial_list_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -1685,7 +1646,7 @@ static void mavlink_test_mission_request_partial_list(uint8_t system_id, uint8_t MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_partial_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index ); + mavlink_msg_mission_request_partial_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index , packet1.mission_type ); mavlink_msg_mission_request_partial_list_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -1702,7 +1663,7 @@ static void mavlink_test_mission_write_partial_list(uint8_t system_id, uint8_t c uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_write_partial_list_t packet_in = { - 17235,17339,17,84 + 17235,17339,17,84,151 }; mavlink_mission_write_partial_list_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1710,6 +1671,7 @@ static void mavlink_test_mission_write_partial_list(uint8_t system_id, uint8_t c packet1.end_index = packet_in.end_index; packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -1724,12 +1686,12 @@ static void mavlink_test_mission_write_partial_list(uint8_t system_id, uint8_t c MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_write_partial_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index ); + mavlink_msg_mission_write_partial_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index , packet1.mission_type ); mavlink_msg_mission_write_partial_list_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_write_partial_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index ); + mavlink_msg_mission_write_partial_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index , packet1.mission_type ); mavlink_msg_mission_write_partial_list_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -1742,7 +1704,7 @@ static void mavlink_test_mission_write_partial_list(uint8_t system_id, uint8_t c MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_write_partial_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index ); + mavlink_msg_mission_write_partial_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index , packet1.mission_type ); mavlink_msg_mission_write_partial_list_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -1759,7 +1721,7 @@ static void mavlink_test_mission_item(uint8_t system_id, uint8_t component_id, m uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_item_t packet_in = { - 17.0,45.0,73.0,101.0,129.0,157.0,185.0,18691,18795,101,168,235,46,113 + 17.0,45.0,73.0,101.0,129.0,157.0,185.0,18691,18795,101,168,235,46,113,180 }; mavlink_mission_item_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1777,6 +1739,7 @@ static void mavlink_test_mission_item(uint8_t system_id, uint8_t component_id, m packet1.frame = packet_in.frame; packet1.current = packet_in.current; packet1.autocontinue = packet_in.autocontinue; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -1791,12 +1754,12 @@ static void mavlink_test_mission_item(uint8_t system_id, uint8_t component_id, m MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_item_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z ); + mavlink_msg_mission_item_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z , packet1.mission_type ); mavlink_msg_mission_item_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_item_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z ); + mavlink_msg_mission_item_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z , packet1.mission_type ); mavlink_msg_mission_item_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -1809,7 +1772,7 @@ static void mavlink_test_mission_item(uint8_t system_id, uint8_t component_id, m MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_item_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z ); + mavlink_msg_mission_item_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z , packet1.mission_type ); mavlink_msg_mission_item_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -1826,13 +1789,14 @@ static void mavlink_test_mission_request(uint8_t system_id, uint8_t component_id uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_request_t packet_in = { - 17235,139,206 + 17235,139,206,17 }; mavlink_mission_request_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.seq = packet_in.seq; packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -1847,12 +1811,12 @@ static void mavlink_test_mission_request(uint8_t system_id, uint8_t component_id MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_mission_request_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.mission_type ); mavlink_msg_mission_request_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_mission_request_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.mission_type ); mavlink_msg_mission_request_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -1865,7 +1829,7 @@ static void mavlink_test_mission_request(uint8_t system_id, uint8_t component_id MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_mission_request_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq , packet1.mission_type ); mavlink_msg_mission_request_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -1992,12 +1956,13 @@ static void mavlink_test_mission_request_list(uint8_t system_id, uint8_t compone uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_request_list_t packet_in = { - 5,72 + 5,72,139 }; mavlink_mission_request_list_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -2012,12 +1977,12 @@ static void mavlink_test_mission_request_list(uint8_t system_id, uint8_t compone MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_mission_request_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.mission_type ); mavlink_msg_mission_request_list_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_mission_request_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.mission_type ); mavlink_msg_mission_request_list_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -2030,7 +1995,7 @@ static void mavlink_test_mission_request_list(uint8_t system_id, uint8_t compone MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component ); + mavlink_msg_mission_request_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.mission_type ); mavlink_msg_mission_request_list_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -2047,13 +2012,14 @@ static void mavlink_test_mission_count(uint8_t system_id, uint8_t component_id, uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_count_t packet_in = { - 17235,139,206 + 17235,139,206,17 }; mavlink_mission_count_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.count = packet_in.count; packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -2068,12 +2034,12 @@ static void mavlink_test_mission_count(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_count_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.count ); + mavlink_msg_mission_count_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.count , packet1.mission_type ); mavlink_msg_mission_count_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_count_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.count ); + mavlink_msg_mission_count_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.count , packet1.mission_type ); mavlink_msg_mission_count_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -2086,7 +2052,7 @@ static void mavlink_test_mission_count(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_count_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.count ); + mavlink_msg_mission_count_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.count , packet1.mission_type ); mavlink_msg_mission_count_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -2103,12 +2069,13 @@ static void mavlink_test_mission_clear_all(uint8_t system_id, uint8_t component_ uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_clear_all_t packet_in = { - 5,72 + 5,72,139 }; mavlink_mission_clear_all_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -2123,12 +2090,12 @@ static void mavlink_test_mission_clear_all(uint8_t system_id, uint8_t component_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_clear_all_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_mission_clear_all_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.mission_type ); mavlink_msg_mission_clear_all_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_clear_all_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_mission_clear_all_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.mission_type ); mavlink_msg_mission_clear_all_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -2141,7 +2108,7 @@ static void mavlink_test_mission_clear_all(uint8_t system_id, uint8_t component_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_clear_all_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component ); + mavlink_msg_mission_clear_all_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.mission_type ); mavlink_msg_mission_clear_all_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -2212,13 +2179,14 @@ static void mavlink_test_mission_ack(uint8_t system_id, uint8_t component_id, ma uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_ack_t packet_in = { - 5,72,139 + 5,72,139,206 }; mavlink_mission_ack_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; packet1.type = packet_in.type; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -2233,12 +2201,12 @@ static void mavlink_test_mission_ack(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_ack_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.type ); + mavlink_msg_mission_ack_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.type , packet1.mission_type ); mavlink_msg_mission_ack_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.type ); + mavlink_msg_mission_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.type , packet1.mission_type ); mavlink_msg_mission_ack_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -2251,7 +2219,7 @@ static void mavlink_test_mission_ack(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_ack_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.type ); + mavlink_msg_mission_ack_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.type , packet1.mission_type ); mavlink_msg_mission_ack_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -2268,7 +2236,7 @@ static void mavlink_test_set_gps_global_origin(uint8_t system_id, uint8_t compon uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_set_gps_global_origin_t packet_in = { - 963497464,963497672,963497880,41 + 963497464,963497672,963497880,41,93372036854776626ULL }; mavlink_set_gps_global_origin_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2276,6 +2244,7 @@ static void mavlink_test_set_gps_global_origin(uint8_t system_id, uint8_t compon packet1.longitude = packet_in.longitude; packet1.altitude = packet_in.altitude; packet1.target_system = packet_in.target_system; + packet1.time_usec = packet_in.time_usec; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -2290,12 +2259,12 @@ static void mavlink_test_set_gps_global_origin(uint8_t system_id, uint8_t compon MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_gps_global_origin_pack(system_id, component_id, &msg , packet1.target_system , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_set_gps_global_origin_pack(system_id, component_id, &msg , packet1.target_system , packet1.latitude , packet1.longitude , packet1.altitude , packet1.time_usec ); mavlink_msg_set_gps_global_origin_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_gps_global_origin_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_set_gps_global_origin_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.latitude , packet1.longitude , packet1.altitude , packet1.time_usec ); mavlink_msg_set_gps_global_origin_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -2308,7 +2277,7 @@ static void mavlink_test_set_gps_global_origin(uint8_t system_id, uint8_t compon MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_gps_global_origin_send(MAVLINK_COMM_1 , packet1.target_system , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_set_gps_global_origin_send(MAVLINK_COMM_1 , packet1.target_system , packet1.latitude , packet1.longitude , packet1.altitude , packet1.time_usec ); mavlink_msg_set_gps_global_origin_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -2325,13 +2294,14 @@ static void mavlink_test_gps_global_origin(uint8_t system_id, uint8_t component_ uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_gps_global_origin_t packet_in = { - 963497464,963497672,963497880 + 963497464,963497672,963497880,93372036854776563ULL }; mavlink_gps_global_origin_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.latitude = packet_in.latitude; packet1.longitude = packet_in.longitude; packet1.altitude = packet_in.altitude; + packet1.time_usec = packet_in.time_usec; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -2346,12 +2316,12 @@ static void mavlink_test_gps_global_origin(uint8_t system_id, uint8_t component_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_gps_global_origin_pack(system_id, component_id, &msg , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_gps_global_origin_pack(system_id, component_id, &msg , packet1.latitude , packet1.longitude , packet1.altitude , packet1.time_usec ); mavlink_msg_gps_global_origin_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_gps_global_origin_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_gps_global_origin_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.latitude , packet1.longitude , packet1.altitude , packet1.time_usec ); mavlink_msg_gps_global_origin_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -2364,7 +2334,7 @@ static void mavlink_test_gps_global_origin(uint8_t system_id, uint8_t component_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_gps_global_origin_send(MAVLINK_COMM_1 , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_gps_global_origin_send(MAVLINK_COMM_1 , packet1.latitude , packet1.longitude , packet1.altitude , packet1.time_usec ); mavlink_msg_gps_global_origin_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -2443,13 +2413,14 @@ static void mavlink_test_mission_request_int(uint8_t system_id, uint8_t componen uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_request_int_t packet_in = { - 17235,139,206 + 17235,139,206,17 }; mavlink_mission_request_int_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.seq = packet_in.seq; packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -2464,12 +2435,12 @@ static void mavlink_test_mission_request_int(uint8_t system_id, uint8_t componen MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_int_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_mission_request_int_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.mission_type ); mavlink_msg_mission_request_int_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_mission_request_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.mission_type ); mavlink_msg_mission_request_int_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -2482,7 +2453,7 @@ static void mavlink_test_mission_request_int(uint8_t system_id, uint8_t componen MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_int_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_mission_request_int_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq , packet1.mission_type ); mavlink_msg_mission_request_int_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -3174,7 +3145,7 @@ static void mavlink_test_rc_channels_override(uint8_t system_id, uint8_t compone uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_rc_channels_override_t packet_in = { - 17235,17339,17443,17547,17651,17755,17859,17963,53,120 + 17235,17339,17443,17547,17651,17755,17859,17963,53,120,18171,18275,18379,18483,18587,18691,18795,18899,19003,19107 }; mavlink_rc_channels_override_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3188,6 +3159,16 @@ static void mavlink_test_rc_channels_override(uint8_t system_id, uint8_t compone packet1.chan8_raw = packet_in.chan8_raw; packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; + packet1.chan9_raw = packet_in.chan9_raw; + packet1.chan10_raw = packet_in.chan10_raw; + packet1.chan11_raw = packet_in.chan11_raw; + packet1.chan12_raw = packet_in.chan12_raw; + packet1.chan13_raw = packet_in.chan13_raw; + packet1.chan14_raw = packet_in.chan14_raw; + packet1.chan15_raw = packet_in.chan15_raw; + packet1.chan16_raw = packet_in.chan16_raw; + packet1.chan17_raw = packet_in.chan17_raw; + packet1.chan18_raw = packet_in.chan18_raw; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -3202,12 +3183,12 @@ static void mavlink_test_rc_channels_override(uint8_t system_id, uint8_t compone MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rc_channels_override_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw ); + mavlink_msg_rc_channels_override_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.chan9_raw , packet1.chan10_raw , packet1.chan11_raw , packet1.chan12_raw , packet1.chan13_raw , packet1.chan14_raw , packet1.chan15_raw , packet1.chan16_raw , packet1.chan17_raw , packet1.chan18_raw ); mavlink_msg_rc_channels_override_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rc_channels_override_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw ); + mavlink_msg_rc_channels_override_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.chan9_raw , packet1.chan10_raw , packet1.chan11_raw , packet1.chan12_raw , packet1.chan13_raw , packet1.chan14_raw , packet1.chan15_raw , packet1.chan16_raw , packet1.chan17_raw , packet1.chan18_raw ); mavlink_msg_rc_channels_override_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -3220,7 +3201,7 @@ static void mavlink_test_rc_channels_override(uint8_t system_id, uint8_t compone MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rc_channels_override_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw ); + mavlink_msg_rc_channels_override_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.chan9_raw , packet1.chan10_raw , packet1.chan11_raw , packet1.chan12_raw , packet1.chan13_raw , packet1.chan14_raw , packet1.chan15_raw , packet1.chan16_raw , packet1.chan17_raw , packet1.chan18_raw ); mavlink_msg_rc_channels_override_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -3237,7 +3218,7 @@ static void mavlink_test_mission_item_int(uint8_t system_id, uint8_t component_i uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_item_int_t packet_in = { - 17.0,45.0,73.0,101.0,963498296,963498504,185.0,18691,18795,101,168,235,46,113 + 17.0,45.0,73.0,101.0,963498296,963498504,185.0,18691,18795,101,168,235,46,113,180 }; mavlink_mission_item_int_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3255,6 +3236,7 @@ static void mavlink_test_mission_item_int(uint8_t system_id, uint8_t component_i packet1.frame = packet_in.frame; packet1.current = packet_in.current; packet1.autocontinue = packet_in.autocontinue; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -3269,12 +3251,12 @@ static void mavlink_test_mission_item_int(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_item_int_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z ); + mavlink_msg_mission_item_int_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z , packet1.mission_type ); mavlink_msg_mission_item_int_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_item_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z ); + mavlink_msg_mission_item_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z , packet1.mission_type ); mavlink_msg_mission_item_int_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -3287,7 +3269,7 @@ static void mavlink_test_mission_item_int(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_item_int_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z ); + mavlink_msg_mission_item_int_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z , packet1.mission_type ); mavlink_msg_mission_item_int_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -3493,12 +3475,16 @@ static void mavlink_test_command_ack(uint8_t system_id, uint8_t component_id, ma uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_command_ack_t packet_in = { - 17235,139 + 17235,139,206,963497672,29,96 }; mavlink_command_ack_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.command = packet_in.command; packet1.result = packet_in.result; + packet1.progress = packet_in.progress; + packet1.result_param2 = packet_in.result_param2; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -3513,12 +3499,12 @@ static void mavlink_test_command_ack(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_command_ack_pack(system_id, component_id, &msg , packet1.command , packet1.result ); + mavlink_msg_command_ack_pack(system_id, component_id, &msg , packet1.command , packet1.result , packet1.progress , packet1.result_param2 , packet1.target_system , packet1.target_component ); mavlink_msg_command_ack_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_command_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.command , packet1.result ); + mavlink_msg_command_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.command , packet1.result , packet1.progress , packet1.result_param2 , packet1.target_system , packet1.target_component ); mavlink_msg_command_ack_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -3531,7 +3517,7 @@ static void mavlink_test_command_ack(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_command_ack_send(MAVLINK_COMM_1 , packet1.command , packet1.result ); + mavlink_msg_command_ack_send(MAVLINK_COMM_1 , packet1.command , packet1.result , packet1.progress , packet1.result_param2 , packet1.target_system , packet1.target_component ); mavlink_msg_command_ack_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -4375,7 +4361,7 @@ static void mavlink_test_optical_flow(uint8_t system_id, uint8_t component_id, m uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_optical_flow_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,18275,18379,77,144 + 93372036854775807ULL,73.0,101.0,129.0,18275,18379,77,144,199.0,227.0 }; mavlink_optical_flow_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4387,6 +4373,8 @@ static void mavlink_test_optical_flow(uint8_t system_id, uint8_t component_id, m packet1.flow_y = packet_in.flow_y; packet1.sensor_id = packet_in.sensor_id; packet1.quality = packet_in.quality; + packet1.flow_rate_x = packet_in.flow_rate_x; + packet1.flow_rate_y = packet_in.flow_rate_y; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -4401,12 +4389,12 @@ static void mavlink_test_optical_flow(uint8_t system_id, uint8_t component_id, m MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_optical_flow_pack(system_id, component_id, &msg , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance ); + mavlink_msg_optical_flow_pack(system_id, component_id, &msg , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance , packet1.flow_rate_x , packet1.flow_rate_y ); mavlink_msg_optical_flow_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_optical_flow_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance ); + mavlink_msg_optical_flow_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance , packet1.flow_rate_x , packet1.flow_rate_y ); mavlink_msg_optical_flow_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -4419,7 +4407,7 @@ static void mavlink_test_optical_flow(uint8_t system_id, uint8_t component_id, m MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_optical_flow_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance ); + mavlink_msg_optical_flow_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance , packet1.flow_rate_x , packet1.flow_rate_y ); mavlink_msg_optical_flow_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -4436,7 +4424,7 @@ static void mavlink_test_global_vision_position_estimate(uint8_t system_id, uint uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_global_vision_position_estimate_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0 + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,{ 241.0, 242.0, 243.0, 244.0, 245.0, 246.0, 247.0, 248.0, 249.0, 250.0, 251.0, 252.0, 253.0, 254.0, 255.0, 256.0, 257.0, 258.0, 259.0, 260.0, 261.0 },97 }; mavlink_global_vision_position_estimate_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4447,7 +4435,9 @@ static void mavlink_test_global_vision_position_estimate(uint8_t system_id, uint packet1.roll = packet_in.roll; packet1.pitch = packet_in.pitch; packet1.yaw = packet_in.yaw; + packet1.reset_counter = packet_in.reset_counter; + mav_array_memcpy(packet1.covariance, packet_in.covariance, sizeof(float)*21); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -4461,12 +4451,12 @@ static void mavlink_test_global_vision_position_estimate(uint8_t system_id, uint MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_global_vision_position_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_global_vision_position_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance , packet1.reset_counter ); mavlink_msg_global_vision_position_estimate_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_global_vision_position_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_global_vision_position_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance , packet1.reset_counter ); mavlink_msg_global_vision_position_estimate_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -4479,7 +4469,7 @@ static void mavlink_test_global_vision_position_estimate(uint8_t system_id, uint MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_global_vision_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_global_vision_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance , packet1.reset_counter ); mavlink_msg_global_vision_position_estimate_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -4496,7 +4486,7 @@ static void mavlink_test_vision_position_estimate(uint8_t system_id, uint8_t com uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_vision_position_estimate_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0 + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,{ 241.0, 242.0, 243.0, 244.0, 245.0, 246.0, 247.0, 248.0, 249.0, 250.0, 251.0, 252.0, 253.0, 254.0, 255.0, 256.0, 257.0, 258.0, 259.0, 260.0, 261.0 },97 }; mavlink_vision_position_estimate_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4507,7 +4497,9 @@ static void mavlink_test_vision_position_estimate(uint8_t system_id, uint8_t com packet1.roll = packet_in.roll; packet1.pitch = packet_in.pitch; packet1.yaw = packet_in.yaw; + packet1.reset_counter = packet_in.reset_counter; + mav_array_memcpy(packet1.covariance, packet_in.covariance, sizeof(float)*21); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -4521,12 +4513,12 @@ static void mavlink_test_vision_position_estimate(uint8_t system_id, uint8_t com MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vision_position_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_vision_position_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance , packet1.reset_counter ); mavlink_msg_vision_position_estimate_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vision_position_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_vision_position_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance , packet1.reset_counter ); mavlink_msg_vision_position_estimate_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -4539,7 +4531,7 @@ static void mavlink_test_vision_position_estimate(uint8_t system_id, uint8_t com MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vision_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_vision_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance , packet1.reset_counter ); mavlink_msg_vision_position_estimate_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -4556,7 +4548,7 @@ static void mavlink_test_vision_speed_estimate(uint8_t system_id, uint8_t compon uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_vision_speed_estimate_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0 + 93372036854775807ULL,73.0,101.0,129.0,{ 157.0, 158.0, 159.0, 160.0, 161.0, 162.0, 163.0, 164.0, 165.0 },173 }; mavlink_vision_speed_estimate_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4564,7 +4556,9 @@ static void mavlink_test_vision_speed_estimate(uint8_t system_id, uint8_t compon packet1.x = packet_in.x; packet1.y = packet_in.y; packet1.z = packet_in.z; + packet1.reset_counter = packet_in.reset_counter; + mav_array_memcpy(packet1.covariance, packet_in.covariance, sizeof(float)*9); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -4578,12 +4572,12 @@ static void mavlink_test_vision_speed_estimate(uint8_t system_id, uint8_t compon MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vision_speed_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z ); + mavlink_msg_vision_speed_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.covariance , packet1.reset_counter ); mavlink_msg_vision_speed_estimate_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vision_speed_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z ); + mavlink_msg_vision_speed_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.covariance , packet1.reset_counter ); mavlink_msg_vision_speed_estimate_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -4596,7 +4590,7 @@ static void mavlink_test_vision_speed_estimate(uint8_t system_id, uint8_t compon MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vision_speed_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z ); + mavlink_msg_vision_speed_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.covariance , packet1.reset_counter ); mavlink_msg_vision_speed_estimate_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -4613,7 +4607,7 @@ static void mavlink_test_vicon_position_estimate(uint8_t system_id, uint8_t comp uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_vicon_position_estimate_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0 + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,{ 241.0, 242.0, 243.0, 244.0, 245.0, 246.0, 247.0, 248.0, 249.0, 250.0, 251.0, 252.0, 253.0, 254.0, 255.0, 256.0, 257.0, 258.0, 259.0, 260.0, 261.0 } }; mavlink_vicon_position_estimate_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4625,6 +4619,7 @@ static void mavlink_test_vicon_position_estimate(uint8_t system_id, uint8_t comp packet1.pitch = packet_in.pitch; packet1.yaw = packet_in.yaw; + mav_array_memcpy(packet1.covariance, packet_in.covariance, sizeof(float)*21); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -4638,12 +4633,12 @@ static void mavlink_test_vicon_position_estimate(uint8_t system_id, uint8_t comp MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vicon_position_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_vicon_position_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance ); mavlink_msg_vicon_position_estimate_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vicon_position_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_vicon_position_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance ); mavlink_msg_vicon_position_estimate_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -4656,7 +4651,7 @@ static void mavlink_test_vicon_position_estimate(uint8_t system_id, uint8_t comp MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vicon_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_vicon_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance ); mavlink_msg_vicon_position_estimate_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -4673,7 +4668,7 @@ static void mavlink_test_highres_imu(uint8_t system_id, uint8_t component_id, ma uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_highres_imu_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,297.0,325.0,353.0,381.0,409.0,20355 + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,297.0,325.0,353.0,381.0,409.0,20355,63 }; mavlink_highres_imu_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4692,6 +4687,7 @@ static void mavlink_test_highres_imu(uint8_t system_id, uint8_t component_id, ma packet1.pressure_alt = packet_in.pressure_alt; packet1.temperature = packet_in.temperature; packet1.fields_updated = packet_in.fields_updated; + packet1.id = packet_in.id; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -4706,12 +4702,12 @@ static void mavlink_test_highres_imu(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_highres_imu_pack(system_id, component_id, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated ); + mavlink_msg_highres_imu_pack(system_id, component_id, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated , packet1.id ); mavlink_msg_highres_imu_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_highres_imu_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated ); + mavlink_msg_highres_imu_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated , packet1.id ); mavlink_msg_highres_imu_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -4724,7 +4720,7 @@ static void mavlink_test_highres_imu(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_highres_imu_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated ); + mavlink_msg_highres_imu_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated , packet1.id ); mavlink_msg_highres_imu_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -4806,7 +4802,7 @@ static void mavlink_test_hil_sensor(uint8_t system_id, uint8_t component_id, mav uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_hil_sensor_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,297.0,325.0,353.0,381.0,409.0,963500584 + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,297.0,325.0,353.0,381.0,409.0,963500584,197 }; mavlink_hil_sensor_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4825,6 +4821,7 @@ static void mavlink_test_hil_sensor(uint8_t system_id, uint8_t component_id, mav packet1.pressure_alt = packet_in.pressure_alt; packet1.temperature = packet_in.temperature; packet1.fields_updated = packet_in.fields_updated; + packet1.id = packet_in.id; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -4839,12 +4836,12 @@ static void mavlink_test_hil_sensor(uint8_t system_id, uint8_t component_id, mav MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_hil_sensor_pack(system_id, component_id, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated ); + mavlink_msg_hil_sensor_pack(system_id, component_id, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated , packet1.id ); mavlink_msg_hil_sensor_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_hil_sensor_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated ); + mavlink_msg_hil_sensor_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated , packet1.id ); mavlink_msg_hil_sensor_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -4857,7 +4854,7 @@ static void mavlink_test_hil_sensor(uint8_t system_id, uint8_t component_id, mav MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_hil_sensor_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated ); + mavlink_msg_hil_sensor_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated , packet1.id ); mavlink_msg_hil_sensor_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -5175,7 +5172,7 @@ static void mavlink_test_hil_gps(uint8_t system_id, uint8_t component_id, mavlin uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_hil_gps_t packet_in = { - 93372036854775807ULL,963497880,963498088,963498296,18275,18379,18483,18587,18691,18795,18899,235,46 + 93372036854775807ULL,963497880,963498088,963498296,18275,18379,18483,18587,18691,18795,18899,235,46,113,19159 }; mavlink_hil_gps_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -5192,6 +5189,8 @@ static void mavlink_test_hil_gps(uint8_t system_id, uint8_t component_id, mavlin packet1.cog = packet_in.cog; packet1.fix_type = packet_in.fix_type; packet1.satellites_visible = packet_in.satellites_visible; + packet1.id = packet_in.id; + packet1.yaw = packet_in.yaw; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -5206,12 +5205,12 @@ static void mavlink_test_hil_gps(uint8_t system_id, uint8_t component_id, mavlin MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_hil_gps_pack(system_id, component_id, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.vn , packet1.ve , packet1.vd , packet1.cog , packet1.satellites_visible ); + mavlink_msg_hil_gps_pack(system_id, component_id, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.vn , packet1.ve , packet1.vd , packet1.cog , packet1.satellites_visible , packet1.id , packet1.yaw ); mavlink_msg_hil_gps_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_hil_gps_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.vn , packet1.ve , packet1.vd , packet1.cog , packet1.satellites_visible ); + mavlink_msg_hil_gps_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.vn , packet1.ve , packet1.vd , packet1.cog , packet1.satellites_visible , packet1.id , packet1.yaw ); mavlink_msg_hil_gps_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -5224,7 +5223,7 @@ static void mavlink_test_hil_gps(uint8_t system_id, uint8_t component_id, mavlin MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_hil_gps_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.vn , packet1.ve , packet1.vd , packet1.cog , packet1.satellites_visible ); + mavlink_msg_hil_gps_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.vn , packet1.ve , packet1.vd , packet1.cog , packet1.satellites_visible , packet1.id , packet1.yaw ); mavlink_msg_hil_gps_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -5375,7 +5374,7 @@ static void mavlink_test_scaled_imu2(uint8_t system_id, uint8_t component_id, ma uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_scaled_imu2_t packet_in = { - 963497464,17443,17547,17651,17755,17859,17963,18067,18171,18275 + 963497464,17443,17547,17651,17755,17859,17963,18067,18171,18275,18379 }; mavlink_scaled_imu2_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -5389,6 +5388,7 @@ static void mavlink_test_scaled_imu2(uint8_t system_id, uint8_t component_id, ma packet1.xmag = packet_in.xmag; packet1.ymag = packet_in.ymag; packet1.zmag = packet_in.zmag; + packet1.temperature = packet_in.temperature; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -5403,12 +5403,12 @@ static void mavlink_test_scaled_imu2(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_imu2_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_scaled_imu2_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.temperature ); mavlink_msg_scaled_imu2_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_imu2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_scaled_imu2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.temperature ); mavlink_msg_scaled_imu2_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -5421,7 +5421,7 @@ static void mavlink_test_scaled_imu2(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_imu2_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_scaled_imu2_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.temperature ); mavlink_msg_scaled_imu2_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -5835,7 +5835,7 @@ static void mavlink_test_gps2_raw(uint8_t system_id, uint8_t component_id, mavli uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_gps2_raw_t packet_in = { - 93372036854775807ULL,963497880,963498088,963498296,963498504,18483,18587,18691,18795,101,168,235 + 93372036854775807ULL,963497880,963498088,963498296,963498504,18483,18587,18691,18795,101,168,235,19055 }; mavlink_gps2_raw_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -5851,6 +5851,7 @@ static void mavlink_test_gps2_raw(uint8_t system_id, uint8_t component_id, mavli packet1.fix_type = packet_in.fix_type; packet1.satellites_visible = packet_in.satellites_visible; packet1.dgps_numch = packet_in.dgps_numch; + packet1.yaw = packet_in.yaw; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -5865,12 +5866,12 @@ static void mavlink_test_gps2_raw(uint8_t system_id, uint8_t component_id, mavli MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_gps2_raw_pack(system_id, component_id, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age ); + mavlink_msg_gps2_raw_pack(system_id, component_id, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age , packet1.yaw ); mavlink_msg_gps2_raw_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_gps2_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age ); + mavlink_msg_gps2_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age , packet1.yaw ); mavlink_msg_gps2_raw_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -5883,7 +5884,7 @@ static void mavlink_test_gps2_raw(uint8_t system_id, uint8_t component_id, mavli MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_gps2_raw_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age ); + mavlink_msg_gps2_raw_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age , packet1.yaw ); mavlink_msg_gps2_raw_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -6147,7 +6148,7 @@ static void mavlink_test_scaled_imu3(uint8_t system_id, uint8_t component_id, ma uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_scaled_imu3_t packet_in = { - 963497464,17443,17547,17651,17755,17859,17963,18067,18171,18275 + 963497464,17443,17547,17651,17755,17859,17963,18067,18171,18275,18379 }; mavlink_scaled_imu3_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -6161,6 +6162,7 @@ static void mavlink_test_scaled_imu3(uint8_t system_id, uint8_t component_id, ma packet1.xmag = packet_in.xmag; packet1.ymag = packet_in.ymag; packet1.zmag = packet_in.zmag; + packet1.temperature = packet_in.temperature; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -6175,12 +6177,12 @@ static void mavlink_test_scaled_imu3(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_imu3_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_scaled_imu3_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.temperature ); mavlink_msg_scaled_imu3_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_imu3_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_scaled_imu3_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.temperature ); mavlink_msg_scaled_imu3_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -6193,7 +6195,7 @@ static void mavlink_test_scaled_imu3(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_imu3_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_scaled_imu3_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.temperature ); mavlink_msg_scaled_imu3_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -6325,7 +6327,7 @@ static void mavlink_test_distance_sensor(uint8_t system_id, uint8_t component_id uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_distance_sensor_t packet_in = { - 963497464,17443,17547,17651,163,230,41,108 + 963497464,17443,17547,17651,163,230,41,108,115.0,143.0,{ 171.0, 172.0, 173.0, 174.0 },247 }; mavlink_distance_sensor_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -6337,7 +6339,11 @@ static void mavlink_test_distance_sensor(uint8_t system_id, uint8_t component_id packet1.id = packet_in.id; packet1.orientation = packet_in.orientation; packet1.covariance = packet_in.covariance; + packet1.horizontal_fov = packet_in.horizontal_fov; + packet1.vertical_fov = packet_in.vertical_fov; + packet1.signal_quality = packet_in.signal_quality; + mav_array_memcpy(packet1.quaternion, packet_in.quaternion, sizeof(float)*4); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -6351,12 +6357,12 @@ static void mavlink_test_distance_sensor(uint8_t system_id, uint8_t component_id MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_distance_sensor_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.min_distance , packet1.max_distance , packet1.current_distance , packet1.type , packet1.id , packet1.orientation , packet1.covariance ); + mavlink_msg_distance_sensor_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.min_distance , packet1.max_distance , packet1.current_distance , packet1.type , packet1.id , packet1.orientation , packet1.covariance , packet1.horizontal_fov , packet1.vertical_fov , packet1.quaternion , packet1.signal_quality ); mavlink_msg_distance_sensor_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_distance_sensor_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.min_distance , packet1.max_distance , packet1.current_distance , packet1.type , packet1.id , packet1.orientation , packet1.covariance ); + mavlink_msg_distance_sensor_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.min_distance , packet1.max_distance , packet1.current_distance , packet1.type , packet1.id , packet1.orientation , packet1.covariance , packet1.horizontal_fov , packet1.vertical_fov , packet1.quaternion , packet1.signal_quality ); mavlink_msg_distance_sensor_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -6369,7 +6375,7 @@ static void mavlink_test_distance_sensor(uint8_t system_id, uint8_t component_id MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_distance_sensor_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.min_distance , packet1.max_distance , packet1.current_distance , packet1.type , packet1.id , packet1.orientation , packet1.covariance ); + mavlink_msg_distance_sensor_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.min_distance , packet1.max_distance , packet1.current_distance , packet1.type , packet1.id , packet1.orientation , packet1.covariance , packet1.horizontal_fov , packet1.vertical_fov , packet1.quaternion , packet1.signal_quality ); mavlink_msg_distance_sensor_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -6616,7 +6622,7 @@ static void mavlink_test_scaled_pressure2(uint8_t system_id, uint8_t component_i uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_scaled_pressure2_t packet_in = { - 963497464,45.0,73.0,17859 + 963497464,45.0,73.0,17859,17963 }; mavlink_scaled_pressure2_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -6624,6 +6630,7 @@ static void mavlink_test_scaled_pressure2(uint8_t system_id, uint8_t component_i packet1.press_abs = packet_in.press_abs; packet1.press_diff = packet_in.press_diff; packet1.temperature = packet_in.temperature; + packet1.temperature_press_diff = packet_in.temperature_press_diff; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -6638,12 +6645,12 @@ static void mavlink_test_scaled_pressure2(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_pressure2_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature ); + mavlink_msg_scaled_pressure2_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature , packet1.temperature_press_diff ); mavlink_msg_scaled_pressure2_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_pressure2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature ); + mavlink_msg_scaled_pressure2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature , packet1.temperature_press_diff ); mavlink_msg_scaled_pressure2_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -6656,7 +6663,7 @@ static void mavlink_test_scaled_pressure2(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_pressure2_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature ); + mavlink_msg_scaled_pressure2_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature , packet1.temperature_press_diff ); mavlink_msg_scaled_pressure2_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -6673,7 +6680,7 @@ static void mavlink_test_att_pos_mocap(uint8_t system_id, uint8_t component_id, uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_att_pos_mocap_t packet_in = { - 93372036854775807ULL,{ 73.0, 74.0, 75.0, 76.0 },185.0,213.0,241.0 + 93372036854775807ULL,{ 73.0, 74.0, 75.0, 76.0 },185.0,213.0,241.0,{ 269.0, 270.0, 271.0, 272.0, 273.0, 274.0, 275.0, 276.0, 277.0, 278.0, 279.0, 280.0, 281.0, 282.0, 283.0, 284.0, 285.0, 286.0, 287.0, 288.0, 289.0 } }; mavlink_att_pos_mocap_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -6683,6 +6690,7 @@ static void mavlink_test_att_pos_mocap(uint8_t system_id, uint8_t component_id, packet1.z = packet_in.z; mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); + mav_array_memcpy(packet1.covariance, packet_in.covariance, sizeof(float)*21); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -6696,12 +6704,12 @@ static void mavlink_test_att_pos_mocap(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_att_pos_mocap_pack(system_id, component_id, &msg , packet1.time_usec , packet1.q , packet1.x , packet1.y , packet1.z ); + mavlink_msg_att_pos_mocap_pack(system_id, component_id, &msg , packet1.time_usec , packet1.q , packet1.x , packet1.y , packet1.z , packet1.covariance ); mavlink_msg_att_pos_mocap_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_att_pos_mocap_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.q , packet1.x , packet1.y , packet1.z ); + mavlink_msg_att_pos_mocap_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.q , packet1.x , packet1.y , packet1.z , packet1.covariance ); mavlink_msg_att_pos_mocap_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -6714,7 +6722,7 @@ static void mavlink_test_att_pos_mocap(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_att_pos_mocap_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.q , packet1.x , packet1.y , packet1.z ); + mavlink_msg_att_pos_mocap_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.q , packet1.x , packet1.y , packet1.z , packet1.covariance ); mavlink_msg_att_pos_mocap_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -6963,7 +6971,7 @@ static void mavlink_test_scaled_pressure3(uint8_t system_id, uint8_t component_i uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_scaled_pressure3_t packet_in = { - 963497464,45.0,73.0,17859 + 963497464,45.0,73.0,17859,17963 }; mavlink_scaled_pressure3_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -6971,6 +6979,7 @@ static void mavlink_test_scaled_pressure3(uint8_t system_id, uint8_t component_i packet1.press_abs = packet_in.press_abs; packet1.press_diff = packet_in.press_diff; packet1.temperature = packet_in.temperature; + packet1.temperature_press_diff = packet_in.temperature_press_diff; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -6985,12 +6994,12 @@ static void mavlink_test_scaled_pressure3(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_pressure3_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature ); + mavlink_msg_scaled_pressure3_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature , packet1.temperature_press_diff ); mavlink_msg_scaled_pressure3_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_pressure3_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature ); + mavlink_msg_scaled_pressure3_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature , packet1.temperature_press_diff ); mavlink_msg_scaled_pressure3_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -7003,7 +7012,7 @@ static void mavlink_test_scaled_pressure3(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_pressure3_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature ); + mavlink_msg_scaled_pressure3_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature , packet1.temperature_press_diff ); mavlink_msg_scaled_pressure3_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -7154,7 +7163,7 @@ static void mavlink_test_battery_status(uint8_t system_id, uint8_t component_id, uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_battery_status_t packet_in = { - 963497464,963497672,17651,{ 17755, 17756, 17757, 17758, 17759, 17760, 17761, 17762, 17763, 17764 },18795,101,168,235,46 + 963497464,963497672,17651,{ 17755, 17756, 17757, 17758, 17759, 17760, 17761, 17762, 17763, 17764 },18795,101,168,235,46,963499336,125,{ 19367, 19368, 19369, 19370 },216,963500064 }; mavlink_battery_status_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -7166,8 +7175,13 @@ static void mavlink_test_battery_status(uint8_t system_id, uint8_t component_id, packet1.battery_function = packet_in.battery_function; packet1.type = packet_in.type; packet1.battery_remaining = packet_in.battery_remaining; + packet1.time_remaining = packet_in.time_remaining; + packet1.charge_state = packet_in.charge_state; + packet1.mode = packet_in.mode; + packet1.fault_bitmask = packet_in.fault_bitmask; mav_array_memcpy(packet1.voltages, packet_in.voltages, sizeof(uint16_t)*10); + mav_array_memcpy(packet1.voltages_ext, packet_in.voltages_ext, sizeof(uint16_t)*4); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -7181,12 +7195,12 @@ static void mavlink_test_battery_status(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_battery_status_pack(system_id, component_id, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.temperature , packet1.voltages , packet1.current_battery , packet1.current_consumed , packet1.energy_consumed , packet1.battery_remaining ); + mavlink_msg_battery_status_pack(system_id, component_id, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.temperature , packet1.voltages , packet1.current_battery , packet1.current_consumed , packet1.energy_consumed , packet1.battery_remaining , packet1.time_remaining , packet1.charge_state , packet1.voltages_ext , packet1.mode , packet1.fault_bitmask ); mavlink_msg_battery_status_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_battery_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.temperature , packet1.voltages , packet1.current_battery , packet1.current_consumed , packet1.energy_consumed , packet1.battery_remaining ); + mavlink_msg_battery_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.temperature , packet1.voltages , packet1.current_battery , packet1.current_consumed , packet1.energy_consumed , packet1.battery_remaining , packet1.time_remaining , packet1.charge_state , packet1.voltages_ext , packet1.mode , packet1.fault_bitmask ); mavlink_msg_battery_status_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -7199,7 +7213,7 @@ static void mavlink_test_battery_status(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_battery_status_send(MAVLINK_COMM_1 , packet1.id , packet1.battery_function , packet1.type , packet1.temperature , packet1.voltages , packet1.current_battery , packet1.current_consumed , packet1.energy_consumed , packet1.battery_remaining ); + mavlink_msg_battery_status_send(MAVLINK_COMM_1 , packet1.id , packet1.battery_function , packet1.type , packet1.temperature , packet1.voltages , packet1.current_battery , packet1.current_consumed , packet1.energy_consumed , packet1.battery_remaining , packet1.time_remaining , packet1.charge_state , packet1.voltages_ext , packet1.mode , packet1.fault_bitmask ); mavlink_msg_battery_status_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -7216,7 +7230,7 @@ static void mavlink_test_autopilot_version(uint8_t system_id, uint8_t component_ uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_autopilot_version_t packet_in = { - 93372036854775807ULL,93372036854776311ULL,963498296,963498504,963498712,963498920,18899,19003,{ 113, 114, 115, 116, 117, 118, 119, 120 },{ 137, 138, 139, 140, 141, 142, 143, 144 },{ 161, 162, 163, 164, 165, 166, 167, 168 } + 93372036854775807ULL,93372036854776311ULL,963498296,963498504,963498712,963498920,18899,19003,{ 113, 114, 115, 116, 117, 118, 119, 120 },{ 137, 138, 139, 140, 141, 142, 143, 144 },{ 161, 162, 163, 164, 165, 166, 167, 168 },{ 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202 } }; mavlink_autopilot_version_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -7232,6 +7246,7 @@ static void mavlink_test_autopilot_version(uint8_t system_id, uint8_t component_ mav_array_memcpy(packet1.flight_custom_version, packet_in.flight_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet1.middleware_custom_version, packet_in.middleware_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet1.os_custom_version, packet_in.os_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet1.uid2, packet_in.uid2, sizeof(uint8_t)*18); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -7245,12 +7260,12 @@ static void mavlink_test_autopilot_version(uint8_t system_id, uint8_t component_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_autopilot_version_pack(system_id, component_id, &msg , packet1.capabilities , packet1.flight_sw_version , packet1.middleware_sw_version , packet1.os_sw_version , packet1.board_version , packet1.flight_custom_version , packet1.middleware_custom_version , packet1.os_custom_version , packet1.vendor_id , packet1.product_id , packet1.uid ); + mavlink_msg_autopilot_version_pack(system_id, component_id, &msg , packet1.capabilities , packet1.flight_sw_version , packet1.middleware_sw_version , packet1.os_sw_version , packet1.board_version , packet1.flight_custom_version , packet1.middleware_custom_version , packet1.os_custom_version , packet1.vendor_id , packet1.product_id , packet1.uid , packet1.uid2 ); mavlink_msg_autopilot_version_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_autopilot_version_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.capabilities , packet1.flight_sw_version , packet1.middleware_sw_version , packet1.os_sw_version , packet1.board_version , packet1.flight_custom_version , packet1.middleware_custom_version , packet1.os_custom_version , packet1.vendor_id , packet1.product_id , packet1.uid ); + mavlink_msg_autopilot_version_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.capabilities , packet1.flight_sw_version , packet1.middleware_sw_version , packet1.os_sw_version , packet1.board_version , packet1.flight_custom_version , packet1.middleware_custom_version , packet1.os_custom_version , packet1.vendor_id , packet1.product_id , packet1.uid , packet1.uid2 ); mavlink_msg_autopilot_version_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -7263,7 +7278,7 @@ static void mavlink_test_autopilot_version(uint8_t system_id, uint8_t component_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_autopilot_version_send(MAVLINK_COMM_1 , packet1.capabilities , packet1.flight_sw_version , packet1.middleware_sw_version , packet1.os_sw_version , packet1.board_version , packet1.flight_custom_version , packet1.middleware_custom_version , packet1.os_custom_version , packet1.vendor_id , packet1.product_id , packet1.uid ); + mavlink_msg_autopilot_version_send(MAVLINK_COMM_1 , packet1.capabilities , packet1.flight_sw_version , packet1.middleware_sw_version , packet1.os_sw_version , packet1.board_version , packet1.flight_custom_version , packet1.middleware_custom_version , packet1.os_custom_version , packet1.vendor_id , packet1.product_id , packet1.uid , packet1.uid2 ); mavlink_msg_autopilot_version_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -7280,7 +7295,7 @@ static void mavlink_test_landing_target(uint8_t system_id, uint8_t component_id, uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_landing_target_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,89,156 + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,89,156,227.0,255.0,283.0,{ 311.0, 312.0, 313.0, 314.0 },51,118 }; mavlink_landing_target_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -7292,7 +7307,13 @@ static void mavlink_test_landing_target(uint8_t system_id, uint8_t component_id, packet1.size_y = packet_in.size_y; packet1.target_num = packet_in.target_num; packet1.frame = packet_in.frame; + packet1.x = packet_in.x; + packet1.y = packet_in.y; + packet1.z = packet_in.z; + packet1.type = packet_in.type; + packet1.position_valid = packet_in.position_valid; + mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -7306,12 +7327,12 @@ static void mavlink_test_landing_target(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_landing_target_pack(system_id, component_id, &msg , packet1.time_usec , packet1.target_num , packet1.frame , packet1.angle_x , packet1.angle_y , packet1.distance , packet1.size_x , packet1.size_y ); + mavlink_msg_landing_target_pack(system_id, component_id, &msg , packet1.time_usec , packet1.target_num , packet1.frame , packet1.angle_x , packet1.angle_y , packet1.distance , packet1.size_x , packet1.size_y , packet1.x , packet1.y , packet1.z , packet1.q , packet1.type , packet1.position_valid ); mavlink_msg_landing_target_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_landing_target_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.target_num , packet1.frame , packet1.angle_x , packet1.angle_y , packet1.distance , packet1.size_x , packet1.size_y ); + mavlink_msg_landing_target_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.target_num , packet1.frame , packet1.angle_x , packet1.angle_y , packet1.distance , packet1.size_x , packet1.size_y , packet1.x , packet1.y , packet1.z , packet1.q , packet1.type , packet1.position_valid ); mavlink_msg_landing_target_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -7324,7 +7345,7 @@ static void mavlink_test_landing_target(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_landing_target_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.target_num , packet1.frame , packet1.angle_x , packet1.angle_y , packet1.distance , packet1.size_x , packet1.size_y ); + mavlink_msg_landing_target_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.target_num , packet1.frame , packet1.angle_x , packet1.angle_y , packet1.distance , packet1.size_x , packet1.size_y , packet1.x , packet1.y , packet1.z , packet1.q , packet1.type , packet1.position_valid ); mavlink_msg_landing_target_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -7341,7 +7362,7 @@ static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, m uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_fence_status_t packet_in = { - 963497464,17443,151,218 + 963497464,17443,151,218,29 }; mavlink_fence_status_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -7349,6 +7370,7 @@ static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, m packet1.breach_count = packet_in.breach_count; packet1.breach_status = packet_in.breach_status; packet1.breach_type = packet_in.breach_type; + packet1.breach_mitigation = packet_in.breach_mitigation; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -7363,12 +7385,12 @@ static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, m MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_fence_status_pack(system_id, component_id, &msg , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time ); + mavlink_msg_fence_status_pack(system_id, component_id, &msg , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time , packet1.breach_mitigation ); mavlink_msg_fence_status_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_fence_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time ); + mavlink_msg_fence_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time , packet1.breach_mitigation ); mavlink_msg_fence_status_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -7381,11 +7403,152 @@ static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, m MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_fence_status_send(MAVLINK_COMM_1 , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time ); + mavlink_msg_fence_status_send(MAVLINK_COMM_1 , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time , packet1.breach_mitigation ); mavlink_msg_fence_status_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_mag_cal_report(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0); + if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_MAG_CAL_REPORT >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_mag_cal_report_t packet_in = { + 17.0,45.0,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,125,192,3,70,325.0,149,216,367.0 + }; + mavlink_mag_cal_report_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.fitness = packet_in.fitness; + packet1.ofs_x = packet_in.ofs_x; + packet1.ofs_y = packet_in.ofs_y; + packet1.ofs_z = packet_in.ofs_z; + packet1.diag_x = packet_in.diag_x; + packet1.diag_y = packet_in.diag_y; + packet1.diag_z = packet_in.diag_z; + packet1.offdiag_x = packet_in.offdiag_x; + packet1.offdiag_y = packet_in.offdiag_y; + packet1.offdiag_z = packet_in.offdiag_z; + packet1.compass_id = packet_in.compass_id; + packet1.cal_mask = packet_in.cal_mask; + packet1.cal_status = packet_in.cal_status; + packet1.autosaved = packet_in.autosaved; + packet1.orientation_confidence = packet_in.orientation_confidence; + packet1.old_orientation = packet_in.old_orientation; + packet1.new_orientation = packet_in.new_orientation; + packet1.scale_factor = packet_in.scale_factor; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mag_cal_report_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_mag_cal_report_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mag_cal_report_pack(system_id, component_id, &msg , packet1.compass_id , packet1.cal_mask , packet1.cal_status , packet1.autosaved , packet1.fitness , packet1.ofs_x , packet1.ofs_y , packet1.ofs_z , packet1.diag_x , packet1.diag_y , packet1.diag_z , packet1.offdiag_x , packet1.offdiag_y , packet1.offdiag_z , packet1.orientation_confidence , packet1.old_orientation , packet1.new_orientation , packet1.scale_factor ); + mavlink_msg_mag_cal_report_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mag_cal_report_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.compass_id , packet1.cal_mask , packet1.cal_status , packet1.autosaved , packet1.fitness , packet1.ofs_x , packet1.ofs_y , packet1.ofs_z , packet1.diag_x , packet1.diag_y , packet1.diag_z , packet1.offdiag_x , packet1.offdiag_y , packet1.offdiag_z , packet1.orientation_confidence , packet1.old_orientation , packet1.new_orientation , packet1.scale_factor ); + mavlink_msg_mag_cal_report_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_EFI_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_efi_status_t packet_in = { + 17.0,45.0,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,297.0,325.0,353.0,381.0,409.0,437.0,197 + }; + mavlink_efi_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.ecu_index = packet_in.ecu_index; + packet1.rpm = packet_in.rpm; + packet1.fuel_consumed = packet_in.fuel_consumed; + packet1.fuel_flow = packet_in.fuel_flow; + packet1.engine_load = packet_in.engine_load; + packet1.throttle_position = packet_in.throttle_position; + packet1.spark_dwell_time = packet_in.spark_dwell_time; + packet1.barometric_pressure = packet_in.barometric_pressure; + packet1.intake_manifold_pressure = packet_in.intake_manifold_pressure; + packet1.intake_manifold_temperature = packet_in.intake_manifold_temperature; + packet1.cylinder_head_temperature = packet_in.cylinder_head_temperature; + packet1.ignition_timing = packet_in.ignition_timing; + packet1.injection_time = packet_in.injection_time; + packet1.exhaust_gas_temperature = packet_in.exhaust_gas_temperature; + packet1.throttle_out = packet_in.throttle_out; + packet1.pt_compensation = packet_in.pt_compensation; + packet1.health = packet_in.health; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_efi_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_efi_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_efi_status_pack(system_id, component_id, &msg , packet1.health , packet1.ecu_index , packet1.rpm , packet1.fuel_consumed , packet1.fuel_flow , packet1.engine_load , packet1.throttle_position , packet1.spark_dwell_time , packet1.barometric_pressure , packet1.intake_manifold_pressure , packet1.intake_manifold_temperature , packet1.cylinder_head_temperature , packet1.ignition_timing , packet1.injection_time , packet1.exhaust_gas_temperature , packet1.throttle_out , packet1.pt_compensation ); + mavlink_msg_efi_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_efi_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.health , packet1.ecu_index , packet1.rpm , packet1.fuel_consumed , packet1.fuel_flow , packet1.engine_load , packet1.throttle_position , packet1.spark_dwell_time , packet1.barometric_pressure , packet1.intake_manifold_pressure , packet1.intake_manifold_temperature , packet1.cylinder_head_temperature , packet1.ignition_timing , packet1.injection_time , packet1.exhaust_gas_temperature , packet1.throttle_out , packet1.pt_compensation ); + mavlink_msg_efi_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_SETUP_SIGNING >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_setup_signing_t packet_in = { + 93372036854775807ULL,29,96,{ 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194 } + }; + mavlink_setup_signing_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.initial_timestamp = packet_in.initial_timestamp; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + mav_array_memcpy(packet1.secret_key, packet_in.secret_key, sizeof(uint8_t)*32); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_setup_signing_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_setup_signing_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_setup_signing_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.secret_key , packet1.initial_timestamp ); + mavlink_msg_setup_signing_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_setup_signing_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.secret_key , packet1.initial_timestamp ); + mavlink_msg_setup_signing_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_BUTTON_CHANGE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_button_change_t packet_in = { + 963497464,963497672,29 + }; + mavlink_button_change_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.last_change_ms = packet_in.last_change_ms; + packet1.state = packet_in.state; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_button_change_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_button_change_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_button_change_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.last_change_ms , packet1.state ); + mavlink_msg_button_change_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_button_change_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.last_change_ms , packet1.state ); + mavlink_msg_button_change_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PLAY_TUNE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_play_tune_t packet_in = { + 5,72,"CDEFGHIJKLMNOPQRSTUVWXYZABCDE","GHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVW" + }; + mavlink_play_tune_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + mav_array_memcpy(packet1.tune, packet_in.tune, sizeof(char)*30); + mav_array_memcpy(packet1.tune2, packet_in.tune2, sizeof(char)*200); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_play_tune_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_play_tune_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_play_tune_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.tune , packet1.tune2 ); + mavlink_msg_play_tune_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_play_tune_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.tune , packet1.tune2 ); + mavlink_msg_play_tune_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CAMERA_INFORMATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_camera_information_t packet_in = { + 963497464,963497672,73.0,101.0,129.0,963498504,18483,18587,18691,{ 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254 },{ 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94 },159,"RSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZ" + }; + mavlink_camera_information_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.firmware_version = packet_in.firmware_version; + packet1.focal_length = packet_in.focal_length; + packet1.sensor_size_h = packet_in.sensor_size_h; + packet1.sensor_size_v = packet_in.sensor_size_v; + packet1.flags = packet_in.flags; + packet1.resolution_h = packet_in.resolution_h; + packet1.resolution_v = packet_in.resolution_v; + packet1.cam_definition_version = packet_in.cam_definition_version; + packet1.lens_id = packet_in.lens_id; + + mav_array_memcpy(packet1.vendor_name, packet_in.vendor_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet1.model_name, packet_in.model_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet1.cam_definition_uri, packet_in.cam_definition_uri, sizeof(char)*140); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_information_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_camera_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_information_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.vendor_name , packet1.model_name , packet1.firmware_version , packet1.focal_length , packet1.sensor_size_h , packet1.sensor_size_v , packet1.resolution_h , packet1.resolution_v , packet1.lens_id , packet1.flags , packet1.cam_definition_version , packet1.cam_definition_uri ); + mavlink_msg_camera_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_information_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.vendor_name , packet1.model_name , packet1.firmware_version , packet1.focal_length , packet1.sensor_size_h , packet1.sensor_size_v , packet1.resolution_h , packet1.resolution_v , packet1.lens_id , packet1.flags , packet1.cam_definition_version , packet1.cam_definition_uri ); + mavlink_msg_camera_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CAMERA_SETTINGS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_camera_settings_t packet_in = { + 963497464,17,52.0,80.0 + }; + mavlink_camera_settings_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.mode_id = packet_in.mode_id; + packet1.zoomLevel = packet_in.zoomLevel; + packet1.focusLevel = packet_in.focusLevel; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_settings_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_camera_settings_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_settings_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.mode_id , packet1.zoomLevel , packet1.focusLevel ); + mavlink_msg_camera_settings_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_settings_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.mode_id , packet1.zoomLevel , packet1.focusLevel ); + mavlink_msg_camera_settings_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_STORAGE_INFORMATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_storage_information_t packet_in = { + 963497464,45.0,73.0,101.0,129.0,157.0,77,144,211,22,"CDEFGHIJKLMNOPQRSTUVWXYZABCDEFG" + }; + mavlink_storage_information_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.total_capacity = packet_in.total_capacity; + packet1.used_capacity = packet_in.used_capacity; + packet1.available_capacity = packet_in.available_capacity; + packet1.read_speed = packet_in.read_speed; + packet1.write_speed = packet_in.write_speed; + packet1.storage_id = packet_in.storage_id; + packet1.storage_count = packet_in.storage_count; + packet1.status = packet_in.status; + packet1.type = packet_in.type; + + mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*32); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_storage_information_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_storage_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_storage_information_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.storage_id , packet1.storage_count , packet1.status , packet1.total_capacity , packet1.used_capacity , packet1.available_capacity , packet1.read_speed , packet1.write_speed , packet1.type , packet1.name ); + mavlink_msg_storage_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_storage_information_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.storage_id , packet1.storage_count , packet1.status , packet1.total_capacity , packet1.used_capacity , packet1.available_capacity , packet1.read_speed , packet1.write_speed , packet1.type , packet1.name ); + mavlink_msg_storage_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_camera_capture_status_t packet_in = { + 963497464,45.0,963497880,101.0,53,120,963498400 + }; + mavlink_camera_capture_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.image_interval = packet_in.image_interval; + packet1.recording_time_ms = packet_in.recording_time_ms; + packet1.available_capacity = packet_in.available_capacity; + packet1.image_status = packet_in.image_status; + packet1.video_status = packet_in.video_status; + packet1.image_count = packet_in.image_count; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_capture_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_camera_capture_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_capture_status_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.image_status , packet1.video_status , packet1.image_interval , packet1.recording_time_ms , packet1.available_capacity , packet1.image_count ); + mavlink_msg_camera_capture_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_capture_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.image_status , packet1.video_status , packet1.image_interval , packet1.recording_time_ms , packet1.available_capacity , packet1.image_count ); + mavlink_msg_camera_capture_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_camera_image_captured_t packet_in = { + 93372036854775807ULL,963497880,963498088,963498296,963498504,963498712,{ 213.0, 214.0, 215.0, 216.0 },963499752,149,216,"YZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRST" + }; + mavlink_camera_image_captured_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_utc = packet_in.time_utc; + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.lat = packet_in.lat; + packet1.lon = packet_in.lon; + packet1.alt = packet_in.alt; + packet1.relative_alt = packet_in.relative_alt; + packet1.image_index = packet_in.image_index; + packet1.camera_id = packet_in.camera_id; + packet1.capture_result = packet_in.capture_result; + + mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); + mav_array_memcpy(packet1.file_url, packet_in.file_url, sizeof(char)*205); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_image_captured_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_camera_image_captured_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_image_captured_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.time_utc , packet1.camera_id , packet1.lat , packet1.lon , packet1.alt , packet1.relative_alt , packet1.q , packet1.image_index , packet1.capture_result , packet1.file_url ); + mavlink_msg_camera_image_captured_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_image_captured_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.time_utc , packet1.camera_id , packet1.lat , packet1.lon , packet1.alt , packet1.relative_alt , packet1.q , packet1.image_index , packet1.capture_result , packet1.file_url ); + mavlink_msg_camera_image_captured_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_FLIGHT_INFORMATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_flight_information_t packet_in = { + 93372036854775807ULL,93372036854776311ULL,93372036854776815ULL,963498712 + }; + mavlink_flight_information_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.arming_time_utc = packet_in.arming_time_utc; + packet1.takeoff_time_utc = packet_in.takeoff_time_utc; + packet1.flight_uuid = packet_in.flight_uuid; + packet1.time_boot_ms = packet_in.time_boot_ms; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_flight_information_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_flight_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_flight_information_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.arming_time_utc , packet1.takeoff_time_utc , packet1.flight_uuid ); + mavlink_msg_flight_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_flight_information_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.arming_time_utc , packet1.takeoff_time_utc , packet1.flight_uuid ); + mavlink_msg_flight_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_MOUNT_ORIENTATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_mount_orientation_t packet_in = { + 963497464,45.0,73.0,101.0,129.0 + }; + mavlink_mount_orientation_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.roll = packet_in.roll; + packet1.pitch = packet_in.pitch; + packet1.yaw = packet_in.yaw; + packet1.yaw_absolute = packet_in.yaw_absolute; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mount_orientation_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_mount_orientation_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mount_orientation_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.roll , packet1.pitch , packet1.yaw , packet1.yaw_absolute ); + mavlink_msg_mount_orientation_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mount_orientation_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.roll , packet1.pitch , packet1.yaw , packet1.yaw_absolute ); + mavlink_msg_mount_orientation_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_LOGGING_DATA >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_logging_data_t packet_in = { + 17235,139,206,17,84,{ 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143 } + }; + mavlink_logging_data_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.sequence = packet_in.sequence; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.length = packet_in.length; + packet1.first_message_offset = packet_in.first_message_offset; + + mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*249); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_data_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_logging_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_data_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.sequence , packet1.length , packet1.first_message_offset , packet1.data ); + mavlink_msg_logging_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_data_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.sequence , packet1.length , packet1.first_message_offset , packet1.data ); + mavlink_msg_logging_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_LOGGING_DATA_ACKED >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_logging_data_acked_t packet_in = { + 17235,139,206,17,84,{ 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143 } + }; + mavlink_logging_data_acked_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.sequence = packet_in.sequence; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.length = packet_in.length; + packet1.first_message_offset = packet_in.first_message_offset; + + mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*249); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_data_acked_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_logging_data_acked_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_data_acked_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.sequence , packet1.length , packet1.first_message_offset , packet1.data ); + mavlink_msg_logging_data_acked_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_data_acked_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.sequence , packet1.length , packet1.first_message_offset , packet1.data ); + mavlink_msg_logging_data_acked_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_LOGGING_ACK >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_logging_ack_t packet_in = { + 17235,139,206 + }; + mavlink_logging_ack_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.sequence = packet_in.sequence; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_ack_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_logging_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_ack_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.sequence ); + mavlink_msg_logging_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.sequence ); + mavlink_msg_logging_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_video_stream_information_t packet_in = { + 17.0,963497672,17651,17755,17859,17963,18067,187,254,65,"VWXYZABCDEFGHIJKLMNOPQRSTUVWXYZ","BCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCD" + }; + mavlink_video_stream_information_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.framerate = packet_in.framerate; + packet1.bitrate = packet_in.bitrate; + packet1.flags = packet_in.flags; + packet1.resolution_h = packet_in.resolution_h; + packet1.resolution_v = packet_in.resolution_v; + packet1.rotation = packet_in.rotation; + packet1.hfov = packet_in.hfov; + packet1.stream_id = packet_in.stream_id; + packet1.count = packet_in.count; + packet1.type = packet_in.type; + + mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*32); + mav_array_memcpy(packet1.uri, packet_in.uri, sizeof(char)*160); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_video_stream_information_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_video_stream_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_video_stream_information_pack(system_id, component_id, &msg , packet1.stream_id , packet1.count , packet1.type , packet1.flags , packet1.framerate , packet1.resolution_h , packet1.resolution_v , packet1.bitrate , packet1.rotation , packet1.hfov , packet1.name , packet1.uri ); + mavlink_msg_video_stream_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_video_stream_information_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.stream_id , packet1.count , packet1.type , packet1.flags , packet1.framerate , packet1.resolution_h , packet1.resolution_v , packet1.bitrate , packet1.rotation , packet1.hfov , packet1.name , packet1.uri ); + mavlink_msg_video_stream_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_VIDEO_STREAM_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_video_stream_status_t packet_in = { + 17.0,963497672,17651,17755,17859,17963,18067,187 + }; + mavlink_video_stream_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.framerate = packet_in.framerate; + packet1.bitrate = packet_in.bitrate; + packet1.flags = packet_in.flags; + packet1.resolution_h = packet_in.resolution_h; + packet1.resolution_v = packet_in.resolution_v; + packet1.rotation = packet_in.rotation; + packet1.hfov = packet_in.hfov; + packet1.stream_id = packet_in.stream_id; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_video_stream_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_video_stream_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_video_stream_status_pack(system_id, component_id, &msg , packet1.stream_id , packet1.flags , packet1.framerate , packet1.resolution_h , packet1.resolution_v , packet1.bitrate , packet1.rotation , packet1.hfov ); + mavlink_msg_video_stream_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_video_stream_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.stream_id , packet1.flags , packet1.framerate , packet1.resolution_h , packet1.resolution_v , packet1.bitrate , packet1.rotation , packet1.hfov ); + mavlink_msg_video_stream_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CAMERA_FOV_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_camera_fov_status_t packet_in = { + 963497464,963497672,963497880,963498088,963498296,963498504,963498712,{ 213.0, 214.0, 215.0, 216.0 },325.0,353.0 + }; + mavlink_camera_fov_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.lat_camera = packet_in.lat_camera; + packet1.lon_camera = packet_in.lon_camera; + packet1.alt_camera = packet_in.alt_camera; + packet1.lat_image = packet_in.lat_image; + packet1.lon_image = packet_in.lon_image; + packet1.alt_image = packet_in.alt_image; + packet1.hfov = packet_in.hfov; + packet1.vfov = packet_in.vfov; + + mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_fov_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_camera_fov_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_fov_status_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.lat_camera , packet1.lon_camera , packet1.alt_camera , packet1.lat_image , packet1.lon_image , packet1.alt_image , packet1.q , packet1.hfov , packet1.vfov ); + mavlink_msg_camera_fov_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_fov_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.lat_camera , packet1.lon_camera , packet1.alt_camera , packet1.lat_image , packet1.lon_image , packet1.alt_image , packet1.q , packet1.hfov , packet1.vfov ); + mavlink_msg_camera_fov_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_camera_tracking_image_status_t packet_in = { + 17.0,45.0,73.0,101.0,129.0,157.0,185.0,89,156,223 + }; + mavlink_camera_tracking_image_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.point_x = packet_in.point_x; + packet1.point_y = packet_in.point_y; + packet1.radius = packet_in.radius; + packet1.rec_top_x = packet_in.rec_top_x; + packet1.rec_top_y = packet_in.rec_top_y; + packet1.rec_bottom_x = packet_in.rec_bottom_x; + packet1.rec_bottom_y = packet_in.rec_bottom_y; + packet1.tracking_status = packet_in.tracking_status; + packet1.tracking_mode = packet_in.tracking_mode; + packet1.target_data = packet_in.target_data; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_tracking_image_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_camera_tracking_image_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_tracking_image_status_pack(system_id, component_id, &msg , packet1.tracking_status , packet1.tracking_mode , packet1.target_data , packet1.point_x , packet1.point_y , packet1.radius , packet1.rec_top_x , packet1.rec_top_y , packet1.rec_bottom_x , packet1.rec_bottom_y ); + mavlink_msg_camera_tracking_image_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_tracking_image_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.tracking_status , packet1.tracking_mode , packet1.target_data , packet1.point_x , packet1.point_y , packet1.radius , packet1.rec_top_x , packet1.rec_top_y , packet1.rec_bottom_x , packet1.rec_bottom_y ); + mavlink_msg_camera_tracking_image_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_camera_tracking_geo_status_t packet_in = { + 963497464,963497672,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,297.0,325.0,149 + }; + mavlink_camera_tracking_geo_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.lat = packet_in.lat; + packet1.lon = packet_in.lon; + packet1.alt = packet_in.alt; + packet1.h_acc = packet_in.h_acc; + packet1.v_acc = packet_in.v_acc; + packet1.vel_n = packet_in.vel_n; + packet1.vel_e = packet_in.vel_e; + packet1.vel_d = packet_in.vel_d; + packet1.vel_acc = packet_in.vel_acc; + packet1.dist = packet_in.dist; + packet1.hdg = packet_in.hdg; + packet1.hdg_acc = packet_in.hdg_acc; + packet1.tracking_status = packet_in.tracking_status; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_tracking_geo_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_camera_tracking_geo_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_tracking_geo_status_pack(system_id, component_id, &msg , packet1.tracking_status , packet1.lat , packet1.lon , packet1.alt , packet1.h_acc , packet1.v_acc , packet1.vel_n , packet1.vel_e , packet1.vel_d , packet1.vel_acc , packet1.dist , packet1.hdg , packet1.hdg_acc ); + mavlink_msg_camera_tracking_geo_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_tracking_geo_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.tracking_status , packet1.lat , packet1.lon , packet1.alt , packet1.h_acc , packet1.v_acc , packet1.vel_n , packet1.vel_e , packet1.vel_d , packet1.vel_acc , packet1.dist , packet1.hdg , packet1.hdg_acc ); + mavlink_msg_camera_tracking_geo_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gimbal_manager_information_t packet_in = { + 963497464,963497672,73.0,101.0,129.0,157.0,185.0,213.0,101 + }; + mavlink_gimbal_manager_information_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.cap_flags = packet_in.cap_flags; + packet1.roll_min = packet_in.roll_min; + packet1.roll_max = packet_in.roll_max; + packet1.pitch_min = packet_in.pitch_min; + packet1.pitch_max = packet_in.pitch_max; + packet1.yaw_min = packet_in.yaw_min; + packet1.yaw_max = packet_in.yaw_max; + packet1.gimbal_device_id = packet_in.gimbal_device_id; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_information_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gimbal_manager_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_information_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.cap_flags , packet1.gimbal_device_id , packet1.roll_min , packet1.roll_max , packet1.pitch_min , packet1.pitch_max , packet1.yaw_min , packet1.yaw_max ); + mavlink_msg_gimbal_manager_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_information_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.cap_flags , packet1.gimbal_device_id , packet1.roll_min , packet1.roll_max , packet1.pitch_min , packet1.pitch_max , packet1.yaw_min , packet1.yaw_max ); + mavlink_msg_gimbal_manager_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gimbal_manager_status_t packet_in = { + 963497464,963497672,29,96,163,230,41 + }; + mavlink_gimbal_manager_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.flags = packet_in.flags; + packet1.gimbal_device_id = packet_in.gimbal_device_id; + packet1.primary_control_sysid = packet_in.primary_control_sysid; + packet1.primary_control_compid = packet_in.primary_control_compid; + packet1.secondary_control_sysid = packet_in.secondary_control_sysid; + packet1.secondary_control_compid = packet_in.secondary_control_compid; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gimbal_manager_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_status_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.flags , packet1.gimbal_device_id , packet1.primary_control_sysid , packet1.primary_control_compid , packet1.secondary_control_sysid , packet1.secondary_control_compid ); + mavlink_msg_gimbal_manager_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.flags , packet1.gimbal_device_id , packet1.primary_control_sysid , packet1.primary_control_compid , packet1.secondary_control_sysid , packet1.secondary_control_compid ); + mavlink_msg_gimbal_manager_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gimbal_manager_set_attitude_t packet_in = { + 963497464,{ 45.0, 46.0, 47.0, 48.0 },157.0,185.0,213.0,101,168,235 + }; + mavlink_gimbal_manager_set_attitude_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.flags = packet_in.flags; + packet1.angular_velocity_x = packet_in.angular_velocity_x; + packet1.angular_velocity_y = packet_in.angular_velocity_y; + packet1.angular_velocity_z = packet_in.angular_velocity_z; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.gimbal_device_id = packet_in.gimbal_device_id; + + mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_attitude_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gimbal_manager_set_attitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_attitude_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.flags , packet1.gimbal_device_id , packet1.q , packet1.angular_velocity_x , packet1.angular_velocity_y , packet1.angular_velocity_z ); + mavlink_msg_gimbal_manager_set_attitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_attitude_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.flags , packet1.gimbal_device_id , packet1.q , packet1.angular_velocity_x , packet1.angular_velocity_y , packet1.angular_velocity_z ); + mavlink_msg_gimbal_manager_set_attitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gimbal_device_information_t packet_in = { + 93372036854775807ULL,963497880,963498088,963498296,157.0,185.0,213.0,241.0,269.0,297.0,19523,19627,"WXYZABCDEFGHIJKLMNOPQRSTUVWXYZA","CDEFGHIJKLMNOPQRSTUVWXYZABCDEFG","IJKLMNOPQRSTUVWXYZABCDEFGHIJKLM" + }; + mavlink_gimbal_device_information_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.uid = packet_in.uid; + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.firmware_version = packet_in.firmware_version; + packet1.hardware_version = packet_in.hardware_version; + packet1.roll_min = packet_in.roll_min; + packet1.roll_max = packet_in.roll_max; + packet1.pitch_min = packet_in.pitch_min; + packet1.pitch_max = packet_in.pitch_max; + packet1.yaw_min = packet_in.yaw_min; + packet1.yaw_max = packet_in.yaw_max; + packet1.cap_flags = packet_in.cap_flags; + packet1.custom_cap_flags = packet_in.custom_cap_flags; + + mav_array_memcpy(packet1.vendor_name, packet_in.vendor_name, sizeof(char)*32); + mav_array_memcpy(packet1.model_name, packet_in.model_name, sizeof(char)*32); + mav_array_memcpy(packet1.custom_name, packet_in.custom_name, sizeof(char)*32); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_information_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gimbal_device_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_information_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.vendor_name , packet1.model_name , packet1.custom_name , packet1.firmware_version , packet1.hardware_version , packet1.uid , packet1.cap_flags , packet1.custom_cap_flags , packet1.roll_min , packet1.roll_max , packet1.pitch_min , packet1.pitch_max , packet1.yaw_min , packet1.yaw_max ); + mavlink_msg_gimbal_device_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_information_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.vendor_name , packet1.model_name , packet1.custom_name , packet1.firmware_version , packet1.hardware_version , packet1.uid , packet1.cap_flags , packet1.custom_cap_flags , packet1.roll_min , packet1.roll_max , packet1.pitch_min , packet1.pitch_max , packet1.yaw_min , packet1.yaw_max ); + mavlink_msg_gimbal_device_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gimbal_device_set_attitude_t packet_in = { + { 17.0, 18.0, 19.0, 20.0 },129.0,157.0,185.0,18691,223,34 + }; + mavlink_gimbal_device_set_attitude_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.angular_velocity_x = packet_in.angular_velocity_x; + packet1.angular_velocity_y = packet_in.angular_velocity_y; + packet1.angular_velocity_z = packet_in.angular_velocity_z; + packet1.flags = packet_in.flags; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_set_attitude_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gimbal_device_set_attitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_set_attitude_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.flags , packet1.q , packet1.angular_velocity_x , packet1.angular_velocity_y , packet1.angular_velocity_z ); + mavlink_msg_gimbal_device_set_attitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_set_attitude_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.flags , packet1.q , packet1.angular_velocity_x , packet1.angular_velocity_y , packet1.angular_velocity_z ); + mavlink_msg_gimbal_device_set_attitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gimbal_device_attitude_status_t packet_in = { + 963497464,{ 45.0, 46.0, 47.0, 48.0 },157.0,185.0,213.0,963499128,19107,247,58 + }; + mavlink_gimbal_device_attitude_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.angular_velocity_x = packet_in.angular_velocity_x; + packet1.angular_velocity_y = packet_in.angular_velocity_y; + packet1.angular_velocity_z = packet_in.angular_velocity_z; + packet1.failure_flags = packet_in.failure_flags; + packet1.flags = packet_in.flags; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_attitude_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gimbal_device_attitude_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_attitude_status_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.time_boot_ms , packet1.flags , packet1.q , packet1.angular_velocity_x , packet1.angular_velocity_y , packet1.angular_velocity_z , packet1.failure_flags ); + mavlink_msg_gimbal_device_attitude_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_attitude_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.time_boot_ms , packet1.flags , packet1.q , packet1.angular_velocity_x , packet1.angular_velocity_y , packet1.angular_velocity_z , packet1.failure_flags ); + mavlink_msg_gimbal_device_attitude_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_autopilot_state_for_gimbal_device_t packet_in = { + 93372036854775807ULL,{ 73.0, 74.0, 75.0, 76.0 },963498712,213.0,241.0,269.0,963499544,325.0,19731,27,94,161 + }; + mavlink_autopilot_state_for_gimbal_device_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_us = packet_in.time_boot_us; + packet1.q_estimated_delay_us = packet_in.q_estimated_delay_us; + packet1.vx = packet_in.vx; + packet1.vy = packet_in.vy; + packet1.vz = packet_in.vz; + packet1.v_estimated_delay_us = packet_in.v_estimated_delay_us; + packet1.feed_forward_angular_velocity_z = packet_in.feed_forward_angular_velocity_z; + packet1.estimator_status = packet_in.estimator_status; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.landed_state = packet_in.landed_state; + + mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_autopilot_state_for_gimbal_device_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_autopilot_state_for_gimbal_device_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_autopilot_state_for_gimbal_device_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.time_boot_us , packet1.q , packet1.q_estimated_delay_us , packet1.vx , packet1.vy , packet1.vz , packet1.v_estimated_delay_us , packet1.feed_forward_angular_velocity_z , packet1.estimator_status , packet1.landed_state ); + mavlink_msg_autopilot_state_for_gimbal_device_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_autopilot_state_for_gimbal_device_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.time_boot_us , packet1.q , packet1.q_estimated_delay_us , packet1.vx , packet1.vy , packet1.vz , packet1.v_estimated_delay_us , packet1.feed_forward_angular_velocity_z , packet1.estimator_status , packet1.landed_state ); + mavlink_msg_autopilot_state_for_gimbal_device_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gimbal_manager_set_pitchyaw_t packet_in = { + 963497464,45.0,73.0,101.0,129.0,65,132,199 + }; + mavlink_gimbal_manager_set_pitchyaw_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.flags = packet_in.flags; + packet1.pitch = packet_in.pitch; + packet1.yaw = packet_in.yaw; + packet1.pitch_rate = packet_in.pitch_rate; + packet1.yaw_rate = packet_in.yaw_rate; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.gimbal_device_id = packet_in.gimbal_device_id; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_pitchyaw_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gimbal_manager_set_pitchyaw_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_pitchyaw_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.flags , packet1.gimbal_device_id , packet1.pitch , packet1.yaw , packet1.pitch_rate , packet1.yaw_rate ); + mavlink_msg_gimbal_manager_set_pitchyaw_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_pitchyaw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.flags , packet1.gimbal_device_id , packet1.pitch , packet1.yaw , packet1.pitch_rate , packet1.yaw_rate ); + mavlink_msg_gimbal_manager_set_pitchyaw_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gimbal_manager_set_manual_control_t packet_in = { + 963497464,45.0,73.0,101.0,129.0,65,132,199 + }; + mavlink_gimbal_manager_set_manual_control_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.flags = packet_in.flags; + packet1.pitch = packet_in.pitch; + packet1.yaw = packet_in.yaw; + packet1.pitch_rate = packet_in.pitch_rate; + packet1.yaw_rate = packet_in.yaw_rate; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.gimbal_device_id = packet_in.gimbal_device_id; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_manual_control_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gimbal_manager_set_manual_control_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_manual_control_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.flags , packet1.gimbal_device_id , packet1.pitch , packet1.yaw , packet1.pitch_rate , packet1.yaw_rate ); + mavlink_msg_gimbal_manager_set_manual_control_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_manual_control_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.flags , packet1.gimbal_device_id , packet1.pitch , packet1.yaw , packet1.pitch_rate , packet1.yaw_rate ); + mavlink_msg_gimbal_manager_set_manual_control_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ESC_INFO >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_esc_info_t packet_in = { + 93372036854775807ULL,{ 963497880, 963497881, 963497882, 963497883 },18483,{ 18587, 18588, 18589, 18590 },235,46,113,180,{ 247, 248, 249, 250 } + }; + mavlink_esc_info_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.counter = packet_in.counter; + packet1.index = packet_in.index; + packet1.count = packet_in.count; + packet1.connection_type = packet_in.connection_type; + packet1.info = packet_in.info; + + mav_array_memcpy(packet1.error_count, packet_in.error_count, sizeof(uint32_t)*4); + mav_array_memcpy(packet1.failure_flags, packet_in.failure_flags, sizeof(uint16_t)*4); + mav_array_memcpy(packet1.temperature, packet_in.temperature, sizeof(uint8_t)*4); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ESC_INFO_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ESC_INFO_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_esc_info_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_esc_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_esc_info_pack(system_id, component_id, &msg , packet1.index , packet1.time_usec , packet1.counter , packet1.count , packet1.connection_type , packet1.info , packet1.failure_flags , packet1.error_count , packet1.temperature ); + mavlink_msg_esc_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_esc_info_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.index , packet1.time_usec , packet1.counter , packet1.count , packet1.connection_type , packet1.info , packet1.failure_flags , packet1.error_count , packet1.temperature ); + mavlink_msg_esc_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ESC_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_esc_status_t packet_in = { + 93372036854775807ULL,{ 963497880, 963497881, 963497882, 963497883 },{ 185.0, 186.0, 187.0, 188.0 },{ 297.0, 298.0, 299.0, 300.0 },173 + }; + mavlink_esc_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.index = packet_in.index; + + mav_array_memcpy(packet1.rpm, packet_in.rpm, sizeof(int32_t)*4); + mav_array_memcpy(packet1.voltage, packet_in.voltage, sizeof(float)*4); + mav_array_memcpy(packet1.current, packet_in.current, sizeof(float)*4); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_esc_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_esc_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_esc_status_pack(system_id, component_id, &msg , packet1.index , packet1.time_usec , packet1.rpm , packet1.voltage , packet1.current ); + mavlink_msg_esc_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_esc_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.index , packet1.time_usec , packet1.rpm , packet1.voltage , packet1.current ); + mavlink_msg_esc_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_WIFI_CONFIG_AP >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_wifi_config_ap_t packet_in = { + "ABCDEFGHIJKLMNOPQRSTUVWXYZABCDE","GHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQ",37,104 + }; + mavlink_wifi_config_ap_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.mode = packet_in.mode; + packet1.response = packet_in.response; + + mav_array_memcpy(packet1.ssid, packet_in.ssid, sizeof(char)*32); + mav_array_memcpy(packet1.password, packet_in.password, sizeof(char)*64); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_wifi_config_ap_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_wifi_config_ap_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_wifi_config_ap_pack(system_id, component_id, &msg , packet1.ssid , packet1.password , packet1.mode , packet1.response ); + mavlink_msg_wifi_config_ap_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_wifi_config_ap_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.ssid , packet1.password , packet1.mode , packet1.response ); + mavlink_msg_wifi_config_ap_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_AIS_VESSEL >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_ais_vessel_t packet_in = { + 963497464,963497672,963497880,17859,17963,18067,18171,18275,18379,18483,211,22,89,156,223,"FGHIJK","MNOPQRSTUVWXYZABCDE" + }; + mavlink_ais_vessel_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.MMSI = packet_in.MMSI; + packet1.lat = packet_in.lat; + packet1.lon = packet_in.lon; + packet1.COG = packet_in.COG; + packet1.heading = packet_in.heading; + packet1.velocity = packet_in.velocity; + packet1.dimension_bow = packet_in.dimension_bow; + packet1.dimension_stern = packet_in.dimension_stern; + packet1.tslc = packet_in.tslc; + packet1.flags = packet_in.flags; + packet1.turn_rate = packet_in.turn_rate; + packet1.navigational_status = packet_in.navigational_status; + packet1.type = packet_in.type; + packet1.dimension_port = packet_in.dimension_port; + packet1.dimension_starboard = packet_in.dimension_starboard; + + mav_array_memcpy(packet1.callsign, packet_in.callsign, sizeof(char)*7); + mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*20); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ais_vessel_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_ais_vessel_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ais_vessel_pack(system_id, component_id, &msg , packet1.MMSI , packet1.lat , packet1.lon , packet1.COG , packet1.heading , packet1.velocity , packet1.turn_rate , packet1.navigational_status , packet1.type , packet1.dimension_bow , packet1.dimension_stern , packet1.dimension_port , packet1.dimension_starboard , packet1.callsign , packet1.name , packet1.tslc , packet1.flags ); + mavlink_msg_ais_vessel_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ais_vessel_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.MMSI , packet1.lat , packet1.lon , packet1.COG , packet1.heading , packet1.velocity , packet1.turn_rate , packet1.navigational_status , packet1.type , packet1.dimension_bow , packet1.dimension_stern , packet1.dimension_port , packet1.dimension_starboard , packet1.callsign , packet1.name , packet1.tslc , packet1.flags ); + mavlink_msg_ais_vessel_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_UAVCAN_NODE_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_uavcan_node_status_t packet_in = { + 93372036854775807ULL,963497880,17859,175,242,53 + }; + mavlink_uavcan_node_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.uptime_sec = packet_in.uptime_sec; + packet1.vendor_specific_status_code = packet_in.vendor_specific_status_code; + packet1.health = packet_in.health; + packet1.mode = packet_in.mode; + packet1.sub_mode = packet_in.sub_mode; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_uavcan_node_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_uavcan_node_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_uavcan_node_status_pack(system_id, component_id, &msg , packet1.time_usec , packet1.uptime_sec , packet1.health , packet1.mode , packet1.sub_mode , packet1.vendor_specific_status_code ); + mavlink_msg_uavcan_node_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_uavcan_node_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.uptime_sec , packet1.health , packet1.mode , packet1.sub_mode , packet1.vendor_specific_status_code ); + mavlink_msg_uavcan_node_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_UAVCAN_NODE_INFO >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_uavcan_node_info_t packet_in = { + 93372036854775807ULL,963497880,963498088,"QRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQ",37,104,{ 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186 },219,30 + }; + mavlink_uavcan_node_info_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.uptime_sec = packet_in.uptime_sec; + packet1.sw_vcs_commit = packet_in.sw_vcs_commit; + packet1.hw_version_major = packet_in.hw_version_major; + packet1.hw_version_minor = packet_in.hw_version_minor; + packet1.sw_version_major = packet_in.sw_version_major; + packet1.sw_version_minor = packet_in.sw_version_minor; + + mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*80); + mav_array_memcpy(packet1.hw_unique_id, packet_in.hw_unique_id, sizeof(uint8_t)*16); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_uavcan_node_info_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_uavcan_node_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_uavcan_node_info_pack(system_id, component_id, &msg , packet1.time_usec , packet1.uptime_sec , packet1.name , packet1.hw_version_major , packet1.hw_version_minor , packet1.hw_unique_id , packet1.sw_version_major , packet1.sw_version_minor , packet1.sw_vcs_commit ); + mavlink_msg_uavcan_node_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_uavcan_node_info_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.uptime_sec , packet1.name , packet1.hw_version_major , packet1.hw_version_minor , packet1.hw_unique_id , packet1.sw_version_major , packet1.sw_version_minor , packet1.sw_vcs_commit ); + mavlink_msg_uavcan_node_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_param_ext_request_read_t packet_in = { + 17235,139,206,"EFGHIJKLMNOPQRS" + }; + mavlink_param_ext_request_read_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.param_index = packet_in.param_index; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + mav_array_memcpy(packet1.param_id, packet_in.param_id, sizeof(char)*16); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_request_read_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_ext_request_read_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_request_read_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_index ); + mavlink_msg_param_ext_request_read_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_request_read_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_index ); + mavlink_msg_param_ext_request_read_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_param_ext_request_list_t packet_in = { + 5,72 + }; + mavlink_param_ext_request_list_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_request_list_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_ext_request_list_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_request_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_param_ext_request_list_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_request_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_param_ext_request_list_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PARAM_EXT_VALUE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_param_ext_value_t packet_in = { + 17235,17339,"EFGHIJKLMNOPQRS","UVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQ",193 + }; + mavlink_param_ext_value_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.param_count = packet_in.param_count; + packet1.param_index = packet_in.param_index; + packet1.param_type = packet_in.param_type; + + mav_array_memcpy(packet1.param_id, packet_in.param_id, sizeof(char)*16); + mav_array_memcpy(packet1.param_value, packet_in.param_value, sizeof(char)*128); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_value_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_ext_value_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_value_pack(system_id, component_id, &msg , packet1.param_id , packet1.param_value , packet1.param_type , packet1.param_count , packet1.param_index ); + mavlink_msg_param_ext_value_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_value_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.param_id , packet1.param_value , packet1.param_type , packet1.param_count , packet1.param_index ); + mavlink_msg_param_ext_value_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PARAM_EXT_SET >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_param_ext_set_t packet_in = { + 5,72,"CDEFGHIJKLMNOPQ","STUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNO",59 + }; + mavlink_param_ext_set_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.param_type = packet_in.param_type; + + mav_array_memcpy(packet1.param_id, packet_in.param_id, sizeof(char)*16); + mav_array_memcpy(packet1.param_value, packet_in.param_value, sizeof(char)*128); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_set_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_ext_set_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_set_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_value , packet1.param_type ); + mavlink_msg_param_ext_set_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_set_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_value , packet1.param_type ); + mavlink_msg_param_ext_set_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PARAM_EXT_ACK >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_param_ext_ack_t packet_in = { + "ABCDEFGHIJKLMNO","QRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLM",181,248 + }; + mavlink_param_ext_ack_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.param_type = packet_in.param_type; + packet1.param_result = packet_in.param_result; + + mav_array_memcpy(packet1.param_id, packet_in.param_id, sizeof(char)*16); + mav_array_memcpy(packet1.param_value, packet_in.param_value, sizeof(char)*128); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_ack_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_ext_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_ack_pack(system_id, component_id, &msg , packet1.param_id , packet1.param_value , packet1.param_type , packet1.param_result ); + mavlink_msg_param_ext_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.param_id , packet1.param_value , packet1.param_type , packet1.param_result ); + mavlink_msg_param_ext_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OBSTACLE_DISTANCE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_obstacle_distance_t packet_in = { + 93372036854775807ULL,{ 17651, 17652, 17653, 17654, 17655, 17656, 17657, 17658, 17659, 17660, 17661, 17662, 17663, 17664, 17665, 17666, 17667, 17668, 17669, 17670, 17671, 17672, 17673, 17674, 17675, 17676, 17677, 17678, 17679, 17680, 17681, 17682, 17683, 17684, 17685, 17686, 17687, 17688, 17689, 17690, 17691, 17692, 17693, 17694, 17695, 17696, 17697, 17698, 17699, 17700, 17701, 17702, 17703, 17704, 17705, 17706, 17707, 17708, 17709, 17710, 17711, 17712, 17713, 17714, 17715, 17716, 17717, 17718, 17719, 17720, 17721, 17722 },25139,25243,217,28,1123.0,1151.0,119 + }; + mavlink_obstacle_distance_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.min_distance = packet_in.min_distance; + packet1.max_distance = packet_in.max_distance; + packet1.sensor_type = packet_in.sensor_type; + packet1.increment = packet_in.increment; + packet1.increment_f = packet_in.increment_f; + packet1.angle_offset = packet_in.angle_offset; + packet1.frame = packet_in.frame; + + mav_array_memcpy(packet1.distances, packet_in.distances, sizeof(uint16_t)*72); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_obstacle_distance_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_obstacle_distance_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_obstacle_distance_pack(system_id, component_id, &msg , packet1.time_usec , packet1.sensor_type , packet1.distances , packet1.increment , packet1.min_distance , packet1.max_distance , packet1.increment_f , packet1.angle_offset , packet1.frame ); + mavlink_msg_obstacle_distance_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_obstacle_distance_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.sensor_type , packet1.distances , packet1.increment , packet1.min_distance , packet1.max_distance , packet1.increment_f , packet1.angle_offset , packet1.frame ); + mavlink_msg_obstacle_distance_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ODOMETRY >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_odometry_t packet_in = { + 93372036854775807ULL,73.0,101.0,129.0,{ 157.0, 158.0, 159.0, 160.0 },269.0,297.0,325.0,353.0,381.0,409.0,{ 437.0, 438.0, 439.0, 440.0, 441.0, 442.0, 443.0, 444.0, 445.0, 446.0, 447.0, 448.0, 449.0, 450.0, 451.0, 452.0, 453.0, 454.0, 455.0, 456.0, 457.0 },{ 1025.0, 1026.0, 1027.0, 1028.0, 1029.0, 1030.0, 1031.0, 1032.0, 1033.0, 1034.0, 1035.0, 1036.0, 1037.0, 1038.0, 1039.0, 1040.0, 1041.0, 1042.0, 1043.0, 1044.0, 1045.0 },177,244,55,122 + }; + mavlink_odometry_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.x = packet_in.x; + packet1.y = packet_in.y; + packet1.z = packet_in.z; + packet1.vx = packet_in.vx; + packet1.vy = packet_in.vy; + packet1.vz = packet_in.vz; + packet1.rollspeed = packet_in.rollspeed; + packet1.pitchspeed = packet_in.pitchspeed; + packet1.yawspeed = packet_in.yawspeed; + packet1.frame_id = packet_in.frame_id; + packet1.child_frame_id = packet_in.child_frame_id; + packet1.reset_counter = packet_in.reset_counter; + packet1.estimator_type = packet_in.estimator_type; + + mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); + mav_array_memcpy(packet1.pose_covariance, packet_in.pose_covariance, sizeof(float)*21); + mav_array_memcpy(packet1.velocity_covariance, packet_in.velocity_covariance, sizeof(float)*21); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ODOMETRY_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ODOMETRY_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_odometry_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_odometry_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_odometry_pack(system_id, component_id, &msg , packet1.time_usec , packet1.frame_id , packet1.child_frame_id , packet1.x , packet1.y , packet1.z , packet1.q , packet1.vx , packet1.vy , packet1.vz , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.pose_covariance , packet1.velocity_covariance , packet1.reset_counter , packet1.estimator_type ); + mavlink_msg_odometry_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_odometry_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.frame_id , packet1.child_frame_id , packet1.x , packet1.y , packet1.z , packet1.q , packet1.vx , packet1.vy , packet1.vz , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.pose_covariance , packet1.velocity_covariance , packet1.reset_counter , packet1.estimator_type ); + mavlink_msg_odometry_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_trajectory_representation_waypoints_t packet_in = { + 93372036854775807ULL,{ 73.0, 74.0, 75.0, 76.0, 77.0 },{ 213.0, 214.0, 215.0, 216.0, 217.0 },{ 353.0, 354.0, 355.0, 356.0, 357.0 },{ 493.0, 494.0, 495.0, 496.0, 497.0 },{ 633.0, 634.0, 635.0, 636.0, 637.0 },{ 773.0, 774.0, 775.0, 776.0, 777.0 },{ 913.0, 914.0, 915.0, 916.0, 917.0 },{ 1053.0, 1054.0, 1055.0, 1056.0, 1057.0 },{ 1193.0, 1194.0, 1195.0, 1196.0, 1197.0 },{ 1333.0, 1334.0, 1335.0, 1336.0, 1337.0 },{ 1473.0, 1474.0, 1475.0, 1476.0, 1477.0 },{ 29091, 29092, 29093, 29094, 29095 },79 + }; + mavlink_trajectory_representation_waypoints_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.valid_points = packet_in.valid_points; + + mav_array_memcpy(packet1.pos_x, packet_in.pos_x, sizeof(float)*5); + mav_array_memcpy(packet1.pos_y, packet_in.pos_y, sizeof(float)*5); + mav_array_memcpy(packet1.pos_z, packet_in.pos_z, sizeof(float)*5); + mav_array_memcpy(packet1.vel_x, packet_in.vel_x, sizeof(float)*5); + mav_array_memcpy(packet1.vel_y, packet_in.vel_y, sizeof(float)*5); + mav_array_memcpy(packet1.vel_z, packet_in.vel_z, sizeof(float)*5); + mav_array_memcpy(packet1.acc_x, packet_in.acc_x, sizeof(float)*5); + mav_array_memcpy(packet1.acc_y, packet_in.acc_y, sizeof(float)*5); + mav_array_memcpy(packet1.acc_z, packet_in.acc_z, sizeof(float)*5); + mav_array_memcpy(packet1.pos_yaw, packet_in.pos_yaw, sizeof(float)*5); + mav_array_memcpy(packet1.vel_yaw, packet_in.vel_yaw, sizeof(float)*5); + mav_array_memcpy(packet1.command, packet_in.command, sizeof(uint16_t)*5); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_trajectory_representation_waypoints_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_trajectory_representation_waypoints_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_trajectory_representation_waypoints_pack(system_id, component_id, &msg , packet1.time_usec , packet1.valid_points , packet1.pos_x , packet1.pos_y , packet1.pos_z , packet1.vel_x , packet1.vel_y , packet1.vel_z , packet1.acc_x , packet1.acc_y , packet1.acc_z , packet1.pos_yaw , packet1.vel_yaw , packet1.command ); + mavlink_msg_trajectory_representation_waypoints_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_trajectory_representation_waypoints_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.valid_points , packet1.pos_x , packet1.pos_y , packet1.pos_z , packet1.vel_x , packet1.vel_y , packet1.vel_z , packet1.acc_x , packet1.acc_y , packet1.acc_z , packet1.pos_yaw , packet1.vel_yaw , packet1.command ); + mavlink_msg_trajectory_representation_waypoints_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_trajectory_representation_bezier_t packet_in = { + 93372036854775807ULL,{ 73.0, 74.0, 75.0, 76.0, 77.0 },{ 213.0, 214.0, 215.0, 216.0, 217.0 },{ 353.0, 354.0, 355.0, 356.0, 357.0 },{ 493.0, 494.0, 495.0, 496.0, 497.0 },{ 633.0, 634.0, 635.0, 636.0, 637.0 },73 + }; + mavlink_trajectory_representation_bezier_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.valid_points = packet_in.valid_points; + + mav_array_memcpy(packet1.pos_x, packet_in.pos_x, sizeof(float)*5); + mav_array_memcpy(packet1.pos_y, packet_in.pos_y, sizeof(float)*5); + mav_array_memcpy(packet1.pos_z, packet_in.pos_z, sizeof(float)*5); + mav_array_memcpy(packet1.delta, packet_in.delta, sizeof(float)*5); + mav_array_memcpy(packet1.pos_yaw, packet_in.pos_yaw, sizeof(float)*5); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_trajectory_representation_bezier_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_trajectory_representation_bezier_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_trajectory_representation_bezier_pack(system_id, component_id, &msg , packet1.time_usec , packet1.valid_points , packet1.pos_x , packet1.pos_y , packet1.pos_z , packet1.delta , packet1.pos_yaw ); + mavlink_msg_trajectory_representation_bezier_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_trajectory_representation_bezier_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.valid_points , packet1.pos_x , packet1.pos_y , packet1.pos_z , packet1.delta , packet1.pos_yaw ); + mavlink_msg_trajectory_representation_bezier_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CELLULAR_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_cellular_status_t packet_in = { + 17235,17339,17443,151,218,29,96 + }; + mavlink_cellular_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.mcc = packet_in.mcc; + packet1.mnc = packet_in.mnc; + packet1.lac = packet_in.lac; + packet1.status = packet_in.status; + packet1.failure_reason = packet_in.failure_reason; + packet1.type = packet_in.type; + packet1.quality = packet_in.quality; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_cellular_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_cellular_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_cellular_status_pack(system_id, component_id, &msg , packet1.status , packet1.failure_reason , packet1.type , packet1.quality , packet1.mcc , packet1.mnc , packet1.lac ); + mavlink_msg_cellular_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_cellular_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.status , packet1.failure_reason , packet1.type , packet1.quality , packet1.mcc , packet1.mnc , packet1.lac ); + mavlink_msg_cellular_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ISBD_LINK_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_isbd_link_status_t packet_in = { + 93372036854775807ULL,93372036854776311ULL,18067,18171,65,132,199,10 + }; + mavlink_isbd_link_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.timestamp = packet_in.timestamp; + packet1.last_heartbeat = packet_in.last_heartbeat; + packet1.failed_sessions = packet_in.failed_sessions; + packet1.successful_sessions = packet_in.successful_sessions; + packet1.signal_quality = packet_in.signal_quality; + packet1.ring_pending = packet_in.ring_pending; + packet1.tx_session_pending = packet_in.tx_session_pending; + packet1.rx_session_pending = packet_in.rx_session_pending; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_isbd_link_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_isbd_link_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_isbd_link_status_pack(system_id, component_id, &msg , packet1.timestamp , packet1.last_heartbeat , packet1.failed_sessions , packet1.successful_sessions , packet1.signal_quality , packet1.ring_pending , packet1.tx_session_pending , packet1.rx_session_pending ); + mavlink_msg_isbd_link_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_isbd_link_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.timestamp , packet1.last_heartbeat , packet1.failed_sessions , packet1.successful_sessions , packet1.signal_quality , packet1.ring_pending , packet1.tx_session_pending , packet1.rx_session_pending ); + mavlink_msg_isbd_link_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CELLULAR_CONFIG >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_cellular_config_t packet_in = { + 5,72,"CDEFGHIJKLMNOPQ","STUVWXYZABCDEFG","IJKLMNOPQRSTUVWXYZABCDEFGHIJKLM","OPQRSTUVWXYZABC",123,190 + }; + mavlink_cellular_config_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.enable_lte = packet_in.enable_lte; + packet1.enable_pin = packet_in.enable_pin; + packet1.roaming = packet_in.roaming; + packet1.response = packet_in.response; + + mav_array_memcpy(packet1.pin, packet_in.pin, sizeof(char)*16); + mav_array_memcpy(packet1.new_pin, packet_in.new_pin, sizeof(char)*16); + mav_array_memcpy(packet1.apn, packet_in.apn, sizeof(char)*32); + mav_array_memcpy(packet1.puk, packet_in.puk, sizeof(char)*16); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_cellular_config_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_cellular_config_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_cellular_config_pack(system_id, component_id, &msg , packet1.enable_lte , packet1.enable_pin , packet1.pin , packet1.new_pin , packet1.apn , packet1.puk , packet1.roaming , packet1.response ); + mavlink_msg_cellular_config_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_cellular_config_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.enable_lte , packet1.enable_pin , packet1.pin , packet1.new_pin , packet1.apn , packet1.puk , packet1.roaming , packet1.response ); + mavlink_msg_cellular_config_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_RAW_RPM >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_raw_rpm_t packet_in = { + 17.0,17 + }; + mavlink_raw_rpm_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.frequency = packet_in.frequency; + packet1.index = packet_in.index; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_RAW_RPM_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_RAW_RPM_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_raw_rpm_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_raw_rpm_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_raw_rpm_pack(system_id, component_id, &msg , packet1.index , packet1.frequency ); + mavlink_msg_raw_rpm_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_raw_rpm_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.index , packet1.frequency ); + mavlink_msg_raw_rpm_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_UTM_GLOBAL_POSITION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_utm_global_position_t packet_in = { + 93372036854775807ULL,963497880,963498088,963498296,963498504,963498712,963498920,963499128,19107,19211,19315,19419,19523,19627,19731,{ 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44 },209,20 + }; + mavlink_utm_global_position_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time = packet_in.time; + packet1.lat = packet_in.lat; + packet1.lon = packet_in.lon; + packet1.alt = packet_in.alt; + packet1.relative_alt = packet_in.relative_alt; + packet1.next_lat = packet_in.next_lat; + packet1.next_lon = packet_in.next_lon; + packet1.next_alt = packet_in.next_alt; + packet1.vx = packet_in.vx; + packet1.vy = packet_in.vy; + packet1.vz = packet_in.vz; + packet1.h_acc = packet_in.h_acc; + packet1.v_acc = packet_in.v_acc; + packet1.vel_acc = packet_in.vel_acc; + packet1.update_rate = packet_in.update_rate; + packet1.flight_state = packet_in.flight_state; + packet1.flags = packet_in.flags; + + mav_array_memcpy(packet1.uas_id, packet_in.uas_id, sizeof(uint8_t)*18); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_utm_global_position_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_utm_global_position_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_utm_global_position_pack(system_id, component_id, &msg , packet1.time , packet1.uas_id , packet1.lat , packet1.lon , packet1.alt , packet1.relative_alt , packet1.vx , packet1.vy , packet1.vz , packet1.h_acc , packet1.v_acc , packet1.vel_acc , packet1.next_lat , packet1.next_lon , packet1.next_alt , packet1.update_rate , packet1.flight_state , packet1.flags ); + mavlink_msg_utm_global_position_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_utm_global_position_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time , packet1.uas_id , packet1.lat , packet1.lon , packet1.alt , packet1.relative_alt , packet1.vx , packet1.vy , packet1.vz , packet1.h_acc , packet1.v_acc , packet1.vel_acc , packet1.next_lat , packet1.next_lon , packet1.next_alt , packet1.update_rate , packet1.flight_state , packet1.flags ); + mavlink_msg_utm_global_position_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_debug_float_array_t packet_in = { + 93372036854775807ULL,17651,"KLMNOPQRS",{ 157.0, 158.0, 159.0, 160.0, 161.0, 162.0, 163.0, 164.0, 165.0, 166.0, 167.0, 168.0, 169.0, 170.0, 171.0, 172.0, 173.0, 174.0, 175.0, 176.0, 177.0, 178.0, 179.0, 180.0, 181.0, 182.0, 183.0, 184.0, 185.0, 186.0, 187.0, 188.0, 189.0, 190.0, 191.0, 192.0, 193.0, 194.0, 195.0, 196.0, 197.0, 198.0, 199.0, 200.0, 201.0, 202.0, 203.0, 204.0, 205.0, 206.0, 207.0, 208.0, 209.0, 210.0, 211.0, 212.0, 213.0, 214.0 } + }; + mavlink_debug_float_array_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.array_id = packet_in.array_id; + + mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*10); + mav_array_memcpy(packet1.data, packet_in.data, sizeof(float)*58); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_debug_float_array_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_debug_float_array_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_debug_float_array_pack(system_id, component_id, &msg , packet1.time_usec , packet1.name , packet1.array_id , packet1.data ); + mavlink_msg_debug_float_array_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_debug_float_array_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.name , packet1.array_id , packet1.data ); + mavlink_msg_debug_float_array_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_orbit_execution_status_t packet_in = { + 93372036854775807ULL,73.0,963498088,963498296,157.0,77 + }; + mavlink_orbit_execution_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.radius = packet_in.radius; + packet1.x = packet_in.x; + packet1.y = packet_in.y; + packet1.z = packet_in.z; + packet1.frame = packet_in.frame; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_orbit_execution_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_orbit_execution_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_orbit_execution_status_pack(system_id, component_id, &msg , packet1.time_usec , packet1.radius , packet1.frame , packet1.x , packet1.y , packet1.z ); + mavlink_msg_orbit_execution_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_orbit_execution_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.radius , packet1.frame , packet1.x , packet1.y , packet1.z ); + mavlink_msg_orbit_execution_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_SMART_BATTERY_INFO >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_smart_battery_info_t packet_in = { + 963497464,963497672,17651,17755,17859,17963,18067,187,254,65,"VWXYZABCDEFGHIJ","LMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGH" + }; + mavlink_smart_battery_info_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.capacity_full_specification = packet_in.capacity_full_specification; + packet1.capacity_full = packet_in.capacity_full; + packet1.cycle_count = packet_in.cycle_count; + packet1.weight = packet_in.weight; + packet1.discharge_minimum_voltage = packet_in.discharge_minimum_voltage; + packet1.charging_minimum_voltage = packet_in.charging_minimum_voltage; + packet1.resting_minimum_voltage = packet_in.resting_minimum_voltage; + packet1.id = packet_in.id; + packet1.battery_function = packet_in.battery_function; + packet1.type = packet_in.type; + + mav_array_memcpy(packet1.serial_number, packet_in.serial_number, sizeof(char)*16); + mav_array_memcpy(packet1.device_name, packet_in.device_name, sizeof(char)*50); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_smart_battery_info_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_smart_battery_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_smart_battery_info_pack(system_id, component_id, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.capacity_full_specification , packet1.capacity_full , packet1.cycle_count , packet1.serial_number , packet1.device_name , packet1.weight , packet1.discharge_minimum_voltage , packet1.charging_minimum_voltage , packet1.resting_minimum_voltage ); + mavlink_msg_smart_battery_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_smart_battery_info_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.capacity_full_specification , packet1.capacity_full , packet1.cycle_count , packet1.serial_number , packet1.device_name , packet1.weight , packet1.discharge_minimum_voltage , packet1.charging_minimum_voltage , packet1.resting_minimum_voltage ); + mavlink_msg_smart_battery_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GENERATOR_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_generator_status_t packet_in = { + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,963498920,963499128,19107,19211,19315 + }; + mavlink_generator_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.status = packet_in.status; + packet1.battery_current = packet_in.battery_current; + packet1.load_current = packet_in.load_current; + packet1.power_generated = packet_in.power_generated; + packet1.bus_voltage = packet_in.bus_voltage; + packet1.bat_current_setpoint = packet_in.bat_current_setpoint; + packet1.runtime = packet_in.runtime; + packet1.time_until_maintenance = packet_in.time_until_maintenance; + packet1.generator_speed = packet_in.generator_speed; + packet1.rectifier_temperature = packet_in.rectifier_temperature; + packet1.generator_temperature = packet_in.generator_temperature; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_generator_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_generator_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_generator_status_pack(system_id, component_id, &msg , packet1.status , packet1.generator_speed , packet1.battery_current , packet1.load_current , packet1.power_generated , packet1.bus_voltage , packet1.rectifier_temperature , packet1.bat_current_setpoint , packet1.generator_temperature , packet1.runtime , packet1.time_until_maintenance ); + mavlink_msg_generator_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_generator_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.status , packet1.generator_speed , packet1.battery_current , packet1.load_current , packet1.power_generated , packet1.bus_voltage , packet1.rectifier_temperature , packet1.bat_current_setpoint , packet1.generator_temperature , packet1.runtime , packet1.time_until_maintenance ); + mavlink_msg_generator_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_actuator_output_status_t packet_in = { + 93372036854775807ULL,963497880,{ 101.0, 102.0, 103.0, 104.0, 105.0, 106.0, 107.0, 108.0, 109.0, 110.0, 111.0, 112.0, 113.0, 114.0, 115.0, 116.0, 117.0, 118.0, 119.0, 120.0, 121.0, 122.0, 123.0, 124.0, 125.0, 126.0, 127.0, 128.0, 129.0, 130.0, 131.0, 132.0 } + }; + mavlink_actuator_output_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.active = packet_in.active; + + mav_array_memcpy(packet1.actuator, packet_in.actuator, sizeof(float)*32); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_actuator_output_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_actuator_output_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_actuator_output_status_pack(system_id, component_id, &msg , packet1.time_usec , packet1.active , packet1.actuator ); + mavlink_msg_actuator_output_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_actuator_output_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.active , packet1.actuator ); + mavlink_msg_actuator_output_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_time_estimate_to_target_t packet_in = { + 963497464,963497672,963497880,963498088,963498296 + }; + mavlink_time_estimate_to_target_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.safe_return = packet_in.safe_return; + packet1.land = packet_in.land; + packet1.mission_next_item = packet_in.mission_next_item; + packet1.mission_end = packet_in.mission_end; + packet1.commanded_action = packet_in.commanded_action; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_time_estimate_to_target_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_time_estimate_to_target_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_time_estimate_to_target_pack(system_id, component_id, &msg , packet1.safe_return , packet1.land , packet1.mission_next_item , packet1.mission_end , packet1.commanded_action ); + mavlink_msg_time_estimate_to_target_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_time_estimate_to_target_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.safe_return , packet1.land , packet1.mission_next_item , packet1.mission_end , packet1.commanded_action ); + mavlink_msg_time_estimate_to_target_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_TUNNEL >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_tunnel_t packet_in = { + 17235,139,206,17,{ 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211 } + }; + mavlink_tunnel_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.payload_type = packet_in.payload_type; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.payload_length = packet_in.payload_length; + + mav_array_memcpy(packet1.payload, packet_in.payload, sizeof(uint8_t)*128); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_TUNNEL_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_TUNNEL_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_tunnel_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_tunnel_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_tunnel_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.payload_type , packet1.payload_length , packet1.payload ); + mavlink_msg_tunnel_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_tunnel_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.payload_type , packet1.payload_length , packet1.payload ); + mavlink_msg_tunnel_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_onboard_computer_status_t packet_in = { + 93372036854775807ULL,963497880,963498088,963498296,{ 963498504, 963498505, 963498506, 963498507 },{ 963499336, 963499337, 963499338, 963499339 },{ 963500168, 963500169, 963500170, 963500171 },{ 963501000, 963501001, 963501002, 963501003, 963501004, 963501005 },{ 963502248, 963502249, 963502250, 963502251, 963502252, 963502253 },{ 963503496, 963503497, 963503498, 963503499, 963503500, 963503501 },{ 963504744, 963504745, 963504746, 963504747, 963504748, 963504749 },{ 963505992, 963505993, 963505994, 963505995, 963505996, 963505997 },{ 27011, 27012, 27013, 27014 },81,{ 148, 149, 150, 151, 152, 153, 154, 155 },{ 172, 173, 174, 175, 176, 177, 178, 179, 180, 181 },{ 74, 75, 76, 77 },{ 86, 87, 88, 89, 90, 91, 92, 93, 94, 95 },244,{ 55, 56, 57, 58, 59, 60, 61, 62 } + }; + mavlink_onboard_computer_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.uptime = packet_in.uptime; + packet1.ram_usage = packet_in.ram_usage; + packet1.ram_total = packet_in.ram_total; + packet1.type = packet_in.type; + packet1.temperature_board = packet_in.temperature_board; + + mav_array_memcpy(packet1.storage_type, packet_in.storage_type, sizeof(uint32_t)*4); + mav_array_memcpy(packet1.storage_usage, packet_in.storage_usage, sizeof(uint32_t)*4); + mav_array_memcpy(packet1.storage_total, packet_in.storage_total, sizeof(uint32_t)*4); + mav_array_memcpy(packet1.link_type, packet_in.link_type, sizeof(uint32_t)*6); + mav_array_memcpy(packet1.link_tx_rate, packet_in.link_tx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet1.link_rx_rate, packet_in.link_rx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet1.link_tx_max, packet_in.link_tx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet1.link_rx_max, packet_in.link_rx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet1.fan_speed, packet_in.fan_speed, sizeof(int16_t)*4); + mav_array_memcpy(packet1.cpu_cores, packet_in.cpu_cores, sizeof(uint8_t)*8); + mav_array_memcpy(packet1.cpu_combined, packet_in.cpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet1.gpu_cores, packet_in.gpu_cores, sizeof(uint8_t)*4); + mav_array_memcpy(packet1.gpu_combined, packet_in.gpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet1.temperature_core, packet_in.temperature_core, sizeof(int8_t)*8); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_onboard_computer_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_onboard_computer_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_onboard_computer_status_pack(system_id, component_id, &msg , packet1.time_usec , packet1.uptime , packet1.type , packet1.cpu_cores , packet1.cpu_combined , packet1.gpu_cores , packet1.gpu_combined , packet1.temperature_board , packet1.temperature_core , packet1.fan_speed , packet1.ram_usage , packet1.ram_total , packet1.storage_type , packet1.storage_usage , packet1.storage_total , packet1.link_type , packet1.link_tx_rate , packet1.link_rx_rate , packet1.link_tx_max , packet1.link_rx_max ); + mavlink_msg_onboard_computer_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_onboard_computer_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.uptime , packet1.type , packet1.cpu_cores , packet1.cpu_combined , packet1.gpu_cores , packet1.gpu_combined , packet1.temperature_board , packet1.temperature_core , packet1.fan_speed , packet1.ram_usage , packet1.ram_total , packet1.storage_type , packet1.storage_usage , packet1.storage_total , packet1.link_type , packet1.link_tx_rate , packet1.link_rx_rate , packet1.link_tx_max , packet1.link_rx_max ); + mavlink_msg_onboard_computer_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_COMPONENT_INFORMATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_component_information_t packet_in = { + 963497464,963497672,963497880,963498088,"QRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFG","IJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXY" + }; + mavlink_component_information_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.metadata_type = packet_in.metadata_type; + packet1.metadata_uid = packet_in.metadata_uid; + packet1.translation_uid = packet_in.translation_uid; + + mav_array_memcpy(packet1.metadata_uri, packet_in.metadata_uri, sizeof(char)*70); + mav_array_memcpy(packet1.translation_uri, packet_in.translation_uri, sizeof(char)*70); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_component_information_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_component_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_component_information_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.metadata_type , packet1.metadata_uid , packet1.metadata_uri , packet1.translation_uid , packet1.translation_uri ); + mavlink_msg_component_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_component_information_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.metadata_type , packet1.metadata_uid , packet1.metadata_uri , packet1.translation_uid , packet1.translation_uri ); + mavlink_msg_component_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PLAY_TUNE_V2 >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_play_tune_v2_t packet_in = { + 963497464,17,84,"GHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRS" + }; + mavlink_play_tune_v2_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.format = packet_in.format; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + mav_array_memcpy(packet1.tune, packet_in.tune, sizeof(char)*248); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_play_tune_v2_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_play_tune_v2_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_play_tune_v2_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.format , packet1.tune ); + mavlink_msg_play_tune_v2_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_play_tune_v2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.format , packet1.tune ); + mavlink_msg_play_tune_v2_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_SUPPORTED_TUNES >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_supported_tunes_t packet_in = { + 963497464,17,84 + }; + mavlink_supported_tunes_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.format = packet_in.format; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_supported_tunes_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_supported_tunes_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_supported_tunes_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.format ); + mavlink_msg_supported_tunes_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_supported_tunes_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.format ); + mavlink_msg_supported_tunes_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_WHEEL_DISTANCE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_wheel_distance_t packet_in = { + 93372036854775807ULL,{ 179.0, 180.0, 181.0, 182.0, 183.0, 184.0, 185.0, 186.0, 187.0, 188.0, 189.0, 190.0, 191.0, 192.0, 193.0, 194.0 },157 + }; + mavlink_wheel_distance_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.count = packet_in.count; + + mav_array_memcpy(packet1.distance, packet_in.distance, sizeof(double)*16); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_wheel_distance_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_wheel_distance_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_wheel_distance_pack(system_id, component_id, &msg , packet1.time_usec , packet1.count , packet1.distance ); + mavlink_msg_wheel_distance_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_wheel_distance_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.count , packet1.distance ); + mavlink_msg_wheel_distance_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_WINCH_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_winch_status_t packet_in = { + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,963498920,18899 + }; + mavlink_winch_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.line_length = packet_in.line_length; + packet1.speed = packet_in.speed; + packet1.tension = packet_in.tension; + packet1.voltage = packet_in.voltage; + packet1.current = packet_in.current; + packet1.status = packet_in.status; + packet1.temperature = packet_in.temperature; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_winch_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_winch_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_winch_status_pack(system_id, component_id, &msg , packet1.time_usec , packet1.line_length , packet1.speed , packet1.tension , packet1.voltage , packet1.current , packet1.temperature , packet1.status ); + mavlink_msg_winch_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_winch_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.line_length , packet1.speed , packet1.tension , packet1.voltage , packet1.current , packet1.temperature , packet1.status ); + mavlink_msg_winch_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_open_drone_id_basic_id_t packet_in = { + 5,72,{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158 },199,10,{ 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96 } + }; + mavlink_open_drone_id_basic_id_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.id_type = packet_in.id_type; + packet1.ua_type = packet_in.ua_type; + + mav_array_memcpy(packet1.id_or_mac, packet_in.id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet1.uas_id, packet_in.uas_id, sizeof(uint8_t)*20); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_basic_id_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_open_drone_id_basic_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_basic_id_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.id_type , packet1.ua_type , packet1.uas_id ); + mavlink_msg_open_drone_id_basic_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_basic_id_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.id_type , packet1.ua_type , packet1.uas_id ); + mavlink_msg_open_drone_id_basic_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_open_drone_id_location_t packet_in = { + 963497464,963497672,73.0,101.0,129.0,157.0,18483,18587,18691,223,34,{ 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120 },161,228,39,106,173,240,51 + }; + mavlink_open_drone_id_location_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.latitude = packet_in.latitude; + packet1.longitude = packet_in.longitude; + packet1.altitude_barometric = packet_in.altitude_barometric; + packet1.altitude_geodetic = packet_in.altitude_geodetic; + packet1.height = packet_in.height; + packet1.timestamp = packet_in.timestamp; + packet1.direction = packet_in.direction; + packet1.speed_horizontal = packet_in.speed_horizontal; + packet1.speed_vertical = packet_in.speed_vertical; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.status = packet_in.status; + packet1.height_reference = packet_in.height_reference; + packet1.horizontal_accuracy = packet_in.horizontal_accuracy; + packet1.vertical_accuracy = packet_in.vertical_accuracy; + packet1.barometer_accuracy = packet_in.barometer_accuracy; + packet1.speed_accuracy = packet_in.speed_accuracy; + packet1.timestamp_accuracy = packet_in.timestamp_accuracy; + + mav_array_memcpy(packet1.id_or_mac, packet_in.id_or_mac, sizeof(uint8_t)*20); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_location_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_open_drone_id_location_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_location_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.status , packet1.direction , packet1.speed_horizontal , packet1.speed_vertical , packet1.latitude , packet1.longitude , packet1.altitude_barometric , packet1.altitude_geodetic , packet1.height_reference , packet1.height , packet1.horizontal_accuracy , packet1.vertical_accuracy , packet1.barometer_accuracy , packet1.speed_accuracy , packet1.timestamp , packet1.timestamp_accuracy ); + mavlink_msg_open_drone_id_location_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_location_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.status , packet1.direction , packet1.speed_horizontal , packet1.speed_vertical , packet1.latitude , packet1.longitude , packet1.altitude_barometric , packet1.altitude_geodetic , packet1.height_reference , packet1.height , packet1.horizontal_accuracy , packet1.vertical_accuracy , packet1.barometer_accuracy , packet1.speed_accuracy , packet1.timestamp , packet1.timestamp_accuracy ); + mavlink_msg_open_drone_id_location_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_open_drone_id_authentication_t packet_in = { + 963497464,17,84,{ 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170 },211,22,89,156,{ 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245 } + }; + mavlink_open_drone_id_authentication_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.timestamp = packet_in.timestamp; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.authentication_type = packet_in.authentication_type; + packet1.data_page = packet_in.data_page; + packet1.page_count = packet_in.page_count; + packet1.length = packet_in.length; + + mav_array_memcpy(packet1.id_or_mac, packet_in.id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet1.authentication_data, packet_in.authentication_data, sizeof(uint8_t)*23); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_authentication_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_open_drone_id_authentication_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_authentication_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.authentication_type , packet1.data_page , packet1.page_count , packet1.length , packet1.timestamp , packet1.authentication_data ); + mavlink_msg_open_drone_id_authentication_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_authentication_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.authentication_type , packet1.data_page , packet1.page_count , packet1.length , packet1.timestamp , packet1.authentication_data ); + mavlink_msg_open_drone_id_authentication_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_open_drone_id_self_id_t packet_in = { + 5,72,{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158 },199,"XYZABCDEFGHIJKLMNOPQRS" + }; + mavlink_open_drone_id_self_id_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.description_type = packet_in.description_type; + + mav_array_memcpy(packet1.id_or_mac, packet_in.id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet1.description, packet_in.description, sizeof(char)*23); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_self_id_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_open_drone_id_self_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_self_id_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.description_type , packet1.description ); + mavlink_msg_open_drone_id_self_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_self_id_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.description_type , packet1.description ); + mavlink_msg_open_drone_id_self_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_open_drone_id_system_t packet_in = { + 963497464,963497672,73.0,101.0,18067,18171,65,132,{ 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218 },3,70,137,204 + }; + mavlink_open_drone_id_system_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.operator_latitude = packet_in.operator_latitude; + packet1.operator_longitude = packet_in.operator_longitude; + packet1.area_ceiling = packet_in.area_ceiling; + packet1.area_floor = packet_in.area_floor; + packet1.area_count = packet_in.area_count; + packet1.area_radius = packet_in.area_radius; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.operator_location_type = packet_in.operator_location_type; + packet1.classification_type = packet_in.classification_type; + packet1.category_eu = packet_in.category_eu; + packet1.class_eu = packet_in.class_eu; + + mav_array_memcpy(packet1.id_or_mac, packet_in.id_or_mac, sizeof(uint8_t)*20); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_system_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_open_drone_id_system_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_system_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.operator_location_type , packet1.classification_type , packet1.operator_latitude , packet1.operator_longitude , packet1.area_count , packet1.area_radius , packet1.area_ceiling , packet1.area_floor , packet1.category_eu , packet1.class_eu ); + mavlink_msg_open_drone_id_system_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_system_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.operator_location_type , packet1.classification_type , packet1.operator_latitude , packet1.operator_longitude , packet1.area_count , packet1.area_radius , packet1.area_ceiling , packet1.area_floor , packet1.category_eu , packet1.class_eu ); + mavlink_msg_open_drone_id_system_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_open_drone_id_operator_id_t packet_in = { + 5,72,{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158 },199,"XYZABCDEFGHIJKLMNOP" + }; + mavlink_open_drone_id_operator_id_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.operator_id_type = packet_in.operator_id_type; + + mav_array_memcpy(packet1.id_or_mac, packet_in.id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet1.operator_id, packet_in.operator_id, sizeof(char)*20); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_operator_id_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_open_drone_id_operator_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_operator_id_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.operator_id_type , packet1.operator_id ); + mavlink_msg_open_drone_id_operator_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_operator_id_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.operator_id_type , packet1.operator_id ); + mavlink_msg_open_drone_id_operator_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_open_drone_id_message_pack_t packet_in = { + 5,72,139,206,{ 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 } + }; + mavlink_open_drone_id_message_pack_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.single_message_size = packet_in.single_message_size; + packet1.msg_pack_size = packet_in.msg_pack_size; + + mav_array_memcpy(packet1.messages, packet_in.messages, sizeof(uint8_t)*250); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_message_pack_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_open_drone_id_message_pack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_message_pack_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.single_message_size , packet1.msg_pack_size , packet1.messages ); + mavlink_msg_open_drone_id_message_pack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_message_pack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.single_message_size , packet1.msg_pack_size , packet1.messages ); + mavlink_msg_open_drone_id_message_pack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i mavlink_message_info[mid].msgid) { + low = mid; + continue; + } + low = mid; + break; + } + if (mavlink_message_info[low].msgid == msgid) { + return &mavlink_message_info[low]; + } + return NULL; +} + +/* + return the message_info struct for a message +*/ +MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info(const mavlink_message_t *msg) +{ + return mavlink_get_message_info_by_id(msg->msgid); +} + +/* + return the message_info struct for a message +*/ +MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info_by_name(const char *name) +{ + static const struct { const char *name; uint32_t msgid; } mavlink_message_names[] = MAVLINK_MESSAGE_NAMES; + /* + use a bisection search to find the right entry. A perfect hash may be better + Note that this assumes the table is sorted with primary key name + */ + uint32_t low=0, high=sizeof(mavlink_message_names)/sizeof(mavlink_message_names[0]); + while (low < high) { + uint32_t mid = (low+1+high)/2; + int cmp = strcmp(mavlink_message_names[mid].name, name); + if (cmp == 0) { + return mavlink_get_message_info_by_id(mavlink_message_names[mid].msgid); + } + if (cmp > 0) { + high = mid-1; + } else { + low = mid; + } + } + return NULL; +} +#endif // MAVLINK_USE_MESSAGE_INFO + + diff --git a/lib/main/MAVLink/mavlink_helpers.h b/lib/main/MAVLink/mavlink_helpers.h index 2f30cb7ad7..fed04d6885 100755 --- a/lib/main/MAVLink/mavlink_helpers.h +++ b/lib/main/MAVLink/mavlink_helpers.h @@ -1,15 +1,21 @@ -#ifndef _MAVLINK_HELPERS_H_ -#define _MAVLINK_HELPERS_H_ +#pragma once #include "string.h" #include "checksum.h" #include "mavlink_types.h" #include "mavlink_conversions.h" +#include #ifndef MAVLINK_HELPER #define MAVLINK_HELPER #endif +#include "mavlink_sha256.h" + +#ifdef MAVLINK_USE_CXX_NAMESPACE +namespace mavlink { +#endif + /* * Internal function to give access to the channel status for each channel */ @@ -41,7 +47,15 @@ MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) #endif return &m_mavlink_buffer[chan]; } -#endif +#endif // MAVLINK_GET_CHANNEL_BUFFER + +/* Enable this option to check the length of each message. + This allows invalid messages to be caught much sooner. Use if the transmission + medium is prone to missing (or extra) characters (e.g. a radio that fades in + and out). Only use if the channel will only contain messages types listed in + the headers. +*/ +//#define MAVLINK_CHECK_MESSAGE_LENGTH /** * @brief Reset the status of a channel. @@ -52,6 +66,137 @@ MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan) status->parse_state = MAVLINK_PARSE_STATE_IDLE; } +/** + * @brief create a signature block for a packet + */ +MAVLINK_HELPER uint8_t mavlink_sign_packet(mavlink_signing_t *signing, + uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN], + const uint8_t *header, uint8_t header_len, + const uint8_t *packet, uint8_t packet_len, + const uint8_t crc[2]) +{ + mavlink_sha256_ctx ctx; + union { + uint64_t t64; + uint8_t t8[8]; + } tstamp; + if (signing == NULL || !(signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING)) { + return 0; + } + signature[0] = signing->link_id; + tstamp.t64 = signing->timestamp; + memcpy(&signature[1], tstamp.t8, 6); + signing->timestamp++; + + mavlink_sha256_init(&ctx); + mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key)); + mavlink_sha256_update(&ctx, header, header_len); + mavlink_sha256_update(&ctx, packet, packet_len); + mavlink_sha256_update(&ctx, crc, 2); + mavlink_sha256_update(&ctx, signature, 7); + mavlink_sha256_final_48(&ctx, &signature[7]); + + return MAVLINK_SIGNATURE_BLOCK_LEN; +} + +/** + * @brief Trim payload of any trailing zero-populated bytes (MAVLink 2 only). + * + * @param payload Serialised payload buffer. + * @param length Length of full-width payload buffer. + * @return Length of payload after zero-filled bytes are trimmed. + */ +MAVLINK_HELPER uint8_t _mav_trim_payload(const char *payload, uint8_t length) +{ + while (length > 1 && payload[length-1] == 0) { + length--; + } + return length; +} + +/** + * @brief check a signature block for a packet + */ +MAVLINK_HELPER bool mavlink_signature_check(mavlink_signing_t *signing, + mavlink_signing_streams_t *signing_streams, + const mavlink_message_t *msg) +{ + if (signing == NULL) { + return true; + } + const uint8_t *p = (const uint8_t *)&msg->magic; + const uint8_t *psig = msg->signature; + const uint8_t *incoming_signature = psig+7; + mavlink_sha256_ctx ctx; + uint8_t signature[6]; + uint16_t i; + + mavlink_sha256_init(&ctx); + mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key)); + mavlink_sha256_update(&ctx, p, MAVLINK_CORE_HEADER_LEN+1+msg->len); + mavlink_sha256_update(&ctx, msg->ck, 2); + mavlink_sha256_update(&ctx, psig, 1+6); + mavlink_sha256_final_48(&ctx, signature); + if (memcmp(signature, incoming_signature, 6) != 0) { + return false; + } + + // now check timestamp + union tstamp { + uint64_t t64; + uint8_t t8[8]; + } tstamp; + uint8_t link_id = psig[0]; + tstamp.t64 = 0; + memcpy(tstamp.t8, psig+1, 6); + + if (signing_streams == NULL) { + return false; + } + + // find stream + for (i=0; inum_signing_streams; i++) { + if (msg->sysid == signing_streams->stream[i].sysid && + msg->compid == signing_streams->stream[i].compid && + link_id == signing_streams->stream[i].link_id) { + break; + } + } + if (i == signing_streams->num_signing_streams) { + if (signing_streams->num_signing_streams >= MAVLINK_MAX_SIGNING_STREAMS) { + // over max number of streams + return false; + } + // new stream. Only accept if timestamp is not more than 1 minute old + if (tstamp.t64 + 6000*1000UL < signing->timestamp) { + return false; + } + // add new stream + signing_streams->stream[i].sysid = msg->sysid; + signing_streams->stream[i].compid = msg->compid; + signing_streams->stream[i].link_id = link_id; + signing_streams->num_signing_streams++; + } else { + union tstamp last_tstamp; + last_tstamp.t64 = 0; + memcpy(last_tstamp.t8, signing_streams->stream[i].timestamp_bytes, 6); + if (tstamp.t64 <= last_tstamp.t64) { + // repeating old timestamp + return false; + } + } + + // remember last timestamp + memcpy(signing_streams->stream[i].timestamp_bytes, psig+1, 6); + + // our next timestamp must be at least this timestamp + if (tstamp.t64 > signing->timestamp) { + signing->timestamp = tstamp.t64; + } + return true; +} + + /** * @brief Finalize a MAVLink message with channel assignment * @@ -64,52 +209,89 @@ MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan) * @param system_id Id of the sending (this) system, 1-127 * @param length Message length */ -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra) -#else -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t length) -#endif +MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra) { - // This is only used for the v2 protocol and we silence it here - (void)min_length; - // This code part is the same for all messages; - msg->magic = MAVLINK_STX; - msg->len = length; + bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0; + bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING); + uint8_t signature_len = signing? MAVLINK_SIGNATURE_BLOCK_LEN : 0; + uint8_t header_len = MAVLINK_CORE_HEADER_LEN+1; + uint8_t buf[MAVLINK_CORE_HEADER_LEN+1]; + if (mavlink1) { + msg->magic = MAVLINK_STX_MAVLINK1; + header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN+1; + } else { + msg->magic = MAVLINK_STX; + } + msg->len = mavlink1?min_length:_mav_trim_payload(_MAV_PAYLOAD(msg), length); msg->sysid = system_id; msg->compid = component_id; - // One sequence number per channel - msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; - msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN); - crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len); -#if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &msg->checksum); -#endif - mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF); - mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8); + msg->incompat_flags = 0; + if (signing) { + msg->incompat_flags |= MAVLINK_IFLAG_SIGNED; + } + msg->compat_flags = 0; + msg->seq = status->current_tx_seq; + status->current_tx_seq = status->current_tx_seq + 1; - return length + MAVLINK_NUM_NON_PAYLOAD_BYTES; + // form the header as a byte array for the crc + buf[0] = msg->magic; + buf[1] = msg->len; + if (mavlink1) { + buf[2] = msg->seq; + buf[3] = msg->sysid; + buf[4] = msg->compid; + buf[5] = msg->msgid & 0xFF; + } else { + buf[2] = msg->incompat_flags; + buf[3] = msg->compat_flags; + buf[4] = msg->seq; + buf[5] = msg->sysid; + buf[6] = msg->compid; + buf[7] = msg->msgid & 0xFF; + buf[8] = (msg->msgid >> 8) & 0xFF; + buf[9] = (msg->msgid >> 16) & 0xFF; + } + + uint16_t checksum = crc_calculate(&buf[1], header_len-1); + crc_accumulate_buffer(&checksum, _MAV_PAYLOAD(msg), msg->len); + crc_accumulate(crc_extra, &checksum); + mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF); + mavlink_ck_b(msg) = (uint8_t)(checksum >> 8); + + msg->checksum = checksum; + + if (signing) { + mavlink_sign_packet(status->signing, + msg->signature, + (const uint8_t *)buf, header_len, + (const uint8_t *)_MAV_PAYLOAD(msg), msg->len, + (const uint8_t *)_MAV_PAYLOAD(msg)+(uint16_t)msg->len); + } + + return msg->len + header_len + 2 + signature_len; } +MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra) +{ + mavlink_status_t *status = mavlink_get_channel_status(chan); + return mavlink_finalize_message_buffer(msg, system_id, component_id, status, min_length, length, crc_extra); +} /** * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel */ -#if MAVLINK_CRC_EXTRA MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t min_length, uint8_t length, uint8_t crc_extra) { return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, min_length, length, crc_extra); } -#else -MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length) + +static inline void _mav_parse_error(mavlink_status_t *status) { - return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length); + status->parse_error++; } -#endif #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len); @@ -117,42 +299,78 @@ MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, /** * @brief Finalize a MAVLink message with channel assignment and send */ -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, +MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint32_t msgid, + const char *packet, uint8_t min_length, uint8_t length, uint8_t crc_extra) -#else -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length) -#endif { uint16_t checksum; uint8_t buf[MAVLINK_NUM_HEADER_BYTES]; uint8_t ck[2]; mavlink_status_t *status = mavlink_get_channel_status(chan); - buf[0] = MAVLINK_STX; - buf[1] = length; - buf[2] = status->current_tx_seq; - buf[3] = mavlink_system.sysid; - buf[4] = mavlink_system.compid; - buf[5] = msgid; + uint8_t header_len = MAVLINK_CORE_HEADER_LEN; + uint8_t signature_len = 0; + uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN]; + bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0; + bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING); + + if (mavlink1) { + length = min_length; + if (msgid > 255) { + // can't send 16 bit messages + _mav_parse_error(status); + return; + } + header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN; + buf[0] = MAVLINK_STX_MAVLINK1; + buf[1] = length; + buf[2] = status->current_tx_seq; + buf[3] = mavlink_system.sysid; + buf[4] = mavlink_system.compid; + buf[5] = msgid & 0xFF; + } else { + uint8_t incompat_flags = 0; + if (signing) { + incompat_flags |= MAVLINK_IFLAG_SIGNED; + } + length = _mav_trim_payload(packet, length); + buf[0] = MAVLINK_STX; + buf[1] = length; + buf[2] = incompat_flags; + buf[3] = 0; // compat_flags + buf[4] = status->current_tx_seq; + buf[5] = mavlink_system.sysid; + buf[6] = mavlink_system.compid; + buf[7] = msgid & 0xFF; + buf[8] = (msgid >> 8) & 0xFF; + buf[9] = (msgid >> 16) & 0xFF; + } status->current_tx_seq++; - checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate((const uint8_t*)&buf[1], header_len); crc_accumulate_buffer(&checksum, packet, length); -#if MAVLINK_CRC_EXTRA crc_accumulate(crc_extra, &checksum); -#endif ck[0] = (uint8_t)(checksum & 0xFF); ck[1] = (uint8_t)(checksum >> 8); - MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); - _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES); + if (signing) { + // possibly add a signature + signature_len = mavlink_sign_packet(status->signing, signature, buf, header_len+1, + (const uint8_t *)packet, length, ck); + } + + MAVLINK_START_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len); + _mavlink_send_uart(chan, (const char *)buf, header_len+1); _mavlink_send_uart(chan, packet, length); _mavlink_send_uart(chan, (const char *)ck, 2); - MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); + if (signature_len != 0) { + _mavlink_send_uart(chan, (const char *)signature, signature_len); + } + MAVLINK_END_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len); } /** * @brief re-send a message over a uart channel * this is more stack efficient than re-marshalling the message + * If the message is signed then the original signature is also sent */ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg) { @@ -162,27 +380,92 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m ck[1] = (uint8_t)(msg->checksum >> 8); // XXX use the right sequence here - MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); - _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES); + uint8_t header_len; + uint8_t signature_len; + + if (msg->magic == MAVLINK_STX_MAVLINK1) { + header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1; + signature_len = 0; + MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len); + // we can't send the structure directly as it has extra mavlink2 elements in it + uint8_t buf[MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1]; + buf[0] = msg->magic; + buf[1] = msg->len; + buf[2] = msg->seq; + buf[3] = msg->sysid; + buf[4] = msg->compid; + buf[5] = msg->msgid & 0xFF; + _mavlink_send_uart(chan, (const char*)buf, header_len); + } else { + header_len = MAVLINK_CORE_HEADER_LEN + 1; + signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0; + MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len); + uint8_t buf[MAVLINK_CORE_HEADER_LEN + 1]; + buf[0] = msg->magic; + buf[1] = msg->len; + buf[2] = msg->incompat_flags; + buf[3] = msg->compat_flags; + buf[4] = msg->seq; + buf[5] = msg->sysid; + buf[6] = msg->compid; + buf[7] = msg->msgid & 0xFF; + buf[8] = (msg->msgid >> 8) & 0xFF; + buf[9] = (msg->msgid >> 16) & 0xFF; + _mavlink_send_uart(chan, (const char *)buf, header_len); + } _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len); _mavlink_send_uart(chan, (const char *)ck, 2); - MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); + if (signature_len != 0) { + _mavlink_send_uart(chan, (const char *)msg->signature, MAVLINK_SIGNATURE_BLOCK_LEN); + } + MAVLINK_END_UART_SEND(chan, header_len + msg->len + 2 + signature_len); } #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Pack a message to send it over a serial byte stream */ -MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg) +MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg) { - memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); - - uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); - + uint8_t signature_len, header_len; + uint8_t *ck; + uint8_t length = msg->len; + + if (msg->magic == MAVLINK_STX_MAVLINK1) { + signature_len = 0; + header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN; + buf[0] = msg->magic; + buf[1] = length; + buf[2] = msg->seq; + buf[3] = msg->sysid; + buf[4] = msg->compid; + buf[5] = msg->msgid & 0xFF; + memcpy(&buf[6], _MAV_PAYLOAD(msg), msg->len); + ck = buf + header_len + 1 + (uint16_t)msg->len; + } else { + length = _mav_trim_payload(_MAV_PAYLOAD(msg), length); + header_len = MAVLINK_CORE_HEADER_LEN; + buf[0] = msg->magic; + buf[1] = length; + buf[2] = msg->incompat_flags; + buf[3] = msg->compat_flags; + buf[4] = msg->seq; + buf[5] = msg->sysid; + buf[6] = msg->compid; + buf[7] = msg->msgid & 0xFF; + buf[8] = (msg->msgid >> 8) & 0xFF; + buf[9] = (msg->msgid >> 16) & 0xFF; + memcpy(&buf[10], _MAV_PAYLOAD(msg), length); + ck = buf + header_len + 1 + (uint16_t)length; + signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0; + } ck[0] = (uint8_t)(msg->checksum & 0xFF); ck[1] = (uint8_t)(msg->checksum >> 8); + if (signature_len > 0) { + memcpy(&ck[2], msg->signature, signature_len); + } - return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; + return header_len + 1 + 2 + (uint16_t)length + (uint16_t)signature_len; } union __mavlink_bitfield { @@ -197,12 +480,78 @@ union __mavlink_bitfield { MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg) { - crc_init(&msg->checksum); + uint16_t crcTmp = 0; + crc_init(&crcTmp); + msg->checksum = crcTmp; } MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) { - crc_accumulate(c, &msg->checksum); + uint16_t checksum = msg->checksum; + crc_accumulate(c, &checksum); + msg->checksum = checksum; +} + +/* + return the crc_entry value for a msgid +*/ +#ifndef MAVLINK_GET_MSG_ENTRY +MAVLINK_HELPER const mavlink_msg_entry_t *mavlink_get_msg_entry(uint32_t msgid) +{ + static const mavlink_msg_entry_t mavlink_message_crcs[] = MAVLINK_MESSAGE_CRCS; + /* + use a bisection search to find the right entry. A perfect hash may be better + Note that this assumes the table is sorted by msgid + */ + uint32_t low=0, high=sizeof(mavlink_message_crcs)/sizeof(mavlink_message_crcs[0]) - 1; + while (low < high) { + uint32_t mid = (low+1+high)/2; + if (msgid < mavlink_message_crcs[mid].msgid) { + high = mid-1; + continue; + } + if (msgid > mavlink_message_crcs[mid].msgid) { + low = mid; + continue; + } + low = mid; + break; + } + if (mavlink_message_crcs[low].msgid != msgid) { + // msgid is not in the table + return NULL; + } + return &mavlink_message_crcs[low]; +} +#endif // MAVLINK_GET_MSG_ENTRY + +/* + return the crc_extra value for a message +*/ +MAVLINK_HELPER uint8_t mavlink_get_crc_extra(const mavlink_message_t *msg) +{ + const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid); + return e?e->crc_extra:0; +} + +/* + return the min message length +*/ +#define MAVLINK_HAVE_MIN_MESSAGE_LENGTH +MAVLINK_HELPER uint8_t mavlink_min_message_length(const mavlink_message_t *msg) +{ + const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid); + return e?e->min_msg_len:0; +} + +/* + return the max message length (including extensions) +*/ +#define MAVLINK_HAVE_MAX_MESSAGE_LENGTH +MAVLINK_HELPER uint8_t mavlink_max_message_length(const mavlink_message_t *msg) +{ + const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid); + return e?e->max_msg_len:0; } /** @@ -211,12 +560,13 @@ MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) * parser in a library that doesn't use any global variables * * @param rxmsg parsing message buffer - * @param status parsing status buffer + * @param status parsing status buffer * @param c The char to parse * * @param r_message NULL if no message could be decoded, otherwise the message data * @param r_mavlink_status if a message was decoded, this is filled with the channel's stats * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC + * */ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, mavlink_status_t* status, @@ -224,30 +574,6 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) { - /* - default message crc function. You can override this per-system to - put this data in a different memory segment - */ -#if MAVLINK_CRC_EXTRA -#ifndef MAVLINK_MESSAGE_CRC - static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; -#define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid] -#endif -#endif - - /* Enable this option to check the length of each message. - This allows invalid messages to be caught much sooner. Use if the transmission - medium is prone to missing (or extra) characters (e.g. a radio that fades in - and out). Only use if the channel will only contain messages types listed in - the headers. - */ -#ifdef MAVLINK_CHECK_MESSAGE_LENGTH -#ifndef MAVLINK_MESSAGE_LENGTH - static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; -#define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid] -#endif -#endif - int bufferIndex = 0; status->msg_received = MAVLINK_FRAMING_INCOMPLETE; @@ -261,6 +587,14 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; rxmsg->len = 0; rxmsg->magic = c; + status->flags &= ~MAVLINK_STATUS_FLAG_IN_MAVLINK1; + mavlink_start_checksum(rxmsg); + } else if (c == MAVLINK_STX_MAVLINK1) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + rxmsg->len = 0; + rxmsg->magic = c; + status->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1; mavlink_start_checksum(rxmsg); } break; @@ -275,7 +609,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, ) { status->buffer_overrun++; - status->parse_error++; + _mav_parse_error(status); status->msg_received = 0; status->parse_state = MAVLINK_PARSE_STATE_IDLE; } @@ -285,16 +619,41 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, rxmsg->len = c; status->packet_idx = 0; mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; + if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) { + rxmsg->incompat_flags = 0; + rxmsg->compat_flags = 0; + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS; + } else { + status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; + } } break; case MAVLINK_PARSE_STATE_GOT_LENGTH: + rxmsg->incompat_flags = c; + if ((rxmsg->incompat_flags & ~MAVLINK_IFLAG_MASK) != 0) { + // message includes an incompatible feature flag + _mav_parse_error(status); + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + break; + } + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS; + break; + + case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS: + rxmsg->compat_flags = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS; + break; + + case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS: rxmsg->seq = c; mavlink_update_checksum(rxmsg, c); status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; break; - + case MAVLINK_PARSE_STATE_GOT_SEQ: rxmsg->sysid = c; mavlink_update_checksum(rxmsg, c); @@ -304,31 +663,57 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, case MAVLINK_PARSE_STATE_GOT_SYSID: rxmsg->compid = c; mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; break; case MAVLINK_PARSE_STATE_GOT_COMPID: -#ifdef MAVLINK_CHECK_MESSAGE_LENGTH - if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c)) - { - status->parse_error++; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - break; - } -#endif rxmsg->msgid = c; mavlink_update_checksum(rxmsg, c); - if (rxmsg->len == 0) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; + if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) { + if(rxmsg->len > 0) { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3; + } else { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } +#ifdef MAVLINK_CHECK_MESSAGE_LENGTH + if (rxmsg->len < mavlink_min_message_length(rxmsg) || + rxmsg->len > mavlink_max_message_length(rxmsg)) { + _mav_parse_error(status); + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + break; + } +#endif + } else { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID1; } break; - case MAVLINK_PARSE_STATE_GOT_MSGID: + case MAVLINK_PARSE_STATE_GOT_MSGID1: + rxmsg->msgid |= c<<8; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID2; + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID2: + rxmsg->msgid |= ((uint32_t)c)<<16; + mavlink_update_checksum(rxmsg, c); + if(rxmsg->len > 0){ + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3; + } else { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } +#ifdef MAVLINK_CHECK_MESSAGE_LENGTH + if (rxmsg->len < mavlink_min_message_length(rxmsg) || + rxmsg->len > mavlink_max_message_length(rxmsg)) + { + _mav_parse_error(status); + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + break; + } +#endif + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID3: _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c; mavlink_update_checksum(rxmsg, c); if (status->packet_idx == rxmsg->len) @@ -337,17 +722,23 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, } break; - case MAVLINK_PARSE_STATE_GOT_PAYLOAD: -#if MAVLINK_CRC_EXTRA - mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid)); -#endif + case MAVLINK_PARSE_STATE_GOT_PAYLOAD: { + const mavlink_msg_entry_t *e = mavlink_get_msg_entry(rxmsg->msgid); + uint8_t crc_extra = e?e->crc_extra:0; + mavlink_update_checksum(rxmsg, crc_extra); if (c != (rxmsg->checksum & 0xFF)) { status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1; } else { status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; } - _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c; + rxmsg->ck[0] = c; + + // zero-fill the packet to cope with short incoming packets + if (e && status->packet_idx < e->max_msg_len) { + memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->max_msg_len - status->packet_idx); + } break; + } case MAVLINK_PARSE_STATE_GOT_CRC1: case MAVLINK_PARSE_STATE_GOT_BAD_CRC1: @@ -357,10 +748,56 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, } else { // Successfully got message status->msg_received = MAVLINK_FRAMING_OK; - } - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c; - memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + } + rxmsg->ck[1] = c; + + if (rxmsg->incompat_flags & MAVLINK_IFLAG_SIGNED) { + status->parse_state = MAVLINK_PARSE_STATE_SIGNATURE_WAIT; + status->signature_wait = MAVLINK_SIGNATURE_BLOCK_LEN; + + // If the CRC is already wrong, don't overwrite msg_received, + // otherwise we can end up with garbage flagged as valid. + if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) { + status->msg_received = MAVLINK_FRAMING_INCOMPLETE; + } + } else { + if (status->signing && + (status->signing->accept_unsigned_callback == NULL || + !status->signing->accept_unsigned_callback(status, rxmsg->msgid))) { + + // If the CRC is already wrong, don't overwrite msg_received. + if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) { + status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE; + } + } + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (r_message != NULL) { + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + } + } + break; + case MAVLINK_PARSE_STATE_SIGNATURE_WAIT: + rxmsg->signature[MAVLINK_SIGNATURE_BLOCK_LEN-status->signature_wait] = c; + status->signature_wait--; + if (status->signature_wait == 0) { + // we have the whole signature, check it is OK + bool sig_ok = mavlink_signature_check(status->signing, status->signing_streams, rxmsg); + if (!sig_ok && + (status->signing->accept_unsigned_callback && + status->signing->accept_unsigned_callback(status, rxmsg->msgid))) { + // accepted via application level override + sig_ok = true; + } + if (sig_ok) { + status->msg_received = MAVLINK_FRAMING_OK; + } else { + status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE; + } + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (r_message !=NULL) { + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + } + } break; } @@ -380,13 +817,18 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, status->packet_rx_success_count++; } - r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg - r_mavlink_status->parse_state = status->parse_state; - r_mavlink_status->packet_idx = status->packet_idx; - r_mavlink_status->current_rx_seq = status->current_rx_seq+1; - r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; - r_mavlink_status->packet_rx_drop_count = status->parse_error; - status->parse_error = 0; + if (r_message != NULL) { + r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg + } + if (r_mavlink_status != NULL) { + r_mavlink_status->parse_state = status->parse_state; + r_mavlink_status->packet_idx = status->packet_idx; + r_mavlink_status->current_rx_seq = status->current_rx_seq+1; + r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; + r_mavlink_status->packet_rx_drop_count = status->parse_error; + r_mavlink_status->flags = status->flags; + } + status->parse_error = 0; if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) { /* @@ -396,7 +838,9 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, mavlink_msg_to_send_buffer() won't overwrite the checksum */ - r_message->checksum = _MAV_PAYLOAD(rxmsg)[status->packet_idx] | (_MAV_PAYLOAD(rxmsg)[status->packet_idx+1]<<8); + if (r_message != NULL) { + r_message->checksum = rxmsg->ck[0] | (rxmsg->ck[1]<<8); + } } return status->msg_received; @@ -418,7 +862,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows * @param c The char to parse * - * @param r_message NULL if no message could be decoded, the message data else + * @param r_message NULL if no message could be decoded, otherwise the message data * @param r_mavlink_status if a message was decoded, this is filled with the channel's stats * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC * @@ -453,6 +897,33 @@ MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_messa r_mavlink_status); } +/** + * Set the protocol version + */ +MAVLINK_HELPER void mavlink_set_proto_version(uint8_t chan, unsigned int version) +{ + mavlink_status_t *status = mavlink_get_channel_status(chan); + if (version > 1) { + status->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1); + } else { + status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } +} + +/** + * Get the protocol version + * + * @return 1 for v1, 2 for v2 + */ +MAVLINK_HELPER unsigned int mavlink_get_proto_version(uint8_t chan) +{ + mavlink_status_t *status = mavlink_get_channel_status(chan); + if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) > 0) { + return 1; + } else { + return 2; + } +} /** * This is a convenience function which handles the complete MAVLink parsing. @@ -469,7 +940,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_messa * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows * @param c The char to parse * - * @param r_message NULL if no message could be decoded, otherwise the message data. + * @param r_message NULL if no message could be decoded, otherwise the message data * @param r_mavlink_status if a message was decoded, this is filled with the channel's stats * @return 0 if no message could be decoded or bad CRC, 1 on good message and CRC * @@ -498,11 +969,12 @@ MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_messa MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) { uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status); - if (msg_received == MAVLINK_FRAMING_BAD_CRC) { + if (msg_received == MAVLINK_FRAMING_BAD_CRC || + msg_received == MAVLINK_FRAMING_BAD_SIGNATURE) { // we got a bad CRC. Treat as a parse failure mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan); mavlink_status_t* status = mavlink_get_channel_status(chan); - status->parse_error++; + _mav_parse_error(status); status->msg_received = MAVLINK_FRAMING_INCOMPLETE; status->parse_state = MAVLINK_PARSE_STATE_IDLE; if (c == MAVLINK_STX) @@ -656,4 +1128,6 @@ MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, } #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS -#endif /* _MAVLINK_HELPERS_H_ */ +#ifdef MAVLINK_USE_CXX_NAMESPACE +} // namespace mavlink +#endif diff --git a/lib/main/MAVLink/mavlink_sha256.h b/lib/main/MAVLink/mavlink_sha256.h new file mode 100644 index 0000000000..7accd03566 --- /dev/null +++ b/lib/main/MAVLink/mavlink_sha256.h @@ -0,0 +1,235 @@ +#pragma once + +/* + sha-256 implementation for MAVLink based on Heimdal sources, with + modifications to suit mavlink headers + */ +/* + * Copyright (c) 1995 - 2001 Kungliga Tekniska Högskolan + * (Royal Institute of Technology, Stockholm, Sweden). + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * 3. Neither the name of the Institute nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + */ + +/* + allow implementation to provide their own sha256 with the same API +*/ +#ifndef HAVE_MAVLINK_SHA256 + +#ifdef MAVLINK_USE_CXX_NAMESPACE +namespace mavlink { +#endif + +#ifndef MAVLINK_HELPER +#define MAVLINK_HELPER +#endif + +typedef struct { + uint32_t sz[2]; + uint32_t counter[8]; + union { + unsigned char save_bytes[64]; + uint32_t save_u32[16]; + } u; +} mavlink_sha256_ctx; + +#define Ch(x,y,z) (((x) & (y)) ^ ((~(x)) & (z))) +#define Maj(x,y,z) (((x) & (y)) ^ ((x) & (z)) ^ ((y) & (z))) + +#define ROTR(x,n) (((x)>>(n)) | ((x) << (32 - (n)))) + +#define Sigma0(x) (ROTR(x,2) ^ ROTR(x,13) ^ ROTR(x,22)) +#define Sigma1(x) (ROTR(x,6) ^ ROTR(x,11) ^ ROTR(x,25)) +#define sigma0(x) (ROTR(x,7) ^ ROTR(x,18) ^ ((x)>>3)) +#define sigma1(x) (ROTR(x,17) ^ ROTR(x,19) ^ ((x)>>10)) + +static const uint32_t mavlink_sha256_constant_256[64] = { + 0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, + 0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5, + 0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3, + 0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174, + 0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc, + 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da, + 0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7, + 0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967, + 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13, + 0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85, + 0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3, + 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070, + 0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5, + 0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3, + 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208, + 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2 +}; + +MAVLINK_HELPER void mavlink_sha256_init(mavlink_sha256_ctx *m) +{ + m->sz[0] = 0; + m->sz[1] = 0; + m->counter[0] = 0x6a09e667; + m->counter[1] = 0xbb67ae85; + m->counter[2] = 0x3c6ef372; + m->counter[3] = 0xa54ff53a; + m->counter[4] = 0x510e527f; + m->counter[5] = 0x9b05688c; + m->counter[6] = 0x1f83d9ab; + m->counter[7] = 0x5be0cd19; +} + +static inline void mavlink_sha256_calc(mavlink_sha256_ctx *m, uint32_t *in) +{ + uint32_t AA, BB, CC, DD, EE, FF, GG, HH; + uint32_t data[64]; + int i; + + AA = m->counter[0]; + BB = m->counter[1]; + CC = m->counter[2]; + DD = m->counter[3]; + EE = m->counter[4]; + FF = m->counter[5]; + GG = m->counter[6]; + HH = m->counter[7]; + + for (i = 0; i < 16; ++i) + data[i] = in[i]; + for (i = 16; i < 64; ++i) + data[i] = sigma1(data[i-2]) + data[i-7] + + sigma0(data[i-15]) + data[i - 16]; + + for (i = 0; i < 64; i++) { + uint32_t T1, T2; + + T1 = HH + Sigma1(EE) + Ch(EE, FF, GG) + mavlink_sha256_constant_256[i] + data[i]; + T2 = Sigma0(AA) + Maj(AA,BB,CC); + + HH = GG; + GG = FF; + FF = EE; + EE = DD + T1; + DD = CC; + CC = BB; + BB = AA; + AA = T1 + T2; + } + + m->counter[0] += AA; + m->counter[1] += BB; + m->counter[2] += CC; + m->counter[3] += DD; + m->counter[4] += EE; + m->counter[5] += FF; + m->counter[6] += GG; + m->counter[7] += HH; +} + +MAVLINK_HELPER void mavlink_sha256_update(mavlink_sha256_ctx *m, const void *v, uint32_t len) +{ + const unsigned char *p = (const unsigned char *)v; + uint32_t old_sz = m->sz[0]; + uint32_t offset; + + m->sz[0] += len * 8; + if (m->sz[0] < old_sz) + ++m->sz[1]; + offset = (old_sz / 8) % 64; + while(len > 0){ + uint32_t l = 64 - offset; + if (len < l) { + l = len; + } + memcpy(m->u.save_bytes + offset, p, l); + offset += l; + p += l; + len -= l; + if(offset == 64){ + int i; + uint32_t current[16]; + const uint32_t *u = m->u.save_u32; + for (i = 0; i < 16; i++){ + const uint8_t *p1 = (const uint8_t *)&u[i]; + uint8_t *p2 = (uint8_t *)¤t[i]; + p2[0] = p1[3]; + p2[1] = p1[2]; + p2[2] = p1[1]; + p2[3] = p1[0]; + } + mavlink_sha256_calc(m, current); + offset = 0; + } + } +} + +/* + get first 48 bits of final sha256 hash + */ +MAVLINK_HELPER void mavlink_sha256_final_48(mavlink_sha256_ctx *m, uint8_t result[6]) +{ + unsigned char zeros[72]; + unsigned offset = (m->sz[0] / 8) % 64; + unsigned int dstart = (120 - offset - 1) % 64 + 1; + uint8_t *p = (uint8_t *)&m->counter[0]; + + *zeros = 0x80; + memset (zeros + 1, 0, sizeof(zeros) - 1); + zeros[dstart+7] = (m->sz[0] >> 0) & 0xff; + zeros[dstart+6] = (m->sz[0] >> 8) & 0xff; + zeros[dstart+5] = (m->sz[0] >> 16) & 0xff; + zeros[dstart+4] = (m->sz[0] >> 24) & 0xff; + zeros[dstart+3] = (m->sz[1] >> 0) & 0xff; + zeros[dstart+2] = (m->sz[1] >> 8) & 0xff; + zeros[dstart+1] = (m->sz[1] >> 16) & 0xff; + zeros[dstart+0] = (m->sz[1] >> 24) & 0xff; + + mavlink_sha256_update(m, zeros, dstart + 8); + + // this ordering makes the result consistent with taking the first + // 6 bytes of more conventional sha256 functions. It assumes + // little-endian ordering of m->counter + result[0] = p[3]; + result[1] = p[2]; + result[2] = p[1]; + result[3] = p[0]; + result[4] = p[7]; + result[5] = p[6]; +} + +// prevent conflicts with users of the header +#undef Ch +#undef ROTR +#undef Sigma0 +#undef Sigma1 +#undef sigma0 +#undef sigma1 + +#ifdef MAVLINK_USE_CXX_NAMESPACE +} // namespace mavlink +#endif + +#endif // HAVE_MAVLINK_SHA256 diff --git a/lib/main/MAVLink/mavlink_types.h b/lib/main/MAVLink/mavlink_types.h index 0a98ccc087..8bdf0b51c7 100755 --- a/lib/main/MAVLink/mavlink_types.h +++ b/lib/main/MAVLink/mavlink_types.h @@ -1,13 +1,17 @@ -#ifndef MAVLINK_TYPES_H_ -#define MAVLINK_TYPES_H_ +#pragma once // Visual Studio versions before 2010 don't have stdint.h, so we just error out. #if (defined _MSC_VER) && (_MSC_VER < 1600) #error "The C-MAVLink implementation requires Visual Studio 2010 or greater" #endif +#include #include +#ifdef MAVLINK_USE_CXX_NAMESPACE +namespace mavlink { +#endif + // Macro to define packed structures #ifdef __GNUC__ #define MAVPACKED( __Declaration__ ) __Declaration__ __attribute__((packed)) @@ -20,26 +24,15 @@ #define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length #endif -#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) -#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and checksum +#define MAVLINK_CORE_HEADER_LEN 9 ///< Length of core header (of the comm. layer) +#define MAVLINK_CORE_HEADER_MAVLINK1_LEN 5 ///< Length of MAVLink1 core header (of the comm. layer) +#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and stx #define MAVLINK_NUM_CHECKSUM_BYTES 2 #define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) -#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length - -#define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255 -#define MAVLINK_EXTENDED_HEADER_LEN 14 - -#if (defined _MSC_VER) || ((defined __APPLE__) && (defined __MACH__)) || (defined __linux__) - /* full fledged 32bit++ OS */ - #define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507 -#else - /* small microcontrollers */ - #define MAVLINK_MAX_EXTENDED_PACKET_LEN 2048 -#endif - -#define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES) +#define MAVLINK_SIGNATURE_BLOCK_LEN 13 +#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_SIGNATURE_BLOCK_LEN) ///< Maximum packet length /** * Old-style 4 byte param union @@ -75,7 +68,7 @@ typedef struct param_union { * The intention is that by replacing the is_double bit with 0 the type can be directly used as a double (as the is_double bit corresponds to the * lowest mantissa bit of a double). If is_double is 0 then mavlink_type gives the type in the union. * The mavlink_types.h header will also need to have shifts/masks to define the bit boundaries in the above, - * as bitfield ordering isn’t consistent between platforms. The above is intended to be for gcc on x86, + * as bitfield ordering isn't consistent between platforms. The above is intended to be for gcc on x86, * which should be the same as gcc on little-endian arm. When using shifts/masks the value will be treated as a 64 bit unsigned number, * and the bits pulled out using the shifts/masks. */ @@ -113,23 +106,20 @@ typedef struct __mavlink_system { MAVPACKED( typedef struct __mavlink_message { - uint16_t checksum; ///< sent at end of packet - uint8_t magic; ///< protocol magic marker - uint8_t len; ///< Length of payload - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint8_t msgid; ///< ID of message in payload + uint16_t checksum; ///< sent at end of packet + uint8_t magic; ///< protocol magic marker + uint8_t len; ///< Length of payload + uint8_t incompat_flags; ///< flags that must be understood + uint8_t compat_flags; ///< flags that can be ignored if not understood + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint32_t msgid:24; ///< ID of message in payload uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; + uint8_t ck[2]; ///< incoming checksum bytes + uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN]; }) mavlink_message_t; -MAVPACKED( -typedef struct __mavlink_extended_message { - mavlink_message_t base_msg; - int32_t extended_payload_len; ///< Length of extended payload if any - uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN]; -}) mavlink_extended_message_t; - typedef enum { MAVLINK_TYPE_CHAR = 0, MAVLINK_TYPE_UINT8_T = 1, @@ -147,7 +137,7 @@ typedef enum { #define MAVLINK_MAX_FIELDS 64 typedef struct __mavlink_field_info { - const char *name; // name of this field + const char *name; // name of this field const char *print_format; // printing format hint, or NULL mavlink_message_type_t type; // type of this field unsigned int array_length; // if non-zero, field is an array @@ -158,6 +148,7 @@ typedef struct __mavlink_field_info { // note that in this structure the order of fields is the order // in the XML file, not necessary the wire order typedef struct __mavlink_message_info { + uint32_t msgid; // message ID const char *name; // name of the message unsigned num_fields; // how many fields in this message mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information @@ -170,12 +161,14 @@ typedef struct __mavlink_message_info { #define mavlink_ck_a(msg) *((msg)->len + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) #define mavlink_ck_b(msg) *(((msg)->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) +#ifndef HAVE_MAVLINK_CHANNEL_T typedef enum { MAVLINK_COMM_0, MAVLINK_COMM_1, MAVLINK_COMM_2, MAVLINK_COMM_3 } mavlink_channel_t; +#endif /* * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number @@ -194,22 +187,35 @@ typedef enum { MAVLINK_PARSE_STATE_UNINIT=0, MAVLINK_PARSE_STATE_IDLE, MAVLINK_PARSE_STATE_GOT_STX, - MAVLINK_PARSE_STATE_GOT_SEQ, MAVLINK_PARSE_STATE_GOT_LENGTH, + MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS, + MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS, + MAVLINK_PARSE_STATE_GOT_SEQ, MAVLINK_PARSE_STATE_GOT_SYSID, MAVLINK_PARSE_STATE_GOT_COMPID, - MAVLINK_PARSE_STATE_GOT_MSGID, + MAVLINK_PARSE_STATE_GOT_MSGID1, + MAVLINK_PARSE_STATE_GOT_MSGID2, + MAVLINK_PARSE_STATE_GOT_MSGID3, MAVLINK_PARSE_STATE_GOT_PAYLOAD, MAVLINK_PARSE_STATE_GOT_CRC1, - MAVLINK_PARSE_STATE_GOT_BAD_CRC1 + MAVLINK_PARSE_STATE_GOT_BAD_CRC1, + MAVLINK_PARSE_STATE_SIGNATURE_WAIT } mavlink_parse_state_t; ///< The state machine for the comm parser typedef enum { MAVLINK_FRAMING_INCOMPLETE=0, MAVLINK_FRAMING_OK=1, - MAVLINK_FRAMING_BAD_CRC=2 + MAVLINK_FRAMING_BAD_CRC=2, + MAVLINK_FRAMING_BAD_SIGNATURE=3 } mavlink_framing_t; +#define MAVLINK_STATUS_FLAG_IN_MAVLINK1 1 // last incoming packet was MAVLink1 +#define MAVLINK_STATUS_FLAG_OUT_MAVLINK1 2 // generate MAVLink1 by default +#define MAVLINK_STATUS_FLAG_IN_SIGNED 4 // last incoming packet was signed and validated +#define MAVLINK_STATUS_FLAG_IN_BADSIG 8 // last incoming packet had a bad signature + +#define MAVLINK_STX_MAVLINK1 0xFE // marker for old protocol + typedef struct __mavlink_status { uint8_t msg_received; ///< Number of received messages uint8_t buffer_overrun; ///< Number of buffer overruns @@ -220,9 +226,76 @@ typedef struct __mavlink_status { uint8_t current_tx_seq; ///< Sequence number of last packet sent uint16_t packet_rx_success_count; ///< Received packets uint16_t packet_rx_drop_count; ///< Number of packet drops + uint8_t flags; ///< MAVLINK_STATUS_FLAG_* + uint8_t signature_wait; ///< number of signature bytes left to receive + struct __mavlink_signing *signing; ///< optional signing state + struct __mavlink_signing_streams *signing_streams; ///< global record of stream timestamps } mavlink_status_t; +/* + a callback function to allow for accepting unsigned packets + */ +typedef bool (*mavlink_accept_unsigned_t)(const mavlink_status_t *status, uint32_t msgid); + +/* + flags controlling signing + */ +#define MAVLINK_SIGNING_FLAG_SIGN_OUTGOING 1 ///< Enable outgoing signing + +/* + state of MAVLink signing for this channel + */ +typedef struct __mavlink_signing { + uint8_t flags; ///< MAVLINK_SIGNING_FLAG_* + uint8_t link_id; ///< Same as MAVLINK_CHANNEL + uint64_t timestamp; ///< Timestamp, in microseconds since UNIX epoch GMT + uint8_t secret_key[32]; + mavlink_accept_unsigned_t accept_unsigned_callback; +} mavlink_signing_t; + +/* + timestamp state of each logical signing stream. This needs to be the same structure for all + connections in order to be secure + */ +#ifndef MAVLINK_MAX_SIGNING_STREAMS +#define MAVLINK_MAX_SIGNING_STREAMS 16 +#endif +typedef struct __mavlink_signing_streams { + uint16_t num_signing_streams; + struct __mavlink_signing_stream { + uint8_t link_id; ///< ID of the link (MAVLINK_CHANNEL) + uint8_t sysid; ///< Remote system ID + uint8_t compid; ///< Remote component ID + uint8_t timestamp_bytes[6]; ///< Timestamp, in microseconds since UNIX epoch GMT + } stream[MAVLINK_MAX_SIGNING_STREAMS]; +} mavlink_signing_streams_t; + + #define MAVLINK_BIG_ENDIAN 0 #define MAVLINK_LITTLE_ENDIAN 1 -#endif /* MAVLINK_TYPES_H_ */ +#define MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM 1 +#define MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT 2 + +/* + entry in table of information about each message type + */ +typedef struct __mavlink_msg_entry { + uint32_t msgid; + uint8_t crc_extra; + uint8_t min_msg_len; // minimum message length + uint8_t max_msg_len; // maximum message length (e.g. including mavlink2 extensions) + uint8_t flags; // MAV_MSG_ENTRY_FLAG_* + uint8_t target_system_ofs; // payload offset to target_system, or 0 + uint8_t target_component_ofs; // payload offset to target_component, or 0 +} mavlink_msg_entry_t; + +/* + incompat_flags bits + */ +#define MAVLINK_IFLAG_SIGNED 0x01 +#define MAVLINK_IFLAG_MASK 0x01 // mask of all understood bits + +#ifdef MAVLINK_USE_CXX_NAMESPACE +} // namespace mavlink +#endif diff --git a/lib/main/MAVLink/minimal/mavlink.h b/lib/main/MAVLink/minimal/mavlink.h new file mode 100644 index 0000000000..32c6c97c68 --- /dev/null +++ b/lib/main/MAVLink/minimal/mavlink.h @@ -0,0 +1,34 @@ +/** @file + * @brief MAVLink comm protocol built from minimal.xml + * @see http://mavlink.org + */ +#pragma once +#ifndef MAVLINK_H +#define MAVLINK_H + +#define MAVLINK_PRIMARY_XML_IDX 1 + +#ifndef MAVLINK_STX +#define MAVLINK_STX 253 +#endif + +#ifndef MAVLINK_ENDIAN +#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN +#endif + +#ifndef MAVLINK_ALIGNED_FIELDS +#define MAVLINK_ALIGNED_FIELDS 1 +#endif + +#ifndef MAVLINK_CRC_EXTRA +#define MAVLINK_CRC_EXTRA 1 +#endif + +#ifndef MAVLINK_COMMAND_24BIT +#define MAVLINK_COMMAND_24BIT 1 +#endif + +#include "version.h" +#include "minimal.h" + +#endif // MAVLINK_H diff --git a/lib/main/MAVLink/common/mavlink_msg_heartbeat.h b/lib/main/MAVLink/minimal/mavlink_msg_heartbeat.h old mode 100755 new mode 100644 similarity index 100% rename from lib/main/MAVLink/common/mavlink_msg_heartbeat.h rename to lib/main/MAVLink/minimal/mavlink_msg_heartbeat.h diff --git a/lib/main/MAVLink/minimal/mavlink_msg_protocol_version.h b/lib/main/MAVLink/minimal/mavlink_msg_protocol_version.h new file mode 100644 index 0000000000..d795ff28c6 --- /dev/null +++ b/lib/main/MAVLink/minimal/mavlink_msg_protocol_version.h @@ -0,0 +1,306 @@ +#pragma once +// MESSAGE PROTOCOL_VERSION PACKING + +#define MAVLINK_MSG_ID_PROTOCOL_VERSION 300 + + +typedef struct __mavlink_protocol_version_t { + uint16_t version; /*< Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc.*/ + uint16_t min_version; /*< Minimum MAVLink version supported*/ + uint16_t max_version; /*< Maximum MAVLink version supported (set to the same value as version by default)*/ + uint8_t spec_version_hash[8]; /*< The first 8 bytes (not characters printed in hex!) of the git hash.*/ + uint8_t library_version_hash[8]; /*< The first 8 bytes (not characters printed in hex!) of the git hash.*/ +} mavlink_protocol_version_t; + +#define MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN 22 +#define MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN 22 +#define MAVLINK_MSG_ID_300_LEN 22 +#define MAVLINK_MSG_ID_300_MIN_LEN 22 + +#define MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC 217 +#define MAVLINK_MSG_ID_300_CRC 217 + +#define MAVLINK_MSG_PROTOCOL_VERSION_FIELD_SPEC_VERSION_HASH_LEN 8 +#define MAVLINK_MSG_PROTOCOL_VERSION_FIELD_LIBRARY_VERSION_HASH_LEN 8 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_PROTOCOL_VERSION { \ + 300, \ + "PROTOCOL_VERSION", \ + 5, \ + { { "version", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_protocol_version_t, version) }, \ + { "min_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_protocol_version_t, min_version) }, \ + { "max_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_protocol_version_t, max_version) }, \ + { "spec_version_hash", NULL, MAVLINK_TYPE_UINT8_T, 8, 6, offsetof(mavlink_protocol_version_t, spec_version_hash) }, \ + { "library_version_hash", NULL, MAVLINK_TYPE_UINT8_T, 8, 14, offsetof(mavlink_protocol_version_t, library_version_hash) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_PROTOCOL_VERSION { \ + "PROTOCOL_VERSION", \ + 5, \ + { { "version", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_protocol_version_t, version) }, \ + { "min_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_protocol_version_t, min_version) }, \ + { "max_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_protocol_version_t, max_version) }, \ + { "spec_version_hash", NULL, MAVLINK_TYPE_UINT8_T, 8, 6, offsetof(mavlink_protocol_version_t, spec_version_hash) }, \ + { "library_version_hash", NULL, MAVLINK_TYPE_UINT8_T, 8, 14, offsetof(mavlink_protocol_version_t, library_version_hash) }, \ + } \ +} +#endif + +/** + * @brief Pack a protocol_version message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param version Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc. + * @param min_version Minimum MAVLink version supported + * @param max_version Maximum MAVLink version supported (set to the same value as version by default) + * @param spec_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. + * @param library_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_protocol_version_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t version, uint16_t min_version, uint16_t max_version, const uint8_t *spec_version_hash, const uint8_t *library_version_hash) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN]; + _mav_put_uint16_t(buf, 0, version); + _mav_put_uint16_t(buf, 2, min_version); + _mav_put_uint16_t(buf, 4, max_version); + _mav_put_uint8_t_array(buf, 6, spec_version_hash, 8); + _mav_put_uint8_t_array(buf, 14, library_version_hash, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); +#else + mavlink_protocol_version_t packet; + packet.version = version; + packet.min_version = min_version; + packet.max_version = max_version; + mav_array_memcpy(packet.spec_version_hash, spec_version_hash, sizeof(uint8_t)*8); + mav_array_memcpy(packet.library_version_hash, library_version_hash, sizeof(uint8_t)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PROTOCOL_VERSION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); +} + +/** + * @brief Pack a protocol_version message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param version Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc. + * @param min_version Minimum MAVLink version supported + * @param max_version Maximum MAVLink version supported (set to the same value as version by default) + * @param spec_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. + * @param library_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_protocol_version_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t version,uint16_t min_version,uint16_t max_version,const uint8_t *spec_version_hash,const uint8_t *library_version_hash) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN]; + _mav_put_uint16_t(buf, 0, version); + _mav_put_uint16_t(buf, 2, min_version); + _mav_put_uint16_t(buf, 4, max_version); + _mav_put_uint8_t_array(buf, 6, spec_version_hash, 8); + _mav_put_uint8_t_array(buf, 14, library_version_hash, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); +#else + mavlink_protocol_version_t packet; + packet.version = version; + packet.min_version = min_version; + packet.max_version = max_version; + mav_array_memcpy(packet.spec_version_hash, spec_version_hash, sizeof(uint8_t)*8); + mav_array_memcpy(packet.library_version_hash, library_version_hash, sizeof(uint8_t)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PROTOCOL_VERSION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); +} + +/** + * @brief Encode a protocol_version struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param protocol_version C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_protocol_version_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_protocol_version_t* protocol_version) +{ + return mavlink_msg_protocol_version_pack(system_id, component_id, msg, protocol_version->version, protocol_version->min_version, protocol_version->max_version, protocol_version->spec_version_hash, protocol_version->library_version_hash); +} + +/** + * @brief Encode a protocol_version struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param protocol_version C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_protocol_version_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_protocol_version_t* protocol_version) +{ + return mavlink_msg_protocol_version_pack_chan(system_id, component_id, chan, msg, protocol_version->version, protocol_version->min_version, protocol_version->max_version, protocol_version->spec_version_hash, protocol_version->library_version_hash); +} + +/** + * @brief Send a protocol_version message + * @param chan MAVLink channel to send the message + * + * @param version Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc. + * @param min_version Minimum MAVLink version supported + * @param max_version Maximum MAVLink version supported (set to the same value as version by default) + * @param spec_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. + * @param library_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_protocol_version_send(mavlink_channel_t chan, uint16_t version, uint16_t min_version, uint16_t max_version, const uint8_t *spec_version_hash, const uint8_t *library_version_hash) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN]; + _mav_put_uint16_t(buf, 0, version); + _mav_put_uint16_t(buf, 2, min_version); + _mav_put_uint16_t(buf, 4, max_version); + _mav_put_uint8_t_array(buf, 6, spec_version_hash, 8); + _mav_put_uint8_t_array(buf, 14, library_version_hash, 8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PROTOCOL_VERSION, buf, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); +#else + mavlink_protocol_version_t packet; + packet.version = version; + packet.min_version = min_version; + packet.max_version = max_version; + mav_array_memcpy(packet.spec_version_hash, spec_version_hash, sizeof(uint8_t)*8); + mav_array_memcpy(packet.library_version_hash, library_version_hash, sizeof(uint8_t)*8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PROTOCOL_VERSION, (const char *)&packet, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); +#endif +} + +/** + * @brief Send a protocol_version message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_protocol_version_send_struct(mavlink_channel_t chan, const mavlink_protocol_version_t* protocol_version) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_protocol_version_send(chan, protocol_version->version, protocol_version->min_version, protocol_version->max_version, protocol_version->spec_version_hash, protocol_version->library_version_hash); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PROTOCOL_VERSION, (const char *)protocol_version, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_protocol_version_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t version, uint16_t min_version, uint16_t max_version, const uint8_t *spec_version_hash, const uint8_t *library_version_hash) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, version); + _mav_put_uint16_t(buf, 2, min_version); + _mav_put_uint16_t(buf, 4, max_version); + _mav_put_uint8_t_array(buf, 6, spec_version_hash, 8); + _mav_put_uint8_t_array(buf, 14, library_version_hash, 8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PROTOCOL_VERSION, buf, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); +#else + mavlink_protocol_version_t *packet = (mavlink_protocol_version_t *)msgbuf; + packet->version = version; + packet->min_version = min_version; + packet->max_version = max_version; + mav_array_memcpy(packet->spec_version_hash, spec_version_hash, sizeof(uint8_t)*8); + mav_array_memcpy(packet->library_version_hash, library_version_hash, sizeof(uint8_t)*8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PROTOCOL_VERSION, (const char *)packet, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE PROTOCOL_VERSION UNPACKING + + +/** + * @brief Get field version from protocol_version message + * + * @return Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc. + */ +static inline uint16_t mavlink_msg_protocol_version_get_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field min_version from protocol_version message + * + * @return Minimum MAVLink version supported + */ +static inline uint16_t mavlink_msg_protocol_version_get_min_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field max_version from protocol_version message + * + * @return Maximum MAVLink version supported (set to the same value as version by default) + */ +static inline uint16_t mavlink_msg_protocol_version_get_max_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field spec_version_hash from protocol_version message + * + * @return The first 8 bytes (not characters printed in hex!) of the git hash. + */ +static inline uint16_t mavlink_msg_protocol_version_get_spec_version_hash(const mavlink_message_t* msg, uint8_t *spec_version_hash) +{ + return _MAV_RETURN_uint8_t_array(msg, spec_version_hash, 8, 6); +} + +/** + * @brief Get field library_version_hash from protocol_version message + * + * @return The first 8 bytes (not characters printed in hex!) of the git hash. + */ +static inline uint16_t mavlink_msg_protocol_version_get_library_version_hash(const mavlink_message_t* msg, uint8_t *library_version_hash) +{ + return _MAV_RETURN_uint8_t_array(msg, library_version_hash, 8, 14); +} + +/** + * @brief Decode a protocol_version message into a struct + * + * @param msg The message to decode + * @param protocol_version C-struct to decode the message contents into + */ +static inline void mavlink_msg_protocol_version_decode(const mavlink_message_t* msg, mavlink_protocol_version_t* protocol_version) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + protocol_version->version = mavlink_msg_protocol_version_get_version(msg); + protocol_version->min_version = mavlink_msg_protocol_version_get_min_version(msg); + protocol_version->max_version = mavlink_msg_protocol_version_get_max_version(msg); + mavlink_msg_protocol_version_get_spec_version_hash(msg, protocol_version->spec_version_hash); + mavlink_msg_protocol_version_get_library_version_hash(msg, protocol_version->library_version_hash); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN? msg->len : MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN; + memset(protocol_version, 0, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); + memcpy(protocol_version, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/minimal/minimal.h b/lib/main/MAVLink/minimal/minimal.h new file mode 100644 index 0000000000..755eeba888 --- /dev/null +++ b/lib/main/MAVLink/minimal/minimal.h @@ -0,0 +1,333 @@ +/** @file + * @brief MAVLink comm protocol generated from minimal.xml + * @see http://mavlink.org + */ +#pragma once +#ifndef MAVLINK_MINIMAL_H +#define MAVLINK_MINIMAL_H + +#ifndef MAVLINK_H + #error Wrong include order: MAVLINK_MINIMAL.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call. +#endif + +#undef MAVLINK_THIS_XML_IDX +#define MAVLINK_THIS_XML_IDX 1 + +#ifdef __cplusplus +extern "C" { +#endif + +// MESSAGE LENGTHS AND CRCS + +#ifndef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS {} +#endif + +#ifndef MAVLINK_MESSAGE_CRCS +#define MAVLINK_MESSAGE_CRCS {{0, 50, 9, 9, 0, 0, 0}, {300, 217, 22, 22, 0, 0, 0}} +#endif + +#include "../protocol.h" + +#define MAVLINK_ENABLED_MINIMAL + +// ENUM DEFINITIONS + + +/** @brief Micro air vehicle / autopilot classes. This identifies the individual model. */ +#ifndef HAVE_ENUM_MAV_AUTOPILOT +#define HAVE_ENUM_MAV_AUTOPILOT +typedef enum MAV_AUTOPILOT +{ + MAV_AUTOPILOT_GENERIC=0, /* Generic autopilot, full support for everything | */ + MAV_AUTOPILOT_RESERVED=1, /* Reserved for future use. | */ + MAV_AUTOPILOT_SLUGS=2, /* SLUGS autopilot, http://slugsuav.soe.ucsc.edu | */ + MAV_AUTOPILOT_ARDUPILOTMEGA=3, /* ArduPilot - Plane/Copter/Rover/Sub/Tracker, https://ardupilot.org | */ + MAV_AUTOPILOT_OPENPILOT=4, /* OpenPilot, http://openpilot.org | */ + MAV_AUTOPILOT_GENERIC_WAYPOINTS_ONLY=5, /* Generic autopilot only supporting simple waypoints | */ + MAV_AUTOPILOT_GENERIC_WAYPOINTS_AND_SIMPLE_NAVIGATION_ONLY=6, /* Generic autopilot supporting waypoints and other simple navigation commands | */ + MAV_AUTOPILOT_GENERIC_MISSION_FULL=7, /* Generic autopilot supporting the full mission command set | */ + MAV_AUTOPILOT_INVALID=8, /* No valid autopilot, e.g. a GCS or other MAVLink component | */ + MAV_AUTOPILOT_PPZ=9, /* PPZ UAV - http://nongnu.org/paparazzi | */ + MAV_AUTOPILOT_UDB=10, /* UAV Dev Board | */ + MAV_AUTOPILOT_FP=11, /* FlexiPilot | */ + MAV_AUTOPILOT_PX4=12, /* PX4 Autopilot - http://px4.io/ | */ + MAV_AUTOPILOT_SMACCMPILOT=13, /* SMACCMPilot - http://smaccmpilot.org | */ + MAV_AUTOPILOT_AUTOQUAD=14, /* AutoQuad -- http://autoquad.org | */ + MAV_AUTOPILOT_ARMAZILA=15, /* Armazila -- http://armazila.com | */ + MAV_AUTOPILOT_AEROB=16, /* Aerob -- http://aerob.ru | */ + MAV_AUTOPILOT_ASLUAV=17, /* ASLUAV autopilot -- http://www.asl.ethz.ch | */ + MAV_AUTOPILOT_SMARTAP=18, /* SmartAP Autopilot - http://sky-drones.com | */ + MAV_AUTOPILOT_AIRRAILS=19, /* AirRails - http://uaventure.com | */ + MAV_AUTOPILOT_ENUM_END=20, /* | */ +} MAV_AUTOPILOT; +#endif + +/** @brief MAVLINK component type reported in HEARTBEAT message. Flight controllers must report the type of the vehicle on which they are mounted (e.g. MAV_TYPE_OCTOROTOR). All other components must report a value appropriate for their type (e.g. a camera must use MAV_TYPE_CAMERA). */ +#ifndef HAVE_ENUM_MAV_TYPE +#define HAVE_ENUM_MAV_TYPE +typedef enum MAV_TYPE +{ + MAV_TYPE_GENERIC=0, /* Generic micro air vehicle | */ + MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ + MAV_TYPE_QUADROTOR=2, /* Quadrotor | */ + MAV_TYPE_COAXIAL=3, /* Coaxial helicopter | */ + MAV_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ + MAV_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */ + MAV_TYPE_GCS=6, /* Operator control unit / ground control station | */ + MAV_TYPE_AIRSHIP=7, /* Airship, controlled | */ + MAV_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ + MAV_TYPE_ROCKET=9, /* Rocket | */ + MAV_TYPE_GROUND_ROVER=10, /* Ground rover | */ + MAV_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ + MAV_TYPE_SUBMARINE=12, /* Submarine | */ + MAV_TYPE_HEXAROTOR=13, /* Hexarotor | */ + MAV_TYPE_OCTOROTOR=14, /* Octorotor | */ + MAV_TYPE_TRICOPTER=15, /* Tricopter | */ + MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */ + MAV_TYPE_KITE=17, /* Kite | */ + MAV_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */ + MAV_TYPE_VTOL_DUOROTOR=19, /* Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter. | */ + MAV_TYPE_VTOL_QUADROTOR=20, /* Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter. | */ + MAV_TYPE_VTOL_TILTROTOR=21, /* Tiltrotor VTOL | */ + MAV_TYPE_VTOL_RESERVED2=22, /* VTOL reserved 2 | */ + MAV_TYPE_VTOL_RESERVED3=23, /* VTOL reserved 3 | */ + MAV_TYPE_VTOL_RESERVED4=24, /* VTOL reserved 4 | */ + MAV_TYPE_VTOL_RESERVED5=25, /* VTOL reserved 5 | */ + MAV_TYPE_GIMBAL=26, /* Gimbal | */ + MAV_TYPE_ADSB=27, /* ADSB system | */ + MAV_TYPE_PARAFOIL=28, /* Steerable, nonrigid airfoil | */ + MAV_TYPE_DODECAROTOR=29, /* Dodecarotor | */ + MAV_TYPE_CAMERA=30, /* Camera | */ + MAV_TYPE_CHARGING_STATION=31, /* Charging station | */ + MAV_TYPE_FLARM=32, /* FLARM collision avoidance system | */ + MAV_TYPE_SERVO=33, /* Servo | */ + MAV_TYPE_ODID=34, /* Open Drone ID. See https://mavlink.io/en/services/opendroneid.html. | */ + MAV_TYPE_DECAROTOR=35, /* Decarotor | */ + MAV_TYPE_ENUM_END=36, /* | */ +} MAV_TYPE; +#endif + +/** @brief These flags encode the MAV mode. */ +#ifndef HAVE_ENUM_MAV_MODE_FLAG +#define HAVE_ENUM_MAV_MODE_FLAG +typedef enum MAV_MODE_FLAG +{ + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED=1, /* 0b00000001 Reserved for future use. | */ + MAV_MODE_FLAG_TEST_ENABLED=2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ + MAV_MODE_FLAG_AUTO_ENABLED=4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ + MAV_MODE_FLAG_GUIDED_ENABLED=8, /* 0b00001000 guided mode enabled, system flies waypoints / mission items. | */ + MAV_MODE_FLAG_STABILIZE_ENABLED=16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ + MAV_MODE_FLAG_HIL_ENABLED=32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ + MAV_MODE_FLAG_MANUAL_INPUT_ENABLED=64, /* 0b01000000 remote control input is enabled. | */ + MAV_MODE_FLAG_SAFETY_ARMED=128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */ + MAV_MODE_FLAG_ENUM_END=129, /* | */ +} MAV_MODE_FLAG; +#endif + +/** @brief These values encode the bit positions of the decode position. These values can be used to read the value of a flag bit by combining the base_mode variable with AND with the flag position value. The result will be either 0 or 1, depending on if the flag is set or not. */ +#ifndef HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION +#define HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION +typedef enum MAV_MODE_FLAG_DECODE_POSITION +{ + MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE=1, /* Eighth bit: 00000001 | */ + MAV_MODE_FLAG_DECODE_POSITION_TEST=2, /* Seventh bit: 00000010 | */ + MAV_MODE_FLAG_DECODE_POSITION_AUTO=4, /* Sixth bit: 00000100 | */ + MAV_MODE_FLAG_DECODE_POSITION_GUIDED=8, /* Fifth bit: 00001000 | */ + MAV_MODE_FLAG_DECODE_POSITION_STABILIZE=16, /* Fourth bit: 00010000 | */ + MAV_MODE_FLAG_DECODE_POSITION_HIL=32, /* Third bit: 00100000 | */ + MAV_MODE_FLAG_DECODE_POSITION_MANUAL=64, /* Second bit: 01000000 | */ + MAV_MODE_FLAG_DECODE_POSITION_SAFETY=128, /* First bit: 10000000 | */ + MAV_MODE_FLAG_DECODE_POSITION_ENUM_END=129, /* | */ +} MAV_MODE_FLAG_DECODE_POSITION; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_STATE +#define HAVE_ENUM_MAV_STATE +typedef enum MAV_STATE +{ + MAV_STATE_UNINIT=0, /* Uninitialized system, state is unknown. | */ + MAV_STATE_BOOT=1, /* System is booting up. | */ + MAV_STATE_CALIBRATING=2, /* System is calibrating and not flight-ready. | */ + MAV_STATE_STANDBY=3, /* System is grounded and on standby. It can be launched any time. | */ + MAV_STATE_ACTIVE=4, /* System is active and might be already airborne. Motors are engaged. | */ + MAV_STATE_CRITICAL=5, /* System is in a non-normal flight mode. It can however still navigate. | */ + MAV_STATE_EMERGENCY=6, /* System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. | */ + MAV_STATE_POWEROFF=7, /* System just initialized its power-down sequence, will shut down now. | */ + MAV_STATE_FLIGHT_TERMINATION=8, /* System is terminating itself. | */ + MAV_STATE_ENUM_END=9, /* | */ +} MAV_STATE; +#endif + +/** @brief Component ids (values) for the different types and instances of onboard hardware/software that might make up a MAVLink system (autopilot, cameras, servos, GPS systems, avoidance systems etc.). + Components must use the appropriate ID in their source address when sending messages. Components can also use IDs to determine if they are the intended recipient of an incoming message. The MAV_COMP_ID_ALL value is used to indicate messages that must be processed by all components. + When creating new entries, components that can have multiple instances (e.g. cameras, servos etc.) should be allocated sequential values. An appropriate number of values should be left free after these components to allow the number of instances to be expanded. */ +#ifndef HAVE_ENUM_MAV_COMPONENT +#define HAVE_ENUM_MAV_COMPONENT +typedef enum MAV_COMPONENT +{ + MAV_COMP_ID_ALL=0, /* Target id (target_component) used to broadcast messages to all components of the receiving system. Components should attempt to process messages with this component ID and forward to components on any other interfaces. Note: This is not a valid *source* component id for a message. | */ + MAV_COMP_ID_AUTOPILOT1=1, /* System flight controller component ("autopilot"). Only one autopilot is expected in a particular system. | */ + MAV_COMP_ID_USER1=25, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER2=26, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER3=27, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER4=28, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER5=29, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER6=30, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER7=31, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER8=32, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER9=33, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER10=34, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER11=35, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER12=36, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER13=37, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER14=38, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER15=39, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER16=40, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER17=41, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER18=42, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER19=43, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER20=44, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER21=45, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER22=46, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER23=47, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER24=48, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER25=49, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER26=50, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER27=51, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER28=52, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER29=53, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER30=54, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER31=55, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER32=56, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER33=57, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER34=58, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER35=59, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER36=60, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER37=61, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER38=62, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER39=63, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER40=64, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER41=65, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER42=66, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER43=67, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_TELEMETRY_RADIO=68, /* Telemetry radio (e.g. SiK radio, or other component that emits RADIO_STATUS messages). | */ + MAV_COMP_ID_USER45=69, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER46=70, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER47=71, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER48=72, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER49=73, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER50=74, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER51=75, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER52=76, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER53=77, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER54=78, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER55=79, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER56=80, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER57=81, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER58=82, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER59=83, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER60=84, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER61=85, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER62=86, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER63=87, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER64=88, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER65=89, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER66=90, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER67=91, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER68=92, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER69=93, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER70=94, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER71=95, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER72=96, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER73=97, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER74=98, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER75=99, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_CAMERA=100, /* Camera #1. | */ + MAV_COMP_ID_CAMERA2=101, /* Camera #2. | */ + MAV_COMP_ID_CAMERA3=102, /* Camera #3. | */ + MAV_COMP_ID_CAMERA4=103, /* Camera #4. | */ + MAV_COMP_ID_CAMERA5=104, /* Camera #5. | */ + MAV_COMP_ID_CAMERA6=105, /* Camera #6. | */ + MAV_COMP_ID_SERVO1=140, /* Servo #1. | */ + MAV_COMP_ID_SERVO2=141, /* Servo #2. | */ + MAV_COMP_ID_SERVO3=142, /* Servo #3. | */ + MAV_COMP_ID_SERVO4=143, /* Servo #4. | */ + MAV_COMP_ID_SERVO5=144, /* Servo #5. | */ + MAV_COMP_ID_SERVO6=145, /* Servo #6. | */ + MAV_COMP_ID_SERVO7=146, /* Servo #7. | */ + MAV_COMP_ID_SERVO8=147, /* Servo #8. | */ + MAV_COMP_ID_SERVO9=148, /* Servo #9. | */ + MAV_COMP_ID_SERVO10=149, /* Servo #10. | */ + MAV_COMP_ID_SERVO11=150, /* Servo #11. | */ + MAV_COMP_ID_SERVO12=151, /* Servo #12. | */ + MAV_COMP_ID_SERVO13=152, /* Servo #13. | */ + MAV_COMP_ID_SERVO14=153, /* Servo #14. | */ + MAV_COMP_ID_GIMBAL=154, /* Gimbal #1. | */ + MAV_COMP_ID_LOG=155, /* Logging component. | */ + MAV_COMP_ID_ADSB=156, /* Automatic Dependent Surveillance-Broadcast (ADS-B) component. | */ + MAV_COMP_ID_OSD=157, /* On Screen Display (OSD) devices for video links. | */ + MAV_COMP_ID_PERIPHERAL=158, /* Generic autopilot peripheral component ID. Meant for devices that do not implement the parameter microservice. | */ + MAV_COMP_ID_QX1_GIMBAL=159, /* Gimbal ID for QX1. | */ + MAV_COMP_ID_FLARM=160, /* FLARM collision alert component. | */ + MAV_COMP_ID_GIMBAL2=171, /* Gimbal #2. | */ + MAV_COMP_ID_GIMBAL3=172, /* Gimbal #3. | */ + MAV_COMP_ID_GIMBAL4=173, /* Gimbal #4 | */ + MAV_COMP_ID_GIMBAL5=174, /* Gimbal #5. | */ + MAV_COMP_ID_GIMBAL6=175, /* Gimbal #6. | */ + MAV_COMP_ID_MISSIONPLANNER=190, /* Component that can generate/supply a mission flight plan (e.g. GCS or developer API). | */ + MAV_COMP_ID_ONBOARD_COMPUTER=191, /* Component that lives on the onboard computer (companion computer) and has some generic functionalities, such as settings system parameters and monitoring the status of some processes that don't directly speak mavlink and so on. | */ + MAV_COMP_ID_PATHPLANNER=195, /* Component that finds an optimal path between points based on a certain constraint (e.g. minimum snap, shortest path, cost, etc.). | */ + MAV_COMP_ID_OBSTACLE_AVOIDANCE=196, /* Component that plans a collision free path between two points. | */ + MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY=197, /* Component that provides position estimates using VIO techniques. | */ + MAV_COMP_ID_PAIRING_MANAGER=198, /* Component that manages pairing of vehicle and GCS. | */ + MAV_COMP_ID_IMU=200, /* Inertial Measurement Unit (IMU) #1. | */ + MAV_COMP_ID_IMU_2=201, /* Inertial Measurement Unit (IMU) #2. | */ + MAV_COMP_ID_IMU_3=202, /* Inertial Measurement Unit (IMU) #3. | */ + MAV_COMP_ID_GPS=220, /* GPS #1. | */ + MAV_COMP_ID_GPS2=221, /* GPS #2. | */ + MAV_COMP_ID_ODID_TXRX_1=236, /* Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). | */ + MAV_COMP_ID_ODID_TXRX_2=237, /* Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). | */ + MAV_COMP_ID_ODID_TXRX_3=238, /* Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). | */ + MAV_COMP_ID_UDP_BRIDGE=240, /* Component to bridge MAVLink to UDP (i.e. from a UART). | */ + MAV_COMP_ID_UART_BRIDGE=241, /* Component to bridge to UART (i.e. from UDP). | */ + MAV_COMP_ID_TUNNEL_NODE=242, /* Component handling TUNNEL messages (e.g. vendor specific GUI of a component). | */ + MAV_COMP_ID_SYSTEM_CONTROL=250, /* Component for handling system messages (e.g. to ARM, takeoff, etc.). | */ + MAV_COMPONENT_ENUM_END=251, /* | */ +} MAV_COMPONENT; +#endif + +// MAVLINK VERSION + +#ifndef MAVLINK_VERSION +#define MAVLINK_VERSION 3 +#endif + +#if (MAVLINK_VERSION == 0) +#undef MAVLINK_VERSION +#define MAVLINK_VERSION 3 +#endif + +// MESSAGE DEFINITIONS +#include "./mavlink_msg_heartbeat.h" +#include "./mavlink_msg_protocol_version.h" + +// base include + + +#undef MAVLINK_THIS_XML_IDX +#define MAVLINK_THIS_XML_IDX 1 + +#if MAVLINK_THIS_XML_IDX == MAVLINK_PRIMARY_XML_IDX +# define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_PROTOCOL_VERSION} +# define MAVLINK_MESSAGE_NAMES {{ "HEARTBEAT", 0 }, { "PROTOCOL_VERSION", 300 }} +# if MAVLINK_COMMAND_24BIT +# include "../mavlink_get_info.h" +# endif +#endif + +#ifdef __cplusplus +} +#endif // __cplusplus +#endif // MAVLINK_MINIMAL_H diff --git a/lib/main/MAVLink/minimal/testsuite.h b/lib/main/MAVLink/minimal/testsuite.h new file mode 100644 index 0000000000..fd9c37f414 --- /dev/null +++ b/lib/main/MAVLink/minimal/testsuite.h @@ -0,0 +1,154 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from minimal.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#pragma once +#ifndef MINIMAL_TESTSUITE_H +#define MINIMAL_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAVLINK_TEST_ALL +#define MAVLINK_TEST_ALL + +static void mavlink_test_minimal(uint8_t, uint8_t, mavlink_message_t *last_msg); + +static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + + mavlink_test_minimal(system_id, component_id, last_msg); +} +#endif + + + + +static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0); + if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_HEARTBEAT >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_heartbeat_t packet_in = { + 963497464,17,84,151,218,3 + }; + mavlink_heartbeat_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.custom_mode = packet_in.custom_mode; + packet1.type = packet_in.type; + packet1.autopilot = packet_in.autopilot; + packet1.base_mode = packet_in.base_mode; + packet1.system_status = packet_in.system_status; + packet1.mavlink_version = packet_in.mavlink_version; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_heartbeat_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_heartbeat_pack(system_id, component_id, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PROTOCOL_VERSION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_protocol_version_t packet_in = { + 17235,17339,17443,{ 151, 152, 153, 154, 155, 156, 157, 158 },{ 175, 176, 177, 178, 179, 180, 181, 182 } + }; + mavlink_protocol_version_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.version = packet_in.version; + packet1.min_version = packet_in.min_version; + packet1.max_version = packet_in.max_version; + + mav_array_memcpy(packet1.spec_version_hash, packet_in.spec_version_hash, sizeof(uint8_t)*8); + mav_array_memcpy(packet1.library_version_hash, packet_in.library_version_hash, sizeof(uint8_t)*8); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_protocol_version_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_protocol_version_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_protocol_version_pack(system_id, component_id, &msg , packet1.version , packet1.min_version , packet1.max_version , packet1.spec_version_hash , packet1.library_version_hash ); + mavlink_msg_protocol_version_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_protocol_version_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.version , packet1.min_version , packet1.max_version , packet1.spec_version_hash , packet1.library_version_hash ); + mavlink_msg_protocol_version_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; ilen + MAVLINK_NUM_NON_PAYLOAD_BYTES; + if (msg->magic == MAVLINK_STX_MAVLINK1) { + return msg->len + MAVLINK_CORE_HEADER_MAVLINK1_LEN+1 + 2; + } + uint16_t signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0; + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES + signature_len; } #if MAVLINK_NEED_BYTE_SWAP -static inline void byte_swap_2(char *dst, const char *src) +static void byte_swap_2(char *dst, const char *src) { dst[0] = src[1]; dst[1] = src[0]; } -static inline void byte_swap_4(char *dst, const char *src) +static void byte_swap_4(char *dst, const char *src) { dst[0] = src[3]; dst[1] = src[2]; dst[2] = src[1]; dst[3] = src[0]; } -static inline void byte_swap_8(char *dst, const char *src) +static void byte_swap_8(char *dst, const char *src) { dst[0] = src[7]; dst[1] = src[6]; @@ -119,19 +114,19 @@ static inline void byte_swap_8(char *dst, const char *src) dst[7] = src[0]; } #elif !MAVLINK_ALIGNED_FIELDS -static inline void byte_copy_2(char *dst, const char *src) +static void byte_copy_2(char *dst, const char *src) { dst[0] = src[0]; dst[1] = src[1]; } -static inline void byte_copy_4(char *dst, const char *src) +static void byte_copy_4(char *dst, const char *src) { dst[0] = src[0]; dst[1] = src[1]; dst[2] = src[2]; dst[3] = src[3]; } -static inline void byte_copy_8(char *dst, const char *src) +static void byte_copy_8(char *dst, const char *src) { memcpy(dst, src, 8); } @@ -173,7 +168,7 @@ static inline void byte_copy_8(char *dst, const char *src) /* like memcpy(), but if src is NULL, do a memset to zero */ -static inline void mav_array_memcpy(void *dest, const void *src, size_t n) +static void mav_array_memcpy(void *dest, const void *src, size_t n) { if (src == NULL) { memset(dest, 0, n); @@ -185,7 +180,7 @@ static inline void mav_array_memcpy(void *dest, const void *src, size_t n) /* * Place a char array into a buffer */ -static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length) +static void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length) { mav_array_memcpy(&buf[wire_offset], b, array_length); @@ -194,7 +189,7 @@ static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const cha /* * Place a uint8_t array into a buffer */ -static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length) +static void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length) { mav_array_memcpy(&buf[wire_offset], b, array_length); @@ -203,7 +198,7 @@ static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const /* * Place a int8_t array into a buffer */ -static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length) +static void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length) { mav_array_memcpy(&buf[wire_offset], b, array_length); @@ -211,7 +206,7 @@ static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const i #if MAVLINK_NEED_BYTE_SWAP #define _MAV_PUT_ARRAY(TYPE, V) \ -static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \ +static void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \ { \ if (b == NULL) { \ memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \ @@ -224,7 +219,7 @@ static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, co } #else #define _MAV_PUT_ARRAY(TYPE, V) \ -static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \ +static void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \ { \ mav_array_memcpy(&buf[wire_offset], b, array_length*sizeof(TYPE)); \ } @@ -245,7 +240,7 @@ _MAV_PUT_ARRAY(double, d) #if MAVLINK_NEED_BYTE_SWAP #define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \ -static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \ +static TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \ { TYPE r; byte_swap_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; } _MAV_MSG_RETURN_TYPE(uint16_t, 2) @@ -259,7 +254,7 @@ _MAV_MSG_RETURN_TYPE(double, 8) #elif !MAVLINK_ALIGNED_FIELDS #define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \ -static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \ +static TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \ { TYPE r; byte_copy_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; } _MAV_MSG_RETURN_TYPE(uint16_t, 2) @@ -272,7 +267,7 @@ _MAV_MSG_RETURN_TYPE(float, 4) _MAV_MSG_RETURN_TYPE(double, 8) #else // nicely aligned, no swap #define _MAV_MSG_RETURN_TYPE(TYPE) \ -static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \ +static TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \ { return *(const TYPE *)(&_MAV_PAYLOAD(msg)[ofs]);} _MAV_MSG_RETURN_TYPE(uint16_t) @@ -285,21 +280,21 @@ _MAV_MSG_RETURN_TYPE(float) _MAV_MSG_RETURN_TYPE(double) #endif // MAVLINK_NEED_BYTE_SWAP -static inline uint16_t _MAV_RETURN_char_array(const mavlink_message_t *msg, char *value, +static uint16_t _MAV_RETURN_char_array(const mavlink_message_t *msg, char *value, uint8_t array_length, uint8_t wire_offset) { memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length); return array_length; } -static inline uint16_t _MAV_RETURN_uint8_t_array(const mavlink_message_t *msg, uint8_t *value, +static uint16_t _MAV_RETURN_uint8_t_array(const mavlink_message_t *msg, uint8_t *value, uint8_t array_length, uint8_t wire_offset) { memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length); return array_length; } -static inline uint16_t _MAV_RETURN_int8_t_array(const mavlink_message_t *msg, int8_t *value, +static uint16_t _MAV_RETURN_int8_t_array(const mavlink_message_t *msg, int8_t *value, uint8_t array_length, uint8_t wire_offset) { memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length); @@ -308,7 +303,7 @@ static inline uint16_t _MAV_RETURN_int8_t_array(const mavlink_message_t *msg, in #if MAVLINK_NEED_BYTE_SWAP #define _MAV_RETURN_ARRAY(TYPE, V) \ -static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \ +static uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \ uint8_t array_length, uint8_t wire_offset) \ { \ uint16_t i; \ @@ -319,7 +314,7 @@ static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg } #else #define _MAV_RETURN_ARRAY(TYPE, V) \ -static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \ +static uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \ uint8_t array_length, uint8_t wire_offset) \ { \ memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length*sizeof(TYPE)); \ @@ -336,4 +331,4 @@ _MAV_RETURN_ARRAY(int64_t, i64) _MAV_RETURN_ARRAY(float, f) _MAV_RETURN_ARRAY(double, d) -#endif // _MAVLINK_PROTOCOL_H_ + diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 6e2d28c864..28453ab2df 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -105,6 +105,8 @@ main_sources(COMMON_SRC drivers/accgyro/accgyro_mpu9250.h drivers/accgyro/accgyro_bno055.c drivers/accgyro/accgyro_bno055.h + drivers/accgyro/accgyro_bno055_serial.c + drivers/accgyro/accgyro_bno055_serial.h drivers/adc.c drivers/adc.h @@ -318,6 +320,8 @@ main_sources(COMMON_SRC flight/imu.h flight/kalman.c flight/kalman.h + flight/smith_predictor.c + flight/smith_predictor.h flight/mixer.c flight/mixer.h flight/pid.c @@ -398,6 +402,8 @@ main_sources(COMMON_SRC rx/ibus.h rx/jetiexbus.c rx/jetiexbus.h + rx/mavlink.c + rx/mavlink.h rx/msp.c rx/msp.h rx/msp_override.c diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 921f4ff734..84394206d5 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -83,6 +83,7 @@ typedef enum { DEBUG_FW_D, DEBUG_IMU2, DEBUG_ALTITUDE, - DEBUG_AUTOTRIM, + DEBUG_SMITH_COMPENSATOR, + DEBUG_AUTOTRIM DEBUG_COUNT } debugType_e; diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index d17d3df290..ff9d246ee2 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -72,15 +72,17 @@ static const CMS_Menu cmsx_menuNavSettings = { OSD_SETTING_ENTRY("RTH ALT MODE", SETTING_NAV_RTH_ALT_MODE), OSD_SETTING_ENTRY("RTH ALT", SETTING_NAV_RTH_ALTITUDE), - OSD_SETTING_ENTRY("CLIMB BEFORE RTH", SETTING_NAV_RTH_CLIMB_FIRST), + OSD_SETTING_ENTRY("CLIMB FIRST", SETTING_NAV_RTH_CLIMB_FIRST), OSD_SETTING_ENTRY("TAIL FIRST", SETTING_NAV_RTH_TAIL_FIRST), OSD_SETTING_ENTRY("LAND AFTER RTH", SETTING_NAV_RTH_ALLOW_LANDING), - OSD_SETTING_ENTRY("LAND VERT SPEED", SETTING_NAV_LANDING_SPEED), + OSD_SETTING_ENTRY("LAND MINALT VSPD", SETTING_NAV_LAND_MINALT_VSPD), + OSD_SETTING_ENTRY("LAND MAXALT VSPD", SETTING_NAV_LAND_MAXALT_VSPD), OSD_SETTING_ENTRY("LAND SPEED MIN AT", SETTING_NAV_LAND_SLOWDOWN_MINALT), OSD_SETTING_ENTRY("LAND SPEED SLOW AT", SETTING_NAV_LAND_SLOWDOWN_MAXALT), OSD_SETTING_ENTRY("MIN RTH DISTANCE", SETTING_NAV_MIN_RTH_DISTANCE), OSD_SETTING_ENTRY("RTH ABORT THRES", SETTING_NAV_RTH_ABORT_THRESHOLD), OSD_SETTING_ENTRY("EMERG LANDING SPEED", SETTING_NAV_EMERG_LANDING_SPEED), + OSD_SETTING_ENTRY("SAFEHOME USAGE MODE", SETTING_SAFEHOME_USAGE_MODE), OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/common/time.h b/src/main/common/time.h index bef2f6b6a2..ca8f4b6107 100644 --- a/src/main/common/time.h +++ b/src/main/common/time.h @@ -24,10 +24,19 @@ #include "config/parameter_group.h" -// time difference, 32 bits always sufficient +// time difference, signed 32 bits of microseconds overflows at ~35 minutes +// this is worth leaving as int32_t for performance reasons, use timeDeltaLarge_t for deltas that can be big typedef int32_t timeDelta_t; +#define TIMEDELTA_MAX INT32_MAX + +// time difference large, signed 64 bits of microseconds overflows at ~300000 years +typedef int64_t timeDeltaLarge_t; +#define TIMEDELTALARGE_MAX INT64_MAX + // millisecond time -typedef uint32_t timeMs_t ; +typedef uint32_t timeMs_t; +#define TIMEMS_MAX UINT32_MAX + // microsecond time #ifdef USE_64BIT_TIME typedef uint64_t timeUs_t; @@ -48,6 +57,7 @@ typedef uint32_t timeUs_t; #define MS2S(ms) ((ms) * 1e-3f) #define HZ2S(hz) US2S(HZ2US(hz)) +// Use this function only to get small deltas (difference overflows at ~35 minutes) static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); } typedef enum { diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c index fdb73b7250..90dc96c02e 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.c +++ b/src/main/drivers/accgyro/accgyro_bno055.c @@ -32,47 +32,6 @@ #ifdef USE_IMU_BNO055 -#define BNO055_ADDR_PWR_MODE 0x3E -#define BNO055_ADDR_OPR_MODE 0x3D -#define BNO055_ADDR_CALIB_STAT 0x35 - -#define BNO055_PWR_MODE_NORMAL 0x00 -#define BNO055_OPR_MODE_CONFIG 0x00 -#define BNO055_OPR_MODE_NDOF 0x0C - -#define BNO055_ADDR_EUL_YAW_LSB 0x1A -#define BNO055_ADDR_EUL_YAW_MSB 0x1B -#define BNO055_ADDR_EUL_ROLL_LSB 0x1C -#define BNO055_ADDR_EUL_ROLL_MSB 0x1D -#define BNO055_ADDR_EUL_PITCH_LSB 0x1E -#define BNO055_ADDR_EUL_PITCH_MSB 0x1F - -#define BNO055_ADDR_MAG_RADIUS_MSB 0x6A -#define BNO055_ADDR_MAG_RADIUS_LSB 0x69 -#define BNO055_ADDR_ACC_RADIUS_MSB 0x68 -#define BNO055_ADDR_ACC_RADIUS_LSB 0x67 - -#define BNO055_ADDR_GYR_OFFSET_Z_MSB 0x66 -#define BNO055_ADDR_GYR_OFFSET_Z_LSB 0x65 -#define BNO055_ADDR_GYR_OFFSET_Y_MSB 0x64 -#define BNO055_ADDR_GYR_OFFSET_Y_LSB 0x63 -#define BNO055_ADDR_GYR_OFFSET_X_MSB 0x62 -#define BNO055_ADDR_GYR_OFFSET_X_LSB 0x61 - -#define BNO055_ADDR_MAG_OFFSET_Z_MSB 0x60 -#define BNO055_ADDR_MAG_OFFSET_Z_LSB 0x5F -#define BNO055_ADDR_MAG_OFFSET_Y_MSB 0x5E -#define BNO055_ADDR_MAG_OFFSET_Y_LSB 0x5D -#define BNO055_ADDR_MAG_OFFSET_X_MSB 0x5C -#define BNO055_ADDR_MAG_OFFSET_X_LSB 0x5B - -#define BNO055_ADDR_ACC_OFFSET_Z_MSB 0x5A -#define BNO055_ADDR_ACC_OFFSET_Z_LSB 0x59 -#define BNO055_ADDR_ACC_OFFSET_Y_MSB 0x58 -#define BNO055_ADDR_ACC_OFFSET_Y_LSB 0x57 -#define BNO055_ADDR_ACC_OFFSET_X_MSB 0x56 -#define BNO055_ADDR_ACC_OFFSET_X_LSB 0x55 - static busDevice_t *busDev; static bool deviceDetect(busDevice_t *busDev) @@ -99,18 +58,45 @@ static void bno055SetMode(uint8_t mode) delay(25); } +static void bno055SetCalibrationData(bno055CalibrationData_t data) +{ + uint8_t buf[12]; + + //Prepare gains + //We do not restore gyro offsets, they are quickly calibrated at startup + uint8_t bufferBit = 0; + for (uint8_t sensorIndex = 0; sensorIndex < 2; sensorIndex++) + { + for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++) + { + buf[bufferBit] = (uint8_t)(data.offset[sensorIndex][axisIndex] & 0xff); + buf[bufferBit + 1] = (uint8_t)((data.offset[sensorIndex][axisIndex] >> 8 ) & 0xff); + bufferBit += 2; + } + } + + busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12); + + //Prepare radius + buf[0] = (uint8_t)(data.radius[ACC] & 0xff); + buf[1] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff); + buf[2] = (uint8_t)(data.radius[MAG] & 0xff); + buf[3] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff); + + //Write to the device + busWriteBuf(busDev, BNO055_ADDR_ACC_RADIUS_LSB, buf, 4); +} + bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration) { busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_BNO055, 0, 0); if (busDev == NULL) { - DEBUG_SET(DEBUG_IMU2, 2, 1); return false; } if (!deviceDetect(busDev)) { - DEBUG_SET(DEBUG_IMU2, 2, 2); busDeviceDeInit(busDev); return false; } @@ -192,33 +178,4 @@ bno055CalibrationData_t bno055GetCalibrationData(void) return data; } -void bno055SetCalibrationData(bno055CalibrationData_t data) -{ - uint8_t buf[12]; - - //Prepare gains - //We do not restore gyro offsets, they are quickly calibrated at startup - uint8_t bufferBit = 0; - for (uint8_t sensorIndex = 0; sensorIndex < 2; sensorIndex++) - { - for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++) - { - buf[bufferBit] = (uint8_t)(data.offset[sensorIndex][axisIndex] & 0xff); - buf[bufferBit + 1] = (uint8_t)((data.offset[sensorIndex][axisIndex] >> 8 ) & 0xff); - bufferBit += 2; - } - } - - busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12); - - //Prepare radius - buf[0] = (uint8_t)(data.radius[ACC] & 0xff); - buf[1] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff); - buf[2] = (uint8_t)(data.radius[MAG] & 0xff); - buf[3] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff); - - //Write to the device - busWriteBuf(busDev, BNO055_ADDR_ACC_RADIUS_LSB, buf, 4); -} - #endif \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h index d0c24f6dc4..de0d751d7a 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.h +++ b/src/main/drivers/accgyro/accgyro_bno055.h @@ -25,6 +25,47 @@ #include "common/vector.h" +#define BNO055_ADDR_PWR_MODE 0x3E +#define BNO055_ADDR_OPR_MODE 0x3D +#define BNO055_ADDR_CALIB_STAT 0x35 + +#define BNO055_PWR_MODE_NORMAL 0x00 +#define BNO055_OPR_MODE_CONFIG 0x00 +#define BNO055_OPR_MODE_NDOF 0x0C + +#define BNO055_ADDR_EUL_YAW_LSB 0x1A +#define BNO055_ADDR_EUL_YAW_MSB 0x1B +#define BNO055_ADDR_EUL_ROLL_LSB 0x1C +#define BNO055_ADDR_EUL_ROLL_MSB 0x1D +#define BNO055_ADDR_EUL_PITCH_LSB 0x1E +#define BNO055_ADDR_EUL_PITCH_MSB 0x1F + +#define BNO055_ADDR_MAG_RADIUS_MSB 0x6A +#define BNO055_ADDR_MAG_RADIUS_LSB 0x69 +#define BNO055_ADDR_ACC_RADIUS_MSB 0x68 +#define BNO055_ADDR_ACC_RADIUS_LSB 0x67 + +#define BNO055_ADDR_GYR_OFFSET_Z_MSB 0x66 +#define BNO055_ADDR_GYR_OFFSET_Z_LSB 0x65 +#define BNO055_ADDR_GYR_OFFSET_Y_MSB 0x64 +#define BNO055_ADDR_GYR_OFFSET_Y_LSB 0x63 +#define BNO055_ADDR_GYR_OFFSET_X_MSB 0x62 +#define BNO055_ADDR_GYR_OFFSET_X_LSB 0x61 + +#define BNO055_ADDR_MAG_OFFSET_Z_MSB 0x60 +#define BNO055_ADDR_MAG_OFFSET_Z_LSB 0x5F +#define BNO055_ADDR_MAG_OFFSET_Y_MSB 0x5E +#define BNO055_ADDR_MAG_OFFSET_Y_LSB 0x5D +#define BNO055_ADDR_MAG_OFFSET_X_MSB 0x5C +#define BNO055_ADDR_MAG_OFFSET_X_LSB 0x5B + +#define BNO055_ADDR_ACC_OFFSET_Z_MSB 0x5A +#define BNO055_ADDR_ACC_OFFSET_Z_LSB 0x59 +#define BNO055_ADDR_ACC_OFFSET_Y_MSB 0x58 +#define BNO055_ADDR_ACC_OFFSET_Y_LSB 0x57 +#define BNO055_ADDR_ACC_OFFSET_X_MSB 0x56 +#define BNO055_ADDR_ACC_OFFSET_X_LSB 0x55 + typedef struct { uint8_t sys; uint8_t gyr; @@ -47,5 +88,4 @@ bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration); fpVector3_t bno055GetEurlerAngles(void); void bno055FetchEulerAngles(int16_t * buffer); bno055CalibStat_t bno055GetCalibStat(void); -bno055CalibrationData_t bno055GetCalibrationData(void); -void bno055SetCalibrationData(bno055CalibrationData_t data); \ No newline at end of file +bno055CalibrationData_t bno055GetCalibrationData(void); \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055_serial.c b/src/main/drivers/accgyro/accgyro_bno055_serial.c new file mode 100644 index 0000000000..9bf2a0725a --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bno055_serial.c @@ -0,0 +1,271 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 http://www.gnu.org/licenses/. + */ +#include "platform.h" +#ifdef USE_IMU_BNO055 + +#define BNO055_BAUD_RATE 115200 +#define BNO055_FRAME_MAX_TIME_MS 10 + +#include +#include +#include "io/serial.h" +#include "drivers/accgyro/accgyro_bno055.h" +#include "build/debug.h" +#include "drivers/time.h" +#include "flight/secondary_imu.h" + +static serialPort_t * bno055SerialPort = NULL; +static uint8_t receiveBuffer[22]; + +typedef enum { + BNO055_RECEIVE_IDLE, + BNO055_RECEIVE_HEADER, + BNO055_RECEIVE_LENGTH, + BNO055_RECEIVE_PAYLOAD, + BNO055_RECEIVE_ACK, +} bno055ReceiveState_e; + +typedef enum { + BNO055_FRAME_ACK, + BNO055_FRAME_DATA, +} bno055FrameType_e; + +typedef enum { + BNO055_DATA_TYPE_NONE, + BNO055_DATA_TYPE_EULER, + BNO055_DATA_TYPE_CALIBRATION_STATS, +} bno055DataType_e; + +static uint8_t bno055ProtocolState = BNO055_RECEIVE_IDLE; +static uint8_t bno055FrameType; +static uint8_t bno055FrameLength; +static uint8_t bno055FrameIndex; +static timeMs_t bno055FrameStartAtMs = 0; +static uint8_t bno055DataType = BNO055_DATA_TYPE_NONE; + + +static void bno055SerialWriteBuffer(const uint8_t reg, const uint8_t *data, const uint8_t count) { + + bno055ProtocolState = BNO055_RECEIVE_IDLE; + serialWrite(bno055SerialPort, 0xAA); // Start Byte + serialWrite(bno055SerialPort, 0x00); // Write command + serialWrite(bno055SerialPort, reg); + serialWrite(bno055SerialPort, count); + for (uint8_t i = 0; i < count; i++) { + serialWrite(bno055SerialPort, data[i]); + } +} + +static void bno055SerialWrite(const uint8_t reg, const uint8_t data) { + uint8_t buff[1]; + buff[0] = data; + + bno055SerialWriteBuffer(reg, buff, 1); +} + +static void bno055SerialRead(const uint8_t reg, const uint8_t len) { + bno055ProtocolState = BNO055_RECEIVE_IDLE; + serialWrite(bno055SerialPort, 0xAA); // Start Byte + serialWrite(bno055SerialPort, 0x01); // Read command + serialWrite(bno055SerialPort, reg); + serialWrite(bno055SerialPort, len); +} + +void bno055SerialDataReceive(uint16_t c, void *data) { + + UNUSED(data); + + const uint8_t incoming = (uint8_t) c; + + //Failsafe for stuck frames + if (bno055ProtocolState != BNO055_RECEIVE_IDLE && millis() - bno055FrameStartAtMs > BNO055_FRAME_MAX_TIME_MS) { + bno055ProtocolState = BNO055_RECEIVE_IDLE; + } + + if (bno055ProtocolState == BNO055_RECEIVE_IDLE && incoming == 0xEE) { + bno055FrameStartAtMs = millis(); + bno055ProtocolState = BNO055_RECEIVE_HEADER; + bno055FrameType = BNO055_FRAME_ACK; + } else if (bno055ProtocolState == BNO055_RECEIVE_IDLE && incoming == 0xBB) { + bno055FrameStartAtMs = millis(); + bno055ProtocolState = BNO055_RECEIVE_HEADER; + bno055FrameType = BNO055_FRAME_DATA; + } else if (bno055ProtocolState == BNO055_RECEIVE_HEADER && bno055FrameType == BNO055_FRAME_ACK) { + receiveBuffer[0] = incoming; + bno055ProtocolState = BNO055_RECEIVE_IDLE; + } else if (bno055ProtocolState == BNO055_RECEIVE_HEADER && bno055FrameType == BNO055_FRAME_DATA) { + bno055FrameLength = incoming; + bno055FrameIndex = 0; + bno055ProtocolState = BNO055_RECEIVE_LENGTH; + } else if (bno055ProtocolState == BNO055_RECEIVE_LENGTH) { + receiveBuffer[bno055FrameIndex] = incoming; + bno055FrameIndex++; + + if (bno055FrameIndex == bno055FrameLength) { + bno055ProtocolState = BNO055_RECEIVE_IDLE; + + if (bno055DataType == BNO055_DATA_TYPE_EULER) { + secondaryImuState.eulerAngles.raw[0] = ((int16_t)((receiveBuffer[3] << 8) | receiveBuffer[2])) / 1.6f; + secondaryImuState.eulerAngles.raw[1] = ((int16_t)((receiveBuffer[5] << 8) | receiveBuffer[4])) / -1.6f; //Pitch has to be reversed to match INAV notation + secondaryImuState.eulerAngles.raw[2] = ((int16_t)((receiveBuffer[1] << 8) | receiveBuffer[0])) / 1.6f; + secondaryImuProcess(); + } else if (bno055DataType == BNO055_DATA_TYPE_CALIBRATION_STATS) { + secondaryImuState.calibrationStatus.mag = receiveBuffer[0] & 0b00000011; + secondaryImuState.calibrationStatus.acc = (receiveBuffer[0] >> 2) & 0b00000011; + secondaryImuState.calibrationStatus.gyr = (receiveBuffer[0] >> 4) & 0b00000011; + secondaryImuState.calibrationStatus.sys = (receiveBuffer[0] >> 6) & 0b00000011; + } + + bno055DataType = BNO055_DATA_TYPE_NONE; + } + } + +} + +static void bno055SerialSetCalibrationData(bno055CalibrationData_t data) +{ + uint8_t buf[12]; + + //Prepare gains + //We do not restore gyro offsets, they are quickly calibrated at startup + uint8_t bufferBit = 0; + for (uint8_t sensorIndex = 0; sensorIndex < 2; sensorIndex++) + { + for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++) + { + buf[bufferBit] = (uint8_t)(data.offset[sensorIndex][axisIndex] & 0xff); + buf[bufferBit + 1] = (uint8_t)((data.offset[sensorIndex][axisIndex] >> 8 ) & 0xff); + bufferBit += 2; + } + } + + bno055SerialWriteBuffer(BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12); + delay(25); + + //Prepare radius + buf[0] = (uint8_t)(data.radius[ACC] & 0xff); + buf[1] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff); + buf[2] = (uint8_t)(data.radius[MAG] & 0xff); + buf[3] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff); + + //Write to the device + bno055SerialWriteBuffer(BNO055_ADDR_ACC_RADIUS_LSB, buf, 4); + delay(25); +} + +bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibration) { + bno055SerialPort = NULL; + + serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_IMU2); + if (!portConfig) { + return false; + } + + bno055SerialPort = openSerialPort( + portConfig->identifier, + FUNCTION_IMU2, + bno055SerialDataReceive, + NULL, + BNO055_BAUD_RATE, + MODE_RXTX, + SERIAL_NOT_INVERTED | SERIAL_UNIDIR | SERIAL_STOPBITS_1 | SERIAL_PARITY_NO + ); + + if (!bno055SerialPort) { + return false; + } + + bno055SerialRead(0x00, 1); // Read ChipID + delay(5); + + //Check ident + if (bno055FrameType != BNO055_FRAME_DATA || receiveBuffer[0] != 0xA0 || bno055ProtocolState != BNO055_RECEIVE_IDLE) { + return false; // Ident does not match, leave + } + + bno055SerialWrite(BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL + delay(25); + + if (setCalibration) { + bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_CONFIG); + delay(25); + + bno055SerialSetCalibrationData(calibrationData); + } + + bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF); + delay(25); + + return true; +} + +/* + * This function is non-blocking and response will be processed by bno055SerialDataReceive + */ +void bno055SerialFetchEulerAngles() { + bno055DataType = BNO055_DATA_TYPE_EULER; + bno055SerialRead(BNO055_ADDR_EUL_YAW_LSB, 6); +} + +/* + * This function is non-blocking and response will be processed by bno055SerialDataReceive + */ +void bno055SerialGetCalibStat(void) { + bno055DataType = BNO055_DATA_TYPE_CALIBRATION_STATS; + bno055SerialRead(BNO055_ADDR_CALIB_STAT, 1); +} + +/* + * This function is blocking and should not be used during flight conditions! + */ +bno055CalibrationData_t bno055SerialGetCalibrationData(void) { + + bno055CalibrationData_t data; + + bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_CONFIG); + delay(25); + + bno055SerialRead(BNO055_ADDR_ACC_OFFSET_X_LSB, 22); + delay(50); + + uint8_t bufferBit = 0; + for (uint8_t sensorIndex = 0; sensorIndex < 3; sensorIndex++) + { + for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++) + { + data.offset[sensorIndex][axisIndex] = (int16_t)((receiveBuffer[bufferBit + 1] << 8) | receiveBuffer[bufferBit]); + bufferBit += 2; + } + } + + data.radius[ACC] = (int16_t)((receiveBuffer[19] << 8) | receiveBuffer[18]); + data.radius[MAG] = (int16_t)((receiveBuffer[21] << 8) | receiveBuffer[20]); + + bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF); + delay(25); + + return data; +} + +#endif \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055_serial.h b/src/main/drivers/accgyro/accgyro_bno055_serial.h new file mode 100644 index 0000000000..1ffd55c646 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bno055_serial.h @@ -0,0 +1,33 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 http://www.gnu.org/licenses/. + */ + +#pragma once + +#include "common/vector.h" +#include "drivers/accgyro/accgyro_bno055.h" + +bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibration); +void bno055SerialFetchEulerAngles(void); +void bno055SerialGetCalibStat(void); +bno055CalibrationData_t bno055SerialGetCalibrationData(void); \ No newline at end of file diff --git a/src/main/drivers/display_font_metadata.h b/src/main/drivers/display_font_metadata.h index be3fb94a40..ebef4b635a 100644 --- a/src/main/drivers/display_font_metadata.h +++ b/src/main/drivers/display_font_metadata.h @@ -10,12 +10,11 @@ typedef struct displayFontMetadata_s { uint16_t charCount; } displayFontMetadata_t; -// 'I', 'N', 'A', 'V', 1 +// 'I', 'N', 'A', 'V' #define FONT_CHR_IS_METADATA(chr) ((chr)->data[0] == 'I' && \ (chr)->data[1] == 'N' && \ (chr)->data[2] == 'A' && \ - (chr)->data[3] == 'V' && \ - (chr)->data[4] == 1) + (chr)->data[3] == 'V') #define FONT_METADATA_CHR_INDEX 255 // Used for runtime detection of display drivers that might diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index d8accf4c31..b620473ca7 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -119,8 +119,9 @@ #define SYM_RPM 0x8B // 139 RPM #define SYM_WAYPOINT 0x8C // 140 Waypoint #define SYM_AZIMUTH 0x8D // 141 Azimuth -// 0x8E // 142 - -// 0x8F // 143 - + +#define SYM_TELEMETRY_0 0x8E // 142 Antenna tracking telemetry +#define SYM_TELEMETRY_1 0x8F // 143 Antenna tracking telemetry #define SYM_BATT_FULL 0x90 // 144 Battery full #define SYM_BATT_5 0x91 // 145 Battery @@ -153,8 +154,8 @@ #define SYM_HEADING 0xA9 // 169 Compass Heading symbol #define SYM_ALT 0xAA // 170 ALT #define SYM_WH 0xAB // 171 WH -#define SYM_WH_KM_0 0xAC // 172 WH/KM left -#define SYM_WH_KM_1 0xAD // 173 WH/KM right +#define SYM_WH_KM 0xAC // 172 WH/KM +#define SYM_WH_MI 0xAD // 173 WH/MI #define SYM_WATT 0xAE // 174 W #define SYM_SCALE 0xAF // 175 Map scale #define SYM_MPH 0xB0 // 176 MPH @@ -246,6 +247,9 @@ #define SYM_SNR 0xEE // SNR #define SYM_MW 0xED // mW +#define SYM_KILOWATT 0xEF // 239 kW + + #else #define TEMP_SENSOR_SYM_COUNT 0 diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index f1f2c1f646..b78d5b5e17 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -54,5 +54,5 @@ bool pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn void pwmWriteBeeper(bool onoffBeep); void beeperPwmInit(ioTag_t tag, uint16_t frequency); -void sendDShotCommand(dshotCommands_e directionSpin); +void sendDShotCommand(dshotCommands_e cmd); void initDShotCommands(void); \ No newline at end of file diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 148b74810f..60a3c91cf8 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2501,7 +2501,7 @@ static void cliFeature(char *cmdline) } } -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) static void printBeeper(uint8_t dumpMask, const beeperConfig_t *beeperConfig, const beeperConfig_t *beeperConfigDefault) { const uint8_t beeperCount = beeperTableEntryCount(); @@ -3484,7 +3484,7 @@ static void printConfig(const char *cmdline, bool doDiff) cliPrintHashLine("feature"); printFeature(dumpMask, &featureConfig_Copy, featureConfig()); -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) cliPrintHashLine("beeper"); printBeeper(dumpMask, &beeperConfig_Copy, beeperConfig()); #endif @@ -3655,7 +3655,7 @@ const clicmd_t cmdTable[] = { #ifdef USE_CLI_BATCH CLI_COMMAND_DEF("batch", "start or end a batch of commands", "start | end", cliBatch), #endif -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n" "\t<+|->[name]", cliBeeper), #endif diff --git a/src/main/fc/config.c b/src/main/fc/config.c index d38554bd21..25ecb435af 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -123,7 +123,14 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .name = SETTING_NAME_DEFAULT ); -PG_REGISTER(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 1); + +PG_RESET_TEMPLATE(beeperConfig_t, beeperConfig, + .beeper_off_flags = 0, + .preferred_beeper_off_flags = 0, + .dshot_beeper_enabled = SETTING_DSHOT_BEEPER_ENABLED_DEFAULT, + .dshot_beeper_tone = SETTING_DSHOT_BEEPER_TONE_DEFAULT, +); PG_REGISTER_WITH_RESET_TEMPLATE(adcChannelConfig_t, adcChannelConfig, PG_ADC_CHANNEL_CONFIG, 0); @@ -287,10 +294,6 @@ void validateAndFixConfig(void) } #endif -#if !defined(USE_MPU_DATA_READY_SIGNAL) - gyroConfigMutable()->gyroSync = false; -#endif - // Call target-specific validation function validateAndFixTargetConfig(); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 40fd41dfda..5a1b6e659b 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -89,6 +89,8 @@ PG_DECLARE(systemConfig_t, systemConfig); typedef struct beeperConfig_s { uint32_t beeper_off_flags; uint32_t preferred_beeper_off_flags; + bool dshot_beeper_enabled; + uint8_t dshot_beeper_tone; } beeperConfig_t; PG_DECLARE(beeperConfig_t, beeperConfig); diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index b425148581..4dc84dafe3 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -22,21 +22,6 @@ #include "config/parameter_group.h" #define MAX_CONTROL_RATE_PROFILE_COUNT 3 -/* -Max and min available values for rates are now stored as absolute -tenths of degrees-per-second [dsp/10] -That means, max. rotation rate 180 equals 1800dps - -New defaults of 200dps for pitch,roll and yaw are more less -equivalent of rates 0 from previous versions of iNav, Cleanflight, Baseflight -and so on. -*/ -#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX 180 -#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN 6 -#define CONTROL_RATE_CONFIG_YAW_RATE_MAX 180 -#define CONTROL_RATE_CONFIG_YAW_RATE_MIN 2 - -#define CONTROL_RATE_CONFIG_TPA_MAX 100 typedef struct controlRateConfig_s { diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3b810641b5..6361cde4d5 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -101,9 +101,6 @@ enum { ALIGN_MAG = 2 }; -#define GYRO_WATCHDOG_DELAY 100 // Watchdog for boards without interrupt for gyro -#define GYRO_SYNC_MAX_CONSECUTIVE_FAILURES 100 // After this many consecutive missed interrupts disable gyro sync and fall back to scheduled updates - #define EMERGENCY_ARMING_TIME_WINDOW_MS 10000 #define EMERGENCY_ARMING_COUNTER_STEP_MS 100 @@ -130,8 +127,8 @@ int16_t headFreeModeHold; uint8_t motorControlEnable = false; static bool isRXDataNew; -static uint32_t gyroSyncFailureCount; static disarmReason_t lastDisarmReason = DISARM_NONE; +timeUs_t lastDisarmTimeUs = 0; static emergencyArmingState_t emergencyArming; static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible @@ -301,6 +298,17 @@ static void updateArmingStatus(void) DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); } +#ifdef USE_DSHOT + /* CHECK: Don't arm if the DShot beeper was used recently, as there is a minimum delay before sending the next DShot command */ + if (micros() - getLastDshotBeeperCommandTimeUs() < getDShotBeaconGuardDelayUs()) { + ENABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER); + } else { + DISABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER); + } +#else + DISABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER); +#endif + if (isModeActivationConditionPresent(BOXPREARM)) { if (IS_RC_MODE_ACTIVE(BOXPREARM)) { if (prearmWasReset && (armingConfig()->prearmTimeoutMs == 0 || millis() - prearmActivationTime < armingConfig()->prearmTimeoutMs)) { @@ -403,6 +411,7 @@ void disarm(disarmReason_t disarmReason) { if (ARMING_FLAG(ARMED)) { lastDisarmReason = disarmReason; + lastDisarmTimeUs = micros(); DISABLE_ARMING_FLAG(ARMED); #ifdef USE_BLACKBOX @@ -429,6 +438,10 @@ void disarm(disarmReason_t disarmReason) } } +timeUs_t getLastDisarmTimeUs(void) { + return lastDisarmTimeUs; +} + disarmReason_t getDisarmReason(void) { return lastDisarmReason; @@ -481,20 +494,6 @@ void tryArm(void) { updateArmingStatus(); -#ifdef USE_DSHOT - if ( - STATE(MULTIROTOR) && - IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) && - emergencyArmingCanOverrideArmingDisabled() && - isMotorProtocolDshot() && - !FLIGHT_MODE(FLIP_OVER_AFTER_CRASH) - ) { - sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED); - ENABLE_ARMING_FLAG(ARMED); - enableFlightMode(FLIP_OVER_AFTER_CRASH); - return; - } -#endif #ifdef USE_PROGRAMMING_FRAMEWORK if ( !isArmingDisabled() || @@ -511,6 +510,21 @@ void tryArm(void) return; } +#ifdef USE_DSHOT + if ( + STATE(MULTIROTOR) && + IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) && + emergencyArmingCanOverrideArmingDisabled() && + isMotorProtocolDshot() && + !FLIGHT_MODE(FLIP_OVER_AFTER_CRASH) + ) { + sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED); + ENABLE_ARMING_FLAG(ARMED); + enableFlightMode(FLIP_OVER_AFTER_CRASH); + return; + } +#endif + #if defined(USE_NAV) // If nav_extra_arming_safety was bypassed we always // allow bypassing it even without the sticks set @@ -791,37 +805,18 @@ void processRx(timeUs_t currentTimeUs) } // Function for loop trigger -void FAST_CODE NOINLINE taskGyro(timeUs_t currentTimeUs) { +void FAST_CODE taskGyro(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); // getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point. // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop const timeDelta_t currentDeltaTime = getTaskDeltaTime(TASK_SELF); - timeUs_t gyroUpdateUs = currentTimeUs; - - if (gyroConfig()->gyroSync) { - while (true) { - gyroUpdateUs = micros(); - if (gyroSyncCheckUpdate()) { - gyroSyncFailureCount = 0; - break; - } - else if ((currentDeltaTime + cmpTimeUs(gyroUpdateUs, currentTimeUs)) >= (timeDelta_t)(getLooptime() + GYRO_WATCHDOG_DELAY)) { - gyroSyncFailureCount++; - break; - } - } - - // If we detect gyro sync failure - disable gyro sync - if (gyroSyncFailureCount > GYRO_SYNC_MAX_CONSECUTIVE_FAILURES) { - gyroConfigMutable()->gyroSync = false; - } - } /* Update actual hardware readings */ gyroUpdate(); #ifdef USE_OPFLOW if (sensors(SENSOR_OPFLOW)) { - opflowGyroUpdateCallback((timeUs_t)currentDeltaTime + (gyroUpdateUs - currentTimeUs)); + opflowGyroUpdateCallback(currentDeltaTime); } #endif } diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index b321c2162e..f41322fa02 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -37,6 +37,7 @@ typedef enum disarmReason_e { void handleInflightCalibrationStickPosition(void); void disarm(disarmReason_t disarmReason); +timeUs_t getLastDisarmTimeUs(void); void tryArm(void); disarmReason_t getDisarmReason(void); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 5fdcebc9e1..5b36dfbaef 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1229,7 +1229,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, motorConfig()->motorPwmProtocol); sbufWriteU16(dst, motorConfig()->motorPwmRate); sbufWriteU16(dst, servoConfig()->servoPwmRate); - sbufWriteU8(dst, gyroConfig()->gyroSync); + sbufWriteU8(dst, 0); break; case MSP_FILTER_CONFIG : @@ -1334,7 +1334,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, navConfig()->general.flags.rth_alt_control_mode); sbufWriteU16(dst, navConfig()->general.rth_abort_threshold); sbufWriteU16(dst, navConfig()->general.rth_altitude); - sbufWriteU16(dst, navConfig()->general.land_descent_rate); + sbufWriteU16(dst, navConfig()->general.land_minalt_vspd); + sbufWriteU16(dst, navConfig()->general.land_maxalt_vspd); sbufWriteU16(dst, navConfig()->general.land_slowdown_minalt); sbufWriteU16(dst, navConfig()->general.land_slowdown_maxalt); sbufWriteU16(dst, navConfig()->general.emerg_descent_rate); @@ -1798,14 +1799,14 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) for (int i = 0; i < 3; i++) { tmp_u8 = sbufReadU8(src); if (i == FD_YAW) { - ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); } else { - ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); } } tmp_u8 = sbufReadU8(src); - ((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = MIN(tmp_u8, CONTROL_RATE_CONFIG_TPA_MAX); + ((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = MIN(tmp_u8, SETTING_TPA_RATE_MAX); ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcMid8 = sbufReadU8(src); ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcExpo8 = sbufReadU8(src); ((controlRateConfig_t*)currentControlRateProfile)->throttle.pa_breakpoint = sbufReadU16(src); @@ -1835,9 +1836,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) for (uint8_t i = 0; i < 3; ++i) { tmp_u8 = sbufReadU8(src); if (i == FD_YAW) { - currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); } else { - currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); } } @@ -1847,9 +1848,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) for (uint8_t i = 0; i < 3; ++i) { tmp_u8 = sbufReadU8(src); if (i == FD_YAW) { - currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); } else { - currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); } } @@ -2163,7 +2164,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) motorConfigMutable()->motorPwmProtocol = sbufReadU8(src); motorConfigMutable()->motorPwmRate = sbufReadU16(src); servoConfigMutable()->servoPwmRate = sbufReadU16(src); - gyroConfigMutable()->gyroSync = sbufReadU8(src); + sbufReadU8(src); //Was gyroSync } else return MSP_RESULT_ERROR; break; @@ -2307,7 +2308,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) navConfigMutable()->general.flags.rth_alt_control_mode = sbufReadU8(src); navConfigMutable()->general.rth_abort_threshold = sbufReadU16(src); navConfigMutable()->general.rth_altitude = sbufReadU16(src); - navConfigMutable()->general.land_descent_rate = sbufReadU16(src); + navConfigMutable()->general.land_minalt_vspd = sbufReadU16(src); + navConfigMutable()->general.land_maxalt_vspd = sbufReadU16(src); navConfigMutable()->general.land_slowdown_minalt = sbufReadU16(src); navConfigMutable()->general.land_slowdown_maxalt = sbufReadU16(src); navConfigMutable()->general.emerg_descent_rate = sbufReadU16(src); diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 7544614810..513b367599 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -58,9 +58,9 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXNAVPOSHOLD, "NAV POSHOLD", 11 }, // old GPS HOLD { BOXMANUAL, "MANUAL", 12 }, { BOXBEEPERON, "BEEPER", 13 }, - { BOXLEDLOW, "LEDLOW", 15 }, + { BOXLEDLOW, "LEDS OFF", 15 }, { BOXLIGHTS, "LIGHTS", 16 }, - { BOXOSD, "OSD SW", 19 }, + { BOXOSD, "OSD OFF", 19 }, { BOXTELEMETRY, "TELEMETRY", 20 }, { BOXAUTOTUNE, "AUTO TUNE", 21 }, { BOXBLACKBOX, "BLACKBOX", 26 }, diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 33d417a1d6..9bb54d7671 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -306,7 +306,7 @@ void fcTasksInit(void) setTaskEnabled(TASK_AUX, true); setTaskEnabled(TASK_SERIAL, true); -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) setTaskEnabled(TASK_BEEPER, true); #endif #ifdef USE_LIGHTS @@ -401,7 +401,7 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_LOW, }, -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) [TASK_BEEPER] = { .taskName = "BEEPER", .taskFunc = beeperUpdate, diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index c05a390ea4..c5c67436a1 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -39,6 +39,7 @@ #include "fc/controlrate_profile.h" #include "fc/rc_adjustments.h" #include "fc/rc_curves.h" +#include "fc/settings.h" #include "navigation/navigation.h" @@ -61,15 +62,6 @@ static uint8_t adjustmentStateMask = 0; #define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex)) -#define RC_ADJUSTMENT_EXPO_MIN 0 -#define RC_ADJUSTMENT_EXPO_MAX 100 - -#define RC_ADJUSTMENT_MANUAL_RATE_MIN 0 -#define RC_ADJUSTMENT_MANUAL_RATE_MAX 100 - -#define RC_ADJUSTMENT_PID_MIN 0 -#define RC_ADJUSTMENT_PID_MAX 200 - // sync with adjustmentFunction_e static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = { { @@ -368,17 +360,17 @@ static void applyAdjustmentU16(adjustmentFunction_e adjustmentFunction, uint16_t static void applyAdjustmentExpo(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta) { - applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_EXPO_MIN, RC_ADJUSTMENT_EXPO_MAX); + applyAdjustmentU8(adjustmentFunction, val, delta, SETTING_RC_EXPO_MIN, SETTING_RC_EXPO_MAX); } static void applyAdjustmentManualRate(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta) { - return applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_MANUAL_RATE_MIN, RC_ADJUSTMENT_MANUAL_RATE_MAX); + return applyAdjustmentU8(adjustmentFunction, val, delta, SETTING_CONSTANT_MANUAL_RATE_MIN, SETTING_CONSTANT_MANUAL_RATE_MAX); } static void applyAdjustmentPID(adjustmentFunction_e adjustmentFunction, uint16_t *val, int delta) { - applyAdjustmentU16(adjustmentFunction, val, delta, RC_ADJUSTMENT_PID_MIN, RC_ADJUSTMENT_PID_MAX); + applyAdjustmentU16(adjustmentFunction, val, delta, SETTING_CONSTANT_RPYL_PID_MIN, SETTING_CONSTANT_RPYL_PID_MAX); } static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) @@ -406,7 +398,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t break; case ADJUSTMENT_PITCH_ROLL_RATE: case ADJUSTMENT_PITCH_RATE: - applyAdjustmentU8(ADJUSTMENT_PITCH_RATE, &controlRateConfig->stabilized.rates[FD_PITCH], delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + applyAdjustmentU8(ADJUSTMENT_PITCH_RATE, &controlRateConfig->stabilized.rates[FD_PITCH], delta, SETTING_PITCH_RATE_MIN, SETTING_PITCH_RATE_MAX); if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { schedulePidGainsUpdate(); break; @@ -415,7 +407,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t FALLTHROUGH; case ADJUSTMENT_ROLL_RATE: - applyAdjustmentU8(ADJUSTMENT_ROLL_RATE, &controlRateConfig->stabilized.rates[FD_ROLL], delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + applyAdjustmentU8(ADJUSTMENT_ROLL_RATE, &controlRateConfig->stabilized.rates[FD_ROLL], delta, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); schedulePidGainsUpdate(); break; case ADJUSTMENT_MANUAL_PITCH_ROLL_RATE: @@ -429,7 +421,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t applyAdjustmentManualRate(ADJUSTMENT_MANUAL_PITCH_RATE, &controlRateConfig->manual.rates[FD_PITCH], delta); break; case ADJUSTMENT_YAW_RATE: - applyAdjustmentU8(ADJUSTMENT_YAW_RATE, &controlRateConfig->stabilized.rates[FD_YAW], delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + applyAdjustmentU8(ADJUSTMENT_YAW_RATE, &controlRateConfig->stabilized.rates[FD_YAW], delta, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); schedulePidGainsUpdate(); break; case ADJUSTMENT_MANUAL_YAW_RATE: @@ -508,10 +500,10 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t schedulePidGainsUpdate(); break; case ADJUSTMENT_NAV_FW_CRUISE_THR: - applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &navConfigMutable()->fw.cruise_throttle, delta, 1000, 2000); + applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &navConfigMutable()->fw.cruise_throttle, delta, SETTING_NAV_FW_CRUISE_THR_MIN, SETTING_NAV_FW_CRUISE_THR_MAX); break; case ADJUSTMENT_NAV_FW_PITCH2THR: - applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &navConfigMutable()->fw.pitch_to_throttle, delta, 0, 100); + applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &navConfigMutable()->fw.pitch_to_throttle, delta, SETTING_NAV_FW_PITCH2THR_MIN, SETTING_NAV_FW_PITCH2THR_MAX); break; case ADJUSTMENT_ROLL_BOARD_ALIGNMENT: updateBoardAlignment(delta, 0); @@ -586,7 +578,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t navigationUsePIDs(); break; case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: - applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &mixerConfigMutable()->fwMinThrottleDownPitchAngle, delta, 0, FW_MIN_THROTTLE_DOWN_PITCH_ANGLE_MAX); + applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &mixerConfigMutable()->fwMinThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX); break; #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) case ADJUSTMENT_VTX_POWER_LEVEL: @@ -599,13 +591,13 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t break; #endif case ADJUSTMENT_TPA: - applyAdjustmentU8(ADJUSTMENT_TPA, &controlRateConfig->throttle.dynPID, delta, 0, CONTROL_RATE_CONFIG_TPA_MAX); + applyAdjustmentU8(ADJUSTMENT_TPA, &controlRateConfig->throttle.dynPID, delta, 0, SETTING_TPA_RATE_MAX); break; case ADJUSTMENT_TPA_BREAKPOINT: applyAdjustmentU16(ADJUSTMENT_TPA_BREAKPOINT, &controlRateConfig->throttle.pa_breakpoint, delta, PWM_RANGE_MIN, PWM_RANGE_MAX); break; case ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS: - applyAdjustmentU8(ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS, &navConfigMutable()->fw.control_smoothness, delta, 0, 9); + applyAdjustmentU8(ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS, &navConfigMutable()->fw.control_smoothness, delta, SETTING_NAV_FW_CONTROL_SMOOTHNESS_MIN, SETTING_NAV_FW_CONTROL_SMOOTHNESS_MAX); break; default: break; diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 47d6267cbd..6f7241244d 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -36,7 +36,7 @@ const char *armingDisableFlagNames[]= { "FS", "ANGLE", "CAL", "OVRLD", "NAV", "COMPASS", "ACC", "ARMSW", "HWFAIL", "BOXFS", "KILLSW", "RX", "THR", "CLI", "CMS", "OSD", "ROLL/PITCH", "AUTOTRIM", "OOM", - "SETTINGFAIL", "PWMOUT", "NOPREARM" + "SETTINGFAIL", "PWMOUT", "NOPREARM", "DSHOTBEEPER" }; #endif @@ -58,7 +58,8 @@ const armingFlag_e armDisableReasonsChecklist[] = { ARMING_DISABLED_ROLLPITCH_NOT_CENTERED, ARMING_DISABLED_SERVO_AUTOTRIM, ARMING_DISABLED_OOM, - ARMING_DISABLED_NO_PREARM + ARMING_DISABLED_NO_PREARM, + ARMING_DISABLED_DSHOT_BEEPER }; armingFlag_e isArmingDisabledReason(void) diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 9f6ec9efa3..84fe113c59 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -44,6 +44,7 @@ typedef enum { ARMING_DISABLED_INVALID_SETTING = (1 << 26), ARMING_DISABLED_PWM_OUTPUT_ERROR = (1 << 27), ARMING_DISABLED_NO_PREARM = (1 << 28), + ARMING_DISABLED_DSHOT_BEEPER = (1 << 29), ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | @@ -52,7 +53,7 @@ typedef enum { ARMING_DISABLED_BOXKILLSWITCH | ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI | ARMING_DISABLED_CMS_MENU | ARMING_DISABLED_OSD_MENU | ARMING_DISABLED_ROLLPITCH_NOT_CENTERED | ARMING_DISABLED_SERVO_AUTOTRIM | ARMING_DISABLED_OOM | ARMING_DISABLED_INVALID_SETTING | - ARMING_DISABLED_PWM_OUTPUT_ERROR | ARMING_DISABLED_NO_PREARM), + ARMING_DISABLED_PWM_OUTPUT_ERROR | ARMING_DISABLED_NO_PREARM | ARMING_DISABLED_DSHOT_BEEPER), } armingFlag_e; // Arming blockers that can be overriden by emergency arming. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f7b0b4f8c3..f6e09d5964 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -10,7 +10,7 @@ tables: values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE", "VL53L1X", "US42"] enum: rangefinderType_e - name: secondary_imu_hardware - values: ["NONE", "BNO055"] + values: ["NONE", "BNO055", "BNO055_SERIAL"] enum: secondaryImuType_e - name: mag_hardware values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", FAKE"] @@ -28,7 +28,7 @@ tables: values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UNUSED"] enum: rxReceiverType_e - name: serial_rx - values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST"] + values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST", "MAVLINK"] - name: rx_spi_protocol values: ["ELERES"] enum: rx_spi_protocol_e @@ -69,9 +69,15 @@ tables: - name: osd_stats_energy_unit values: ["MAH", "WH"] enum: osd_stats_energy_unit_e + - name: osd_stats_min_voltage_unit + values: ["BATTERY", "CELL"] + enum: osd_stats_min_voltage_unit_e - name: osd_video_system values: ["AUTO", "PAL", "NTSC"] enum: videoSystem_e + - name: osd_telemetry + values: ["OFF", "ON","TEST"] + enum: osd_telemetry_e - name: osd_alignment values: ["LEFT", "RIGHT"] enum: osd_alignment_e @@ -87,7 +93,8 @@ tables: "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY", - "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE", "AUTOTRIM"] + "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE", + "SMITH_COMPENSATOR", "AUTOTRIM"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator @@ -104,7 +111,7 @@ tables: enum: navRTHAllowLanding_e - name: bat_capacity_unit values: ["MAH", "MWH"] - enum: batCapacityUnit_e + enum: batCapacityUnit_e - name: bat_voltage_source values: ["RAW", "SAG_COMP"] enum: batVoltageSource_e @@ -163,6 +170,22 @@ tables: values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"] - name: osd_plus_code_short values: ["0", "2", "4", "6"] + - name: safehome_usage_mode + values: ["OFF", "RTH", "RTH_FS"] + enum: safehomeUsageMode_e + - name: nav_rth_climb_first + enum: navRTHClimbFirst_e + values: ["OFF", "ON", "ON_FW_SPIRAL"] + +constants: + RPYL_PID_MIN: 0 + RPYL_PID_MAX: 200 + + MANUAL_RATE_MIN: 0 + MANUAL_RATE_MAX: 100 + + ROLL_PITCH_RATE_MIN: 6 + ROLL_PITCH_RATE_MAX: 180 groups: - name: PG_GYRO_CONFIG @@ -173,11 +196,6 @@ groups: description: "This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible." default_value: 1000 max: 9000 - - name: gyro_sync - description: "This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf" - default_value: ON - field: gyroSync - type: bool - name: align_gyro description: "When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP." default_value: "DEFAULT" @@ -1204,7 +1222,7 @@ groups: default_value: 0 field: throttle.dynPID min: 0 - max: CONTROL_RATE_CONFIG_TPA_MAX + max: 100 - name: tpa_breakpoint description: "See tpa_rate." default_value: 1500 @@ -1235,20 +1253,20 @@ groups: description: "Defines rotation rate on ROLL axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure." default_value: 20 field: stabilized.rates[FD_ROLL] - min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN - max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX + min: ROLL_PITCH_RATE_MIN + max: ROLL_PITCH_RATE_MAX - name: pitch_rate description: "Defines rotation rate on PITCH axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure." default_value: 20 field: stabilized.rates[FD_PITCH] - min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN - max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX + min: ROLL_PITCH_RATE_MIN + max: ROLL_PITCH_RATE_MAX - name: yaw_rate description: "Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure." default_value: 20 field: stabilized.rates[FD_YAW] - min: CONTROL_RATE_CONFIG_YAW_RATE_MIN - max: CONTROL_RATE_CONFIG_YAW_RATE_MAX + min: 2 + max: 180 - name: manual_rc_expo description: "Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100]" default_value: 70 @@ -1265,20 +1283,20 @@ groups: description: "Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]%" default_value: 100 field: manual.rates[FD_ROLL] - min: 0 - max: 100 + min: MANUAL_RATE_MIN + max: MANUAL_RATE_MAX - name: manual_pitch_rate description: "Servo travel multiplier for the PITCH axis in `MANUAL` flight mode [0-100]%" default_value: 100 field: manual.rates[FD_PITCH] - min: 0 - max: 100 + min: MANUAL_RATE_MIN + max: MANUAL_RATE_MAX - name: manual_yaw_rate description: "Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]%" default_value: 100 field: manual.rates[FD_YAW] - min: 0 - max: 100 + min: MANUAL_RATE_MIN + max: MANUAL_RATE_MAX - name: fpv_mix_degrees field: misc.fpvCamAngleDegrees min: 0 @@ -1499,182 +1517,182 @@ groups: description: "Multicopter rate stabilisation P-gain for PITCH" default_value: 40 field: bank_mc.pid[PID_PITCH].P - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_i_pitch description: "Multicopter rate stabilisation I-gain for PITCH" default_value: 30 field: bank_mc.pid[PID_PITCH].I - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_d_pitch description: "Multicopter rate stabilisation D-gain for PITCH" default_value: 23 field: bank_mc.pid[PID_PITCH].D - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_cd_pitch description: "Multicopter Control Derivative gain for PITCH" default_value: 60 field: bank_mc.pid[PID_PITCH].FF - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_p_roll description: "Multicopter rate stabilisation P-gain for ROLL" default_value: 40 field: bank_mc.pid[PID_ROLL].P - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_i_roll description: "Multicopter rate stabilisation I-gain for ROLL" default_value: 30 field: bank_mc.pid[PID_ROLL].I - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_d_roll description: "Multicopter rate stabilisation D-gain for ROLL" default_value: 23 field: bank_mc.pid[PID_ROLL].D - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_cd_roll description: "Multicopter Control Derivative gain for ROLL" default_value: 60 field: bank_mc.pid[PID_ROLL].FF - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_p_yaw description: "Multicopter rate stabilisation P-gain for YAW" default_value: 85 field: bank_mc.pid[PID_YAW].P - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_i_yaw description: "Multicopter rate stabilisation I-gain for YAW" default_value: 45 field: bank_mc.pid[PID_YAW].I - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_d_yaw description: "Multicopter rate stabilisation D-gain for YAW" default_value: 0 field: bank_mc.pid[PID_YAW].D - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_cd_yaw description: "Multicopter Control Derivative gain for YAW" default_value: 60 field: bank_mc.pid[PID_YAW].FF - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_p_level description: "Multicopter attitude stabilisation P-gain" default_value: 20 field: bank_mc.pid[PID_LEVEL].P - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_i_level description: "Multicopter attitude stabilisation low-pass filter cutoff" default_value: 15 field: bank_mc.pid[PID_LEVEL].I - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_d_level description: "Multicopter attitude stabilisation HORIZON transition point" default_value: 75 field: bank_mc.pid[PID_LEVEL].D - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_p_pitch description: "Fixed-wing rate stabilisation P-gain for PITCH" default_value: 5 field: bank_fw.pid[PID_PITCH].P - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_i_pitch description: "Fixed-wing rate stabilisation I-gain for PITCH" default_value: 7 field: bank_fw.pid[PID_PITCH].I - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_d_pitch description: "Fixed wing rate stabilisation D-gain for PITCH" default_value: 0 field: bank_fw.pid[PID_PITCH].D - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_ff_pitch description: "Fixed-wing rate stabilisation FF-gain for PITCH" default_value: 50 field: bank_fw.pid[PID_PITCH].FF - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_p_roll description: "Fixed-wing rate stabilisation P-gain for ROLL" default_value: 5 field: bank_fw.pid[PID_ROLL].P - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_i_roll description: "Fixed-wing rate stabilisation I-gain for ROLL" default_value: 7 field: bank_fw.pid[PID_ROLL].I - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_d_roll description: "Fixed wing rate stabilisation D-gain for ROLL" default_value: 0 field: bank_fw.pid[PID_ROLL].D - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_ff_roll description: "Fixed-wing rate stabilisation FF-gain for ROLL" default_value: 50 field: bank_fw.pid[PID_ROLL].FF - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_p_yaw description: "Fixed-wing rate stabilisation P-gain for YAW" default_value: 6 field: bank_fw.pid[PID_YAW].P - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_i_yaw description: "Fixed-wing rate stabilisation I-gain for YAW" default_value: 10 field: bank_fw.pid[PID_YAW].I - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_d_yaw description: "Fixed wing rate stabilisation D-gain for YAW" default_value: 0 field: bank_fw.pid[PID_YAW].D - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_ff_yaw description: "Fixed-wing rate stabilisation FF-gain for YAW" default_value: 60 field: bank_fw.pid[PID_YAW].FF - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_p_level description: "Fixed-wing attitude stabilisation P-gain" default_value: 20 field: bank_fw.pid[PID_LEVEL].P - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_i_level description: "Fixed-wing attitude stabilisation low-pass filter cutoff" default_value: 5 field: bank_fw.pid[PID_LEVEL].I - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_d_level description: "Fixed-wing attitude stabilisation HORIZON transition point" default_value: 75 field: bank_fw.pid[PID_LEVEL].D - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: max_angle_inclination_rll description: "Maximum inclination in level (angle) mode (ROLL axis). 100=10°" default_value: 300 @@ -1726,10 +1744,10 @@ groups: table: direction - name: fw_reference_airspeed description: "Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present." - default_value: 1000 + default_value: 1500 field: fixedWingReferenceAirspeed - min: 1 - max: 5000 + min: 300 + max: 6000 - name: fw_turn_assist_yaw_gain description: "Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter" default_value: 1 @@ -2049,6 +2067,27 @@ groups: field: fixedWingLevelTrim min: -10 max: 10 + - name: smith_predictor_strength + description: "The strength factor of a Smith Predictor of PID measurement. In percents" + default_value: 0.5 + field: smithPredictorStrength + condition: USE_SMITH_PREDICTOR + min: 0 + max: 1 + - name: smith_predictor_delay + description: "Expected delay of the gyro signal. In milliseconds" + default_value: 0 + field: smithPredictorDelay + condition: USE_SMITH_PREDICTOR + min: 0 + max: 8 + - name: smith_predictor_lpf_hz + description: "Cutoff frequency for the Smith Predictor Low Pass Filter" + default_value: 50 + field: smithPredictorFilterHz + condition: USE_SMITH_PREDICTOR + min: 1 + max: 500 - name: PG_PID_AUTOTUNE_CONFIG type: pidAutotuneConfig_t @@ -2282,20 +2321,26 @@ groups: field: general.max_manual_climb_rate min: 10 max: 2000 - - name: nav_landing_speed - description: "Vertical descent velocity during the RTH landing phase. [cm/s]" + - name: nav_land_minalt_vspd + description: "Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s]" + default_value: 50 + field: general.land_minalt_vspd + min: 50 + max: 500 + - name: nav_land_maxalt_vspd + description: "Vertical descent velocity above nav_land_slowdown_maxalt during the RTH landing phase. [cm/s]" default_value: 200 - field: general.land_descent_rate + field: general.land_maxalt_vspd min: 100 max: 2000 - name: nav_land_slowdown_minalt - description: "Defines at what altitude the descent velocity should start to be 25% of nav_landing_speed [cm]" + description: "Defines at what altitude the descent velocity should start to be `nav_land_minalt_vspd` [cm]" default_value: 500 field: general.land_slowdown_minalt min: 50 max: 1000 - name: nav_land_slowdown_maxalt - description: "Defines at what altitude the descent velocity should start to ramp down from 100% nav_landing_speed to 25% nav_landing_speed. [cm]" + description: "Defines at what altitude the descent velocity should start to ramp down from `nav_land_maxalt_vspd` to `nav_land_minalt_vspd` during the RTH landing phase [cm]" default_value: 2000 field: general.land_slowdown_maxalt min: 500 @@ -2318,10 +2363,10 @@ groups: field: general.flags.nav_overrides_motor_stop table: nav_overrides_motor_stop - name: nav_rth_climb_first - description: "If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way." - default_value: ON + description: "If set to ON or ON_FW_SPIRAL aircraft will climb to nav_rth_altitude first before turning to head home. If set to OFF aircraft will turn and head home immediately climbing on the way. For a fixed wing ON will use a linear climb, ON_FW_SPIRAL will use a loiter turning climb with climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius (ON_FW_SPIRAL is a fixed wing setting and behaves the same as ON for a multirotor)" + default_value: "ON" field: general.flags.rth_climb_first - type: bool + table: nav_rth_climb_first - name: nav_rth_climb_ignore_emerg description: "If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status." default_value: OFF @@ -2342,6 +2387,11 @@ groups: default_value: "AT_LEAST" field: general.flags.rth_alt_control_mode table: nav_rth_alt_mode + - name: nav_rth_alt_control_override + description: "If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the RTH climb phase using full pitch or roll stick held for > 1 second. RTH altitude is reset to the current altitude using pitch down stick. RTH CLIMB FIRST is overridden using right roll stick so craft turns and heads directly to home (CLIMB FIRST override only works for fixed wing)" + default_value: OFF + field: general.flags.rth_alt_control_override + type: bool - name: nav_rth_abort_threshold description: "RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm]" default_value: 50000 @@ -2369,6 +2419,11 @@ groups: field: general.safehome_max_distance min: 0 max: 65000 + - name: safehome_usage_mode + description: "Used to control when safehomes will be used. Possible values are `OFF`, `RTH` and `RTH_FS`. See [Safehome documentation](Safehomes.md#Safehome) for more information." + default_value: "RTH" + field: general.flags.safehome_usage_mode + table: safehome_usage_mode - name: nav_mc_bank_angle description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude" default_value: 30 @@ -2815,6 +2870,13 @@ groups: min: 0 max: 255 default_value: 1 + - name: mavlink_version + field: mavlink.version + description: "Version of MAVLink to use" + type: uint8_t + min: 1 + max: 2 + default_value: 2 - name: PG_ELERES_CONFIG type: eleresConfig_t headers: ["rx/eleres.h"] @@ -2868,6 +2930,12 @@ groups: headers: ["io/osd.h", "drivers/osd.h"] condition: USE_OSD members: + - name: osd_telemetry + description: "To enable OSD telemetry for antenna tracker. Possible values are `OFF`, `ON` and `TEST`" + table: osd_telemetry + field: telemetry + type: uint8_t + default_value: "OFF" - name: osd_video_system description: "Video system used. Possible values are `AUTO`, `PAL` and `NTSC`" default_value: "AUTO" @@ -2892,6 +2960,12 @@ groups: field: stats_energy_unit table: osd_stats_energy_unit type: uint8_t + - name: osd_stats_min_voltage_unit + description: "Display minimum voltage of the `BATTERY` or the average per `CELL` in the OSD stats." + default_value: "BATTERY" + field: stats_min_voltage_unit + table: osd_stats_min_voltage_unit + type: uint8_t - name: osd_rssi_alarm description: "Value below which to make the OSD RSSI indicator blink" @@ -3488,3 +3562,20 @@ groups: field: speedSource table: djiOsdSpeedSource type: uint8_t + + - name: PG_BEEPER_CONFIG + type: beeperConfig_t + headers: [ "fc/config.h" ] + members: + - name: dshot_beeper_enabled + description: "Whether using DShot motors as beepers is enabled" + default_value: ON + field: dshot_beeper_enabled + type: bool + - name: dshot_beeper_tone + description: "Sets the DShot beeper tone" + min: 1 + max: 5 + default_value: 1 + field: dshot_beeper_tone + type: uint8_t diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 7a0c3612d7..90173dc473 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -47,6 +47,7 @@ #include "flight/pid.h" #include "navigation/navigation.h" +#include "navigation/navigation_private.h" #include "rx/rx.h" @@ -362,12 +363,15 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) // Craft is closer than minimum failsafe procedure distance (if set to non-zero) // GPS must also be working, and home position set - if ((failsafeConfig()->failsafe_min_distance > 0) && - ((GPS_distanceToHome * 100) < failsafeConfig()->failsafe_min_distance) && - sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { + if (failsafeConfig()->failsafe_min_distance > 0 && + sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { - // Use the alternate, minimum distance failsafe procedure instead - return failsafeConfig()->failsafe_min_distance_procedure; + // get the distance to the original arming point + uint32_t distance = calculateDistanceToDestination(&original_rth_home); + if (distance < failsafeConfig()->failsafe_min_distance) { + // Use the alternate, minimum distance failsafe procedure instead + return failsafeConfig()->failsafe_min_distance_procedure; + } } return failsafeConfig()->failsafe_procedure; diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 042a52186f..cafc01b007 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -57,7 +57,7 @@ typedef enum { /* In this phase, the connection from the receiver * has been confirmed as lost and it will either * transition into FAILSAFE_RX_LOSS_RECOVERED if the - * RX link is recovered inmmediately or one of the + * RX link is recovered immediately or one of the * recovery phases otherwise (as configured via * failsafe_procedure) or into FAILSAFE_RX_LOSS_IDLE * if failsafe_procedure is NONE. diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index af1ca40a20..cc9f6b2bcc 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -73,6 +73,8 @@ static EXTENDED_FASTRAM int throttleRangeMin = 0; static EXTENDED_FASTRAM int throttleRangeMax = 0; static EXTENDED_FASTRAM int8_t motorYawMultiplier = 1; +int motorZeroCommand = 0; + PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0); PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, @@ -273,7 +275,6 @@ void mixerInit(void) void mixerResetDisarmedMotors(void) { - int motorZeroCommand; if (feature(FEATURE_REVERSIBLE_MOTORS)) { motorZeroCommand = reversibleMotorsConfig()->neutral; @@ -720,3 +721,18 @@ void loadPrimaryMotorMixer(void) { currentMixer[i] = *primaryMotorMixer(i); } } + +bool areMotorsRunning(void) +{ + if (ARMING_FLAG(ARMED)) { + return true; + } else { + for (int i = 0; i < motorCount; i++) { + if (motor_disarmed[i] != motorZeroCommand) { + return true; + } + } + } + + return false; +} diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 60107e28e3..0933eab2ee 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -25,11 +25,6 @@ #define MAX_SUPPORTED_MOTORS 12 #endif -#define YAW_JUMP_PREVENTION_LIMIT_LOW 80 -#define YAW_JUMP_PREVENTION_LIMIT_HIGH 500 - -#define FW_MIN_THROTTLE_DOWN_PITCH_ANGLE_MAX 450 - // Digital protocol has fixed values #define DSHOT_DISARM_COMMAND 0 #define DSHOT_MIN_THROTTLE 48 @@ -132,4 +127,5 @@ void processContinuousServoAutotrim(const float dT); void stopMotors(void); void stopPwmAllMotors(void); -void loadPrimaryMotorMixer(void); \ No newline at end of file +void loadPrimaryMotorMixer(void); +bool areMotorsRunning(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 89458e30ab..bbf16c19c2 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -48,6 +48,7 @@ FILE_COMPILE_FOR_SPEED #include "flight/rpm_filter.h" #include "flight/secondary_imu.h" #include "flight/kalman.h" +#include "flight/smith_predictor.h" #include "io/gps.h" @@ -109,6 +110,8 @@ typedef struct { bool itermFreezeActive; biquadFilter_t rateTargetFilter; + + smithPredictor_t smithPredictor; } pidState_t; STATIC_FASTRAM bool pidFiltersConfigured = false; @@ -159,7 +162,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; static EXTENDED_FASTRAM float fixedWingLevelTrim; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 1); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 2); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -297,6 +300,12 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, #endif .fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT, + +#ifdef USE_SMITH_PREDICTOR + .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT, + .smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT, + .smithPredictorFilterHz = SETTING_SMITH_PREDICTOR_LPF_HZ_DEFAULT, +#endif ); bool pidInitFilters(void) @@ -349,6 +358,30 @@ bool pidInitFilters(void) } } +#ifdef USE_SMITH_PREDICTOR + smithPredictorInit( + &pidState[FD_ROLL].smithPredictor, + pidProfile()->smithPredictorDelay, + pidProfile()->smithPredictorStrength, + pidProfile()->smithPredictorFilterHz, + getLooptime() + ); + smithPredictorInit( + &pidState[FD_PITCH].smithPredictor, + pidProfile()->smithPredictorDelay, + pidProfile()->smithPredictorStrength, + pidProfile()->smithPredictorFilterHz, + getLooptime() + ); + smithPredictorInit( + &pidState[FD_YAW].smithPredictor, + pidProfile()->smithPredictorDelay, + pidProfile()->smithPredictorStrength, + pidProfile()->smithPredictorFilterHz, + getLooptime() + ); +#endif + pidFiltersConfigured = true; return true; @@ -1068,6 +1101,13 @@ void FAST_CODE pidController(float dT) pidState[axis].gyroRate = gyroKalmanUpdate(axis, pidState[axis].gyroRate, pidState[axis].rateTarget); } #endif + +#ifdef USE_SMITH_PREDICTOR + DEBUG_SET(DEBUG_SMITH_COMPENSATOR, axis, pidState[axis].gyroRate); + pidState[axis].gyroRate = applySmithPredictor(axis, &pidState[axis].smithPredictor, pidState[axis].gyroRate); + DEBUG_SET(DEBUG_SMITH_COMPENSATOR, axis + 3, pidState[axis].gyroRate); +#endif + DEBUG_SET(DEBUG_PID_MEASUREMENT, axis, pidState[axis].gyroRate); } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index cb8dc55780..e418722c12 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -161,6 +161,11 @@ typedef struct pidProfile_s { #endif float fixedWingLevelTrim; +#ifdef USE_SMITH_PREDICTOR + float smithPredictorStrength; + float smithPredictorDelay; + uint16_t smithPredictorFilterHz; +#endif } pidProfile_t; typedef struct pidAutotuneConfig_s { diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index 4ddc719037..7bae4fa731 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -145,21 +145,6 @@ static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float spee // returns Wh static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { - // Fixed wing only for now - if (!STATE(FIXED_WING_LEGACY)) - return -1; - - if (!(feature(FEATURE_VBAT) && batteryWasFullWhenPluggedIn() - && feature(FEATURE_CURRENT_METER) && (batteryMetersConfig()->cruise_power > 0) - && (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH) && (currentBatteryProfile->capacity.value > 0) - && navigationPositionEstimateIsHealthy() && isImuHeadingValid() && (navConfig()->fw.cruise_speed > 0) - && ((!STATE(FIXED_WING_LEGACY)) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && isFixedWingLaunchDetected())) - && (ARMING_FLAG(ARMED)) -#ifdef USE_WIND_ESTIMATOR - && (!takeWindIntoAccount || isEstimatedWindSpeedValid()) -#endif - )) - return -1; const float RTH_initial_altitude_change = MAX(0, (getFinalRTHAltitude() - getEstimatedActualPosition(Z)) / 100); @@ -224,6 +209,26 @@ float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) { // returns meters float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) { + // Fixed wing only for now + if (!(STATE(FIXED_WING_LEGACY) || ARMING_FLAG(ARMED))) { + return -1; + } + +#ifdef USE_WIND_ESTIMATOR + if (takeWindIntoAccount && !isEstimatedWindSpeedValid()) { + return -1; + } +#endif + + // check requirements + const bool areBatterySettingsOK = feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && batteryWasFullWhenPluggedIn(); + const bool areRTHEstimatorSettingsOK = batteryMetersConfig()->cruise_power > 0 && currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH &¤tBatteryProfile->capacity.value > 0 && navConfig()->fw.cruise_speed > 0; + const bool isNavigationOK = navigationPositionEstimateIsHealthy() && isImuHeadingValid(); + + if (!(areBatterySettingsOK && areRTHEstimatorSettingsOK && isNavigationOK)) { + return -1; + } + const float remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount); // error: return error code directly diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index 93cd11b9d1..b8e5a20160 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -29,10 +29,13 @@ #include "common/utils.h" #include "common/axis.h" +#include "build/debug.h" +#include "scheduler/scheduler.h" #include "config/parameter_group_ids.h" #include "drivers/sensor.h" #include "drivers/accgyro/accgyro_bno055.h" +#include "drivers/accgyro/accgyro_bno055_serial.h" #include "fc/settings.h" @@ -94,6 +97,17 @@ void secondaryImuInit(void) if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) { secondaryImuState.active = bno055Init(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag)); + + if (secondaryImuState.active) { + rescheduleTask(TASK_SECONDARY_IMU, TASK_PERIOD_HZ(10)); + } + + } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) { + secondaryImuState.active = bno055SerialInit(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag)); + + if (secondaryImuState.active) { + rescheduleTask(TASK_SECONDARY_IMU, TASK_PERIOD_HZ(50)); + } } if (!secondaryImuState.active) { @@ -102,19 +116,7 @@ void secondaryImuInit(void) } -void taskSecondaryImu(timeUs_t currentTimeUs) -{ - if (!secondaryImuState.active) - { - return; - } - /* - * Secondary IMU works in decidegrees - */ - UNUSED(currentTimeUs); - - bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw); - +void secondaryImuProcess(void) { //Apply mag declination secondaryImuState.eulerAngles.raw[2] += secondaryImuState.magDeclination; @@ -141,17 +143,6 @@ void taskSecondaryImu(timeUs_t currentTimeUs) secondaryImuState.eulerAngles.values.pitch = rotated.y; secondaryImuState.eulerAngles.values.yaw = rotated.z; - /* - * Every 10 cycles fetch current calibration state - */ - static uint8_t tick = 0; - tick++; - if (tick == 10) - { - secondaryImuState.calibrationStatus = bno055GetCalibStat(); - tick = 0; - } - DEBUG_SET(DEBUG_IMU2, 0, secondaryImuState.eulerAngles.values.roll); DEBUG_SET(DEBUG_IMU2, 1, secondaryImuState.eulerAngles.values.pitch); DEBUG_SET(DEBUG_IMU2, 2, secondaryImuState.eulerAngles.values.yaw); @@ -162,8 +153,56 @@ void taskSecondaryImu(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_IMU2, 6, secondaryImuState.magDeclination); } +void taskSecondaryImu(timeUs_t currentTimeUs) +{ + static uint8_t tick = 0; + tick++; + + if (!secondaryImuState.active) + { + return; + } + /* + * Secondary IMU works in decidegrees + */ + UNUSED(currentTimeUs); + + if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) { + bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw); + secondaryImuProcess(); + + /* + * Every 2 seconds fetch current calibration state + */ + if (tick == 20) + { + secondaryImuState.calibrationStatus = bno055GetCalibStat(); + tick = 0; + } + } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) { + /* + * Every 2 seconds fetch current calibration state + */ + if (tick == 100) + { + bno055SerialGetCalibStat(); + tick = 0; + } else { + bno055SerialFetchEulerAngles(); + } + } +} + void secondaryImuFetchCalibration(void) { - bno055CalibrationData_t calibrationData = bno055GetCalibrationData(); + bno055CalibrationData_t calibrationData; + + if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) { + calibrationData = bno055GetCalibrationData(); + } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) { + calibrationData = bno055SerialGetCalibrationData(); + } else { + return; + } secondaryImuConfigMutable()->calibrationOffsetAcc[X] = calibrationData.offset[ACC][X]; secondaryImuConfigMutable()->calibrationOffsetAcc[Y] = calibrationData.offset[ACC][Y]; diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h index b674bc9c93..e7414a48c7 100644 --- a/src/main/flight/secondary_imu.h +++ b/src/main/flight/secondary_imu.h @@ -28,10 +28,12 @@ #include "common/time.h" #include "sensors/sensors.h" #include "drivers/accgyro/accgyro_bno055.h" +#include "drivers/accgyro/accgyro_bno055_serial.h" typedef enum { - SECONDARY_IMU_NONE = 0, - SECONDARY_IMU_BNO055 = 1, + SECONDARY_IMU_NONE = 0, + SECONDARY_IMU_BNO055 = 1, + SECONDARY_IMU_BNO055_SERIAL = 2, } secondaryImuType_e; typedef struct secondaryImuConfig_s { @@ -60,6 +62,7 @@ extern secondaryImuState_t secondaryImuState; PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig); +void secondaryImuProcess(void); void secondaryImuInit(void); void taskSecondaryImu(timeUs_t currentTimeUs); void secondaryImuFetchCalibration(void); diff --git a/src/main/flight/smith_predictor.c b/src/main/flight/smith_predictor.c new file mode 100644 index 0000000000..271c088add --- /dev/null +++ b/src/main/flight/smith_predictor.c @@ -0,0 +1,70 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 http://www.gnu.org/licenses/. + * + * This code is a derivative of work done in EmuFlight Project https://github.com/emuflight/EmuFlight + * + */ + +#include "platform.h" +#ifdef USE_SMITH_PREDICTOR + +#include +#include "common/axis.h" +#include "common/utils.h" +#include "flight/smith_predictor.h" +#include "build/debug.h" + +FUNCTION_COMPILE_FOR_SPEED +float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample) { + UNUSED(axis); + if (predictor->enabled) { + predictor->data[predictor->idx] = sample; + + predictor->idx++; + if (predictor->idx > predictor->samples) { + predictor->idx = 0; + } + + // filter the delayed data to help reduce the overall noise this prediction adds + float delayed = pt1FilterApply(&predictor->smithPredictorFilter, predictor->data[predictor->idx]); + float delayCompensatedSample = predictor->smithPredictorStrength * (sample - delayed); + + sample += delayCompensatedSample; + } + return sample; +} + +FUNCTION_COMPILE_FOR_SIZE +void smithPredictorInit(smithPredictor_t *predictor, float delay, float strength, uint16_t filterLpfHz, uint32_t looptime) { + if (delay > 0.1) { + predictor->enabled = true; + predictor->samples = (delay * 1000) / looptime; + predictor->idx = 0; + predictor->smithPredictorStrength = strength; + pt1FilterInit(&predictor->smithPredictorFilter, filterLpfHz, looptime * 1e-6f); + } else { + predictor->enabled = false; + } +} + +#endif \ No newline at end of file diff --git a/src/main/flight/smith_predictor.h b/src/main/flight/smith_predictor.h new file mode 100644 index 0000000000..1d8ebcb13b --- /dev/null +++ b/src/main/flight/smith_predictor.h @@ -0,0 +1,45 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 http://www.gnu.org/licenses/. + * + * This code is a derivative of work done in EmuFlight Project https://github.com/emuflight/EmuFlight + * + */ + +#pragma once + +#include +#include "common/filter.h" + +#define MAX_SMITH_SAMPLES 64 + +typedef struct smithPredictor_s { + bool enabled; + uint8_t samples; + uint8_t idx; + float data[MAX_SMITH_SAMPLES + 1]; + pt1Filter_t smithPredictorFilter; + float smithPredictorStrength; +} smithPredictor_t; + +float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample); +void smithPredictorInit(smithPredictor_t *predictor, float delay, float strength, uint16_t filterLpfHz, uint32_t looptime); \ No newline at end of file diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index e24216cda0..999cee97d2 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -45,6 +45,13 @@ #include "config/feature.h" +#ifdef USE_DSHOT +#include "drivers/pwm_output.h" +#include "fc/fc_core.h" +#include "flight/mixer.h" +static timeUs_t lastDshotBeeperCommandTimeUs; +#endif + #include "io/beeper.h" #define MAX_MULTI_BEEPS 20 //size limit for 'beep_multiBeeps[]' @@ -331,6 +338,17 @@ void beeperUpdate(timeUs_t currentTimeUs) } if (!beeperIsOn) { +#ifdef USE_DSHOT + if (!areMotorsRunning() + && beeperConfig()->dshot_beeper_enabled + && (currentBeeperEntry->mode == BEEPER_RX_SET || currentBeeperEntry->mode == BEEPER_RX_LOST) + && currentTimeUs - getLastDisarmTimeUs() > getDShotBeaconGuardDelayUs()) + { + lastDshotBeeperCommandTimeUs = currentTimeUs; + sendDShotCommand(beeperConfig()->dshot_beeper_tone); + } +#endif + beeperIsOn = 1; if (currentBeeperEntry->sequence[beeperPos] != 0) { if (!(getBeeperOffMask() & (1 << (currentBeeperEntry->mode - 1)))) @@ -407,3 +425,25 @@ int beeperTableEntryCount(void) { return (int)BEEPER_TABLE_ENTRY_COUNT; } + +#ifdef USE_DSHOT +timeUs_t getDShotBeaconGuardDelayUs(void) { + // Based on Digital_Cmd_Spec.txt - all delays have 100ms added to ensure that the minimum time has passed. + switch (beeperConfig()->dshot_beeper_tone) { + case 1: + case 2: + return 260000 + 100000; + case 3: + case 4: + return 280000 + 100000; + case 5: + default: + return 1020000 + 100000; + } +} + +timeUs_t getLastDshotBeeperCommandTimeUs(void) +{ + return lastDshotBeeperCommandTimeUs; +} +#endif diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 0fcc8249dd..38f38b236e 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -60,3 +60,7 @@ uint32_t getArmingBeepTimeMicros(void); beeperMode_e beeperModeForTableIndex(int idx); const char *beeperNameForTableIndex(int idx); int beeperTableEntryCount(void); +#ifdef USE_DSHOT +timeUs_t getDShotBeaconGuardDelayUs(void); +timeUs_t getLastDshotBeeperCommandTimeUs(void); +#endif diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 8fd74fc8b6..022d85fa36 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -935,7 +935,7 @@ void ledStripUpdate(timeUs_t currentTimeUs) for (timId_e timId = 0; timId < timTimerCount; timId++) { // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value. // max delay is limited to 5s - int32_t delta = cmpTimeUs(currentTimeUs, timerVal[timId]); + timeDelta_t delta = cmpTimeUs(currentTimeUs, timerVal[timId]); if (delta < 0 && delta > -LED_STRIP_MS(5000)) continue; // not ready yet timActive |= 1 << timId; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 7c8825e3a1..7f9d038753 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -104,6 +104,7 @@ FILE_COMPILE_FOR_SPEED #include "sensors/pitotmeter.h" #include "sensors/temperature.h" #include "sensors/esc_sensor.h" +#include "sensors/rangefinder.h" #include "programming/logic_condition.h" #include "programming/global_variables.h" @@ -143,7 +144,7 @@ FILE_COMPILE_FOR_SPEED #define OSD_CENTER_LEN(x) ((osdDisplayPort->cols - x) / 2) #define OSD_CENTER_S(s) OSD_CENTER_LEN(strlen(s)) -#define OSD_MIN_FONT_VERSION 1 +#define OSD_MIN_FONT_VERSION 2 static unsigned currentLayout = 0; static int layoutOverride = -1; @@ -155,8 +156,8 @@ static float GForce, GForceAxis[XYZ_AXIS_COUNT]; typedef struct statistic_s { uint16_t max_speed; uint16_t min_voltage; // /100 - int16_t max_current; // /100 - int16_t max_power; // /100 + int16_t max_current; + int32_t max_power; int16_t min_rssi; int16_t min_lq; // for CRSF int16_t min_rssi_dbm; // for CRSF @@ -192,7 +193,7 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) -PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 15); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 0); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0); static int digitCount(int32_t value) @@ -312,11 +313,11 @@ static void osdLeftAlignString(char *buff) * prefixed by a a symbol to indicate the unit used. * @param dist Distance in centimeters */ -static void osdFormatDistanceSymbol(char *buff, int32_t dist) +static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) { switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_IMPERIAL: - if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, 0, 3, 3)) { + if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, decimals, 3, 3)) { buff[3] = SYM_DIST_MI; } else { buff[3] = SYM_DIST_FT; @@ -326,7 +327,7 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist) case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_METRIC: - if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, 0, 3, 3)) { + if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, decimals, 3, 3)) { buff[3] = SYM_DIST_KM; } else { buff[3] = SYM_DIST_M; @@ -533,7 +534,7 @@ static uint16_t osdConvertRSSI(void) return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); } -static uint16_t osdGetLQ(void) +static uint16_t osdGetCrsfLQ(void) { int16_t statsLQ = rxLinkStatistics.uplinkLQ; int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); @@ -544,7 +545,7 @@ static uint16_t osdGetLQ(void) } } -static uint16_t osdGetdBm(void) +static int16_t osdGetCrsfdBm(void) { return rxLinkStatistics.uplinkRSSI; } @@ -736,6 +737,8 @@ static const char * osdArmingDisabledReasonMessage(void) return OSD_MESSAGE_STR(OSD_MSG_PWM_INIT_ERROR); case ARMING_DISABLED_NO_PREARM: return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM); + case ARMING_DISABLED_DSHOT_BEEPER: + return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER); // Cases without message case ARMING_DISABLED_CMS_MENU: FALLTHROUGH; @@ -801,6 +804,15 @@ static const char * osdFailsafeInfoMessage(void) } return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST); } +#if defined(USE_SAFE_HOME) +static const char * divertingToSafehomeMessage(void) +{ + if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) { + return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); + } + return NULL; +} +#endif static const char * navigationStateMessage(void) { @@ -836,6 +848,11 @@ static const char * navigationStateMessage(void) return OSD_MESSAGE_STR(OSD_MSG_LANDING); case MW_NAV_STATE_HOVER_ABOVE_HOME: if (STATE(FIXED_WING_LEGACY)) { +#if defined(USE_SAFE_HOME) + if (safehome_applied) { + return OSD_MESSAGE_STR(OSD_MSG_LOITERING_SAFEHOME); + } +#endif return OSD_MESSAGE_STR(OSD_MSG_LOITERING_HOME); } return OSD_MESSAGE_STR(OSD_MSG_HOVERING); @@ -967,7 +984,7 @@ static bool osdIsHeadingValid(void) } else { return isImuHeadingValid(); } -#else +#else return isImuHeadingValid(); #endif } @@ -980,7 +997,7 @@ int16_t osdGetHeading(void) } else { return attitude.values.yaw; } -#else +#else return attitude.values.yaw; #endif } @@ -1166,6 +1183,67 @@ static void osdDrawRadar(uint16_t *drawn, uint32_t *usedScale) osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale); } +static uint16_t crc_accumulate(uint8_t data, uint16_t crcAccum) +{ + uint8_t tmp; + tmp = data ^ (uint8_t)(crcAccum & 0xff); + tmp ^= (tmp << 4); + crcAccum = (crcAccum >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4); + return crcAccum; +} + + +static void osdDisplayTelemetry(void) +{ + uint32_t trk_data; + uint16_t trk_crc = 0; + char trk_buffer[31]; + static int16_t trk_elevation = 127; + static uint16_t trk_bearing = 0; + + if (ARMING_FLAG(ARMED)) { + if (STATE(GPS_FIX)){ + if (GPS_distanceToHome > 5) { + trk_bearing = GPS_directionToHome; + trk_bearing += 360 + 180; + trk_bearing %= 360; + int32_t alt = CENTIMETERS_TO_METERS(osdGetAltitude()); + float at = atan2(alt, GPS_distanceToHome); + trk_elevation = (float)at * 57.2957795; // 57.2957795 = 1 rad + trk_elevation += 37; // because elevation in telemetry should be from -37 to 90 + if (trk_elevation < 0) { + trk_elevation = 0; + } + } + } + } + else{ + trk_elevation = 127; + trk_bearing = 0; + } + + trk_data = 0; // bit 0 - packet type 0 = bearing/elevation, 1 = 2 byte data packet + trk_data = trk_data | (uint32_t)(0x7F & trk_elevation) << 1; // bits 1-7 - elevation angle to target. NOTE number is abused. constrained value of -37 to 90 sent as 0 to 127. + trk_data = trk_data | (uint32_t)trk_bearing << 8; // bits 8-17 - bearing angle to target. 0 = true north. 0 to 360 + trk_crc = crc_accumulate(0xFF & trk_data, trk_crc); // CRC First Byte bits 0-7 + trk_crc = crc_accumulate(0xFF & trk_bearing, trk_crc); // CRC Second Byte bits 8-15 + trk_crc = crc_accumulate(trk_bearing >> 8, trk_crc); // CRC Third Byte bits 16-17 + trk_data = trk_data | (uint32_t)trk_crc << 17; // bits 18-29 CRC & 0x3FFFF + + for (uint8_t t_ctr = 0; t_ctr < 30; t_ctr++) { // Prepare screen buffer and write data line. + if (trk_data & (uint32_t)1 << t_ctr){ + trk_buffer[29 - t_ctr] = SYM_TELEMETRY_0; + } + else{ + trk_buffer[29 - t_ctr] = SYM_TELEMETRY_1; + } + } + trk_buffer[30] = 0; + displayWrite(osdDisplayPort, 0, 0, trk_buffer); + if (osdConfig()->telemetry>1){ + displayWrite(osdDisplayPort, 0, 3, trk_buffer); // Test display because normal telemetry line is not visible + } +} #endif static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) { @@ -1458,7 +1536,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_HOME_DIST: { buff[0] = SYM_HOME; - osdFormatDistanceSymbol(&buff[1], GPS_distanceToHome * 100); + osdFormatDistanceSymbol(&buff[1], GPS_distanceToHome * 100, 0); uint16_t dist_alarm = osdConfig()->dist_alarm; if (dist_alarm > 0 && GPS_distanceToHome > dist_alarm) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); @@ -1468,7 +1546,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_TRIP_DIST: buff[0] = SYM_TOTAL; - osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance()); + osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0); break; case OSD_HEADING: @@ -1586,6 +1664,19 @@ static bool osdDrawSingleElement(uint8_t item) break; } +#ifdef USE_RANGEFINDER + case OSD_RANGEFINDER: + { + int32_t range = rangefinderGetLatestRawAltitude(); + if (range < 0) { + buff[0] = '-'; + } else { + osdFormatDistanceSymbol(buff, range, 1); + } + } + break; +#endif + case OSD_ONTIME: { osdFormatOnTime(buff); @@ -1614,7 +1705,7 @@ static bool osdDrawSingleElement(uint8_t item) /*static int32_t updatedTimeSeconds = 0;*/ timeUs_t currentTimeUs = micros(); static int32_t timeSeconds = -1; - if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) { + if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { #ifdef USE_WIND_ESTIMATOR timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation); #else @@ -1645,7 +1736,7 @@ static bool osdDrawSingleElement(uint8_t item) static timeUs_t updatedTimestamp = 0; timeUs_t currentTimeUs = micros(); static int32_t distanceMeters = -1; - if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) { + if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { #ifdef USE_WIND_ESTIMATOR distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation); #else @@ -1665,7 +1756,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[5] = '\0'; TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else { - osdFormatDistanceSymbol(buff + 1, distanceMeters * 100); + osdFormatDistanceSymbol(buff + 1, distanceMeters * 100, 0); if (distanceMeters == 0) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } @@ -1680,7 +1771,7 @@ static bool osdDrawSingleElement(uint8_t item) else if (FLIGHT_MODE(MANUAL_MODE)) p = "MANU"; else if (FLIGHT_MODE(NAV_RTH_MODE)) - p = "RTH "; + p = isWaypointMissionRTHActive() ? "WRTH" : "RTH "; else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) p = "HOLD"; else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) @@ -1911,7 +2002,7 @@ static bool osdDrawSingleElement(uint8_t item) pitchAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.pitch); } else { rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); - pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); + pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); } #else rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); @@ -2144,8 +2235,8 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_POWER: { - osdFormatCentiNumber(buff, getPower(), 0, 2, 0, 3); - buff[3] = SYM_WATT; + bool kiloWatt = osdFormatCentiNumber(buff, getPower(), 1000, 2, 2, 3); + buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; buff[4] = '\0'; uint8_t current_alarm = osdConfig()->current_alarm; @@ -2232,7 +2323,7 @@ static bool osdDrawSingleElement(uint8_t item) if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, - 1, efficiencyTimeDelta * 1e-6f); + 1, US2S(efficiencyTimeDelta)); efficiencyUpdated = currentTimeUs; } else { @@ -2262,7 +2353,7 @@ static bool osdDrawSingleElement(uint8_t item) if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f, - 1, efficiencyTimeDelta * 1e-6f); + 1, US2S(efficiencyTimeDelta)); efficiencyUpdated = currentTimeUs; } else { @@ -2274,9 +2365,8 @@ static bool osdDrawSingleElement(uint8_t item) } else { buff[0] = buff[1] = buff[2] = '-'; } - buff[3] = SYM_WH_KM_0; - buff[4] = SYM_WH_KM_1; - buff[5] = '\0'; + buff[3] = SYM_WH_KM; + buff[4] = '\0'; break; } @@ -2662,8 +2752,11 @@ void osdDrawNextElement(void) elementIndex = osdIncElementIndex(elementIndex); } while(!osdDrawSingleElement(elementIndex) && index != elementIndex); - // Draw artificial horizon last + // Draw artificial horizon + tracking telemtry last osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); + if (osdConfig()->telemetry>0){ + osdDisplayTelemetry(); + } } PG_RESET_TEMPLATE(osdConfig_t, osdConfig, @@ -2744,7 +2837,8 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .force_grid = SETTING_OSD_FORCE_GRID_DEFAULT, - .stats_energy_unit = SETTING_OSD_STATS_ENERGY_UNIT_DEFAULT + .stats_energy_unit = SETTING_OSD_STATS_ENERGY_UNIT_DEFAULT, + .stats_min_voltage_unit = SETTING_OSD_STATS_MIN_VOLTAGE_UNIT_DEFAULT ); void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) @@ -2993,9 +3087,8 @@ static void osdCompleteAsyncInitialization(void) osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); } else strcpy(string_buffer, "---"); - string_buffer[3] = SYM_WH_KM_0; - string_buffer[4] = SYM_WH_KM_1; - string_buffer[5] = '\0'; + string_buffer[3] = SYM_WH_KM; + string_buffer[4] = '\0'; displayWrite(osdDisplayPort, STATS_VALUE_X_POS-3, y, string_buffer); } #endif // USE_ADC @@ -3038,7 +3131,7 @@ static void osdResetStats(void) static void osdUpdateStats(void) { - int16_t value; + int32_t value; if (feature(FEATURE_GPS)) { value = osdGet3DSpeed(); @@ -3053,11 +3146,11 @@ static void osdUpdateStats(void) if (stats.min_voltage > value) stats.min_voltage = value; - value = abs(getAmperage() / 100); + value = abs(getAmperage()); if (stats.max_current < value) stats.max_current = value; - value = abs(getPower() / 100); + value = labs(getPower()); if (stats.max_power < value) stats.max_power = value; @@ -3065,11 +3158,11 @@ static void osdUpdateStats(void) if (stats.min_rssi > value) stats.min_rssi = value; - value = osdGetLQ(); + value = osdGetCrsfLQ(); if (stats.min_lq > value) stats.min_lq = value; - value = osdGetdBm(); + value = osdGetCrsfdBm(); if (stats.min_rssi_dbm > value) stats.min_rssi_dbm = value; @@ -3088,7 +3181,7 @@ static void osdShowStatsPage1(void) displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); displayClearScreen(osdDisplayPort); - displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2"); + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->"); if (feature(FEATURE_GPS)) { displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); @@ -3109,22 +3202,24 @@ static void osdShowStatsPage1(void) osdFormatAltitudeStr(buff, stats.max_altitude); displayWrite(osdDisplayPort, statValuesX, top++, buff); -#if defined(USE_SERIALRX_CRSF) - displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); - itoa(stats.min_lq, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + switch (rxConfig()->serialrx_provider) { + case SERIALRX_CRSF: + displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); + itoa(stats.min_lq, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); - itoa(stats.min_rssi_dbm, buff, 10); - tfp_sprintf(buff, "%s%c", buff, SYM_DBM); - displayWrite(osdDisplayPort, statValuesX, top++, buff); -#else - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); - itoa(stats.min_rssi, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); -#endif + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); + itoa(stats.min_rssi_dbm, buff, 10); + tfp_sprintf(buff, "%s%c", buff, SYM_DBM); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + break; + default: + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); uint16_t flySeconds = getFlightTime(); @@ -3151,22 +3246,28 @@ static void osdShowStatsPage2(void) displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); displayClearScreen(osdDisplayPort); - displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 2/2"); + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- <- 2/2"); - displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); - osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); + if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) { + displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); + osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); + } else { + displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :"); + osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3); + } tfp_sprintf(buff, "%s%c", buff, SYM_VOLT); displayWrite(osdDisplayPort, statValuesX, top++, buff); if (feature(FEATURE_CURRENT_METER)) { displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); - itoa(stats.max_current, buff, 10); + osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3); tfp_sprintf(buff, "%s%c", buff, SYM_AMP); displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); - itoa(stats.max_power, buff, 10); - tfp_sprintf(buff, "%s%c", buff, SYM_WATT); + bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3); + buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; + buff[4] = '\0'; displayWrite(osdDisplayPort, statValuesX, top++, buff); if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { @@ -3187,9 +3288,8 @@ static void osdShowStatsPage2(void) SYM_MAH_KM_0, SYM_MAH_KM_1); else { osdFormatCentiNumber(buff, getMWhDrawn() * 10000 / totalDistance, 0, 2, 0, 3); - buff[3] = SYM_WH_KM_0; - buff[4] = SYM_WH_KM_1; - buff[5] = '\0'; + buff[3] = SYM_WH_KM; + buff[4] = '\0'; } // If traveled distance is less than 100 meters efficiency numbers are useless and unreliable so display --- instead if (totalDistance < 10000) { @@ -3199,9 +3299,8 @@ static void osdShowStatsPage2(void) buff[4] = SYM_MAH_KM_1; buff[5] = '\0'; } else { - buff[3] = SYM_WH_KM_0; - buff[4] = SYM_WH_KM_1; - buff[5] = '\0'; + buff[3] = SYM_WH_KM; + buff[4] = '\0'; } } displayWrite(osdDisplayPort, statValuesX, top++, buff); @@ -3264,13 +3363,17 @@ static void osdShowArmed(void) } y += 4; #if defined (USE_SAFE_HOME) - if (isSafeHomeInUse()) { - textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; - char buf2[12]; // format the distance first - osdFormatDistanceStr(buf2, safehome_distance); - tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_used); - // write this message above the ARMED message to make it obvious - displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr); + if (safehome_distance) { // safehome found during arming + if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { + strcpy(buf, "SAFEHOME FOUND; MODE OFF"); + } else { + char buf2[12]; // format the distance first + osdFormatDistanceStr(buf2, safehome_distance); + tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_index); + } + textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; + // write this message above the ARMED message to make it obvious + displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr); } #endif } else { @@ -3296,7 +3399,7 @@ static void osdShowArmed(void) static void osdFilterData(timeUs_t currentTimeUs) { static timeUs_t lastRefresh = 0; - float refresh_dT = cmpTimeUs(currentTimeUs, lastRefresh) * 1e-6; + float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh)); GForce = sqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS; for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS; @@ -3339,7 +3442,7 @@ static void osdRefresh(timeUs_t currentTimeUs) uint32_t delay = ARMED_SCREEN_DISPLAY_TIME; statsPagesCheck = 0; #if defined(USE_SAFE_HOME) - if (isSafeHomeInUse()) + if (safehome_distance) delay *= 3; #endif osdSetNextRefreshIn(delay); @@ -3541,6 +3644,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter const char *failsafePhaseMessage = osdFailsafePhaseMessage(); const char *failsafeInfoMessage = osdFailsafeInfoMessage(); const char *navStateFSMessage = navigationStateMessage(); + if (failsafePhaseMessage) { messages[messageCount++] = failsafePhaseMessage; } @@ -3550,6 +3654,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (navStateFSMessage) { messages[messageCount++] = navStateFSMessage; } +#if defined(USE_SAFE_HOME) + const char *safehomeMessage = divertingToSafehomeMessage(); + if (safehomeMessage) { + messages[messageCount++] = safehomeMessage; + } +#endif if (messageCount > 0) { message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; if (message == failsafeInfoMessage) { @@ -3566,6 +3676,11 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } } else { if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { + if (isWaypointMissionRTHActive()) { + // if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL); + } + if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) { // Countdown display for remaining Waypoints tfp_sprintf(messageBuf, "TO WP %u/%u", posControl.activeWaypointIndex + 1, posControl.waypointCount); @@ -3584,6 +3699,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = navStateMessage; } } +#if defined(USE_SAFE_HOME) + const char *safehomeMessage = divertingToSafehomeMessage(); + if (safehomeMessage) { + messages[messageCount++] = safehomeMessage; + } +#endif } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH); const char *launchStateMessage = fixedWingLaunchStateMessage(); diff --git a/src/main/io/osd.h b/src/main/io/osd.h old mode 100755 new mode 100644 index 1dc185b0d1..c9ad2eaea2 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -77,6 +77,7 @@ #define OSD_MSG_CLI_ACTIVE "CLI IS ACTIVE" #define OSD_MSG_PWM_INIT_ERROR "PWM INIT ERROR" #define OSD_MSG_NO_PREARM "NO PREARM" +#define OSD_MSG_DSHOT_BEEPER "MOTOR BEEPER ACTIVE" #define OSD_MSG_RTH_FS "(RTH)" #define OSD_MSG_EMERG_LANDING_FS "(EMERGENCY LANDING)" #define OSD_MSG_MOVE_EXIT_FS "!MOVE STICKS TO EXIT FS!" @@ -86,6 +87,7 @@ #define OSD_MSG_HOLDING_WAYPOINT "HOLDING WAYPOINT" #define OSD_MSG_TO_WP "TO WP" #define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT" +#define OSD_MSG_WP_RTH_CANCEL "CANCEL WP MODE TO EXIT RTH" #define OSD_MSG_EMERG_LANDING "EMERGENCY LANDING" #define OSD_MSG_LANDING "LANDING" #define OSD_MSG_LOITERING_HOME "LOITERING AROUND HOME" @@ -99,6 +101,11 @@ #define OSD_MSG_HEADFREE "(HEADFREE)" #define OSD_MSG_UNABLE_ARM "UNABLE TO ARM" +#if defined(USE_SAFE_HOME) +#define OSD_MSG_DIVERT_SAFEHOME "DIVERTING TO SAFEHOME" +#define OSD_MSG_LOITERING_SAFEHOME "LOITERING AROUND SAFEHOME" +#endif + typedef enum { OSD_RSSI_VALUE, OSD_MAIN_BATT_VOLTAGE, @@ -220,6 +227,7 @@ typedef enum { OSD_TPA, OSD_NAV_FW_CONTROL_SMOOTHNESS, OSD_VERSION, + OSD_RANGEFINDER, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -236,6 +244,11 @@ typedef enum { OSD_STATS_ENERGY_UNIT_WH, } osd_stats_energy_unit_e; +typedef enum { + OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY, + OSD_STATS_MIN_VOLTAGE_UNIT_CELL, +} osd_stats_min_voltage_unit_e; + typedef enum { OSD_CROSSHAIRS_STYLE_DEFAULT, OSD_CROSSHAIRS_STYLE_AIRCRAFT, @@ -332,6 +345,7 @@ typedef struct osdConfig_s { uint8_t units; // from osd_unit_e uint8_t stats_energy_unit; // from osd_stats_energy_unit_e + uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e #ifdef USE_WIND_ESTIMATOR bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance @@ -355,6 +369,7 @@ typedef struct osdConfig_s { uint8_t pan_servo_index; // Index of the pan servo used for home direction offset int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm uint8_t crsf_lq_format; + uint8_t telemetry; // use telemetry on displayed pixel line 0 } osdConfig_t; diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index ed72a973d3..96d345d0d3 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -112,7 +112,7 @@ }) -/* +/* * DJI HD goggles use MSPv1 compatible with Betaflight 4.1.0 * DJI uses a subset of messages and assume fixed bit positions for flight modes * @@ -505,6 +505,8 @@ static const char * osdArmingDisabledReasonMessage(void) return OSD_MESSAGE_STR("PWM ERR"); case ARMING_DISABLED_NO_PREARM: return OSD_MESSAGE_STR("NO PREARM"); + case ARMING_DISABLED_DSHOT_BEEPER: + return OSD_MESSAGE_STR("MOTOR BEEPER ACTIVE"); // Cases without message case ARMING_DISABLED_CMS_MENU: FALLTHROUGH; @@ -685,7 +687,7 @@ static void osdDJIFormatDistanceStr(char *buff, int32_t dist) if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { // Show feet when dist < 0.5mi tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT"); - } + } else { // Show miles when dist >= 0.5mi tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100*FEET_PER_MILE)), @@ -722,7 +724,7 @@ static void osdDJIEfficiencyMahPerKM(char *buff) if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, - 1, efficiencyTimeDelta * 1e-6f); + 1, US2S(efficiencyTimeDelta)); efficiencyUpdated = currentTimeUs; } @@ -755,7 +757,7 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name) if (enabledElements[0] == 'W') { enabledElements += 1; } - + int elemLen = strlen(enabledElements); if (elemLen > 0) { @@ -825,14 +827,14 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name) // during a lost aircraft recovery and blinking // will cause it to be missing from some frames. } - } + } else { if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { const char *navStateMessage = navigationStateMessage(); if (navStateMessage) { messages[messageCount++] = navStateMessage; } - } + } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { messages[messageCount++] = "AUTOLAUNCH"; } @@ -994,7 +996,7 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms for (int i = 0; i < STICK_CHANNEL_COUNT; i++) { sbufWriteU16(dst, rxGetChannelValue(i)); } - break; + break; case DJI_MSP_RAW_GPS: sbufWriteU8(dst, gpsSol.fixType); diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 7b4f05c76b..3c76e160b3 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -44,7 +44,7 @@ typedef enum { FUNCTION_RCDEVICE = (1 << 10), // 1024 FUNCTION_VTX_SMARTAUDIO = (1 << 11), // 2048 FUNCTION_VTX_TRAMP = (1 << 12), // 4096 - FUNCTION_UNUSED_1 = (1 << 13), // 8192: former UAV_INTERCONNECT + FUNCTION_UNUSED_1 = (1 << 13), // 8192: former\ UAV_INTERCONNECT FUNCTION_OPTICAL_FLOW = (1 << 14), // 16384 FUNCTION_LOG = (1 << 15), // 32768 FUNCTION_RANGEFINDER = (1 << 16), // 65536 @@ -55,6 +55,7 @@ typedef enum { FUNCTION_DJI_HD_OSD = (1 << 21), // 2097152 FUNCTION_SERVO_SERIAL = (1 << 22), // 4194304 FUNCTION_TELEMETRY_SMARTPORT_MASTER = (1 << 23), // 8388608 + FUNCTION_IMU2 = (1 << 24), // 16777216 } serialPortFunction_e; typedef enum { diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index a45f41c8bc..0119e61e75 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -427,11 +427,11 @@ void esc4wayProcess(serialPort_t *mspPort) port = mspPort; // Start here with UART Main loop - #ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) // fix for buzzer often starts beeping continuously when the ESCs are read // switch beeper silent here beeperSilence(); - #endif +#endif bool isExitScheduled = false; while (1) { diff --git a/src/main/io/smartport_master.c b/src/main/io/smartport_master.c index ade2b00acc..60b9a93e26 100644 --- a/src/main/io/smartport_master.c +++ b/src/main/io/smartport_master.c @@ -550,7 +550,7 @@ void smartportMasterHandle(timeUs_t currentTimeUs) return; } - if (!pollTimestamp || (cmpTimeUs(currentTimeUs, pollTimestamp) > SMARTPORT_POLLING_INTERVAL * 1000)) { + if (!pollTimestamp || (cmpTimeUs(currentTimeUs, pollTimestamp) > MS2US(SMARTPORT_POLLING_INTERVAL))) { if (forwardRequestCount() && (forcedPolledPhyID == -1)) { // forward next payload if there is one in queue and we are not waiting from the response of the previous one smartportMasterForwardNextPayload(); } else { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c old mode 100755 new mode 100644 index c953686269..94db2be867 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -74,10 +74,14 @@ gpsLocation_t GPS_home; uint32_t GPS_distanceToHome; // distance to home point in meters int16_t GPS_directionToHome; // direction to home point in degrees +fpVector3_t original_rth_home; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET + radar_pois_t radar_pois[RADAR_MAX_POIS]; #if defined(USE_SAFE_HOME) -int8_t safehome_used; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise -uint32_t safehome_distance; // distance to the selected safehome +int8_t safehome_index = -1; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise +uint32_t safehome_distance = 0; // distance to the nearest safehome +fpVector3_t nearestSafeHome; // The nearestSafeHome found during arming +bool safehome_applied = false; // whether the safehome has been applied to home. PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0); @@ -92,7 +96,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 10); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 11); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -107,7 +111,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .rth_tail_first = SETTING_NAV_RTH_TAIL_FIRST_DEFAULT, .disarm_on_landing = SETTING_NAV_DISARM_ON_LANDING_DEFAULT, .rth_allow_landing = SETTING_NAV_RTH_ALLOW_LANDING_DEFAULT, + .rth_alt_control_override = SETTING_NAV_RTH_ALT_CONTROL_OVERRIDE_DEFAULT, // Override RTH Altitude and Climb First using Pitch and Roll stick .nav_overrides_motor_stop = SETTING_NAV_OVERRIDES_MOTOR_STOP_DEFAULT, + .safehome_usage_mode = SETTING_SAFEHOME_USAGE_MODE_DEFAULT, }, // General navigation parameters @@ -118,9 +124,10 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT, .max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT, - .land_descent_rate = SETTING_NAV_LANDING_SPEED_DEFAULT, // centimeters/s .land_slowdown_minalt = SETTING_NAV_LAND_SLOWDOWN_MINALT_DEFAULT, // altitude in centimeters .land_slowdown_maxalt = SETTING_NAV_LAND_SLOWDOWN_MAXALT_DEFAULT, // altitude in meters + .land_minalt_vspd = SETTING_NAV_LAND_MINALT_VSPD_DEFAULT, // centimeters/s + .land_maxalt_vspd = SETTING_NAV_LAND_MAXALT_VSPD_DEFAULT, // centimeters/s .emerg_descent_rate = SETTING_NAV_EMERG_LANDING_SPEED_DEFAULT, // centimeters/s .min_rth_distance = SETTING_NAV_MIN_RTH_DISTANCE_DEFAULT, // centimeters, if closer than this land immediately .rth_altitude = SETTING_NAV_RTH_ALTITUDE_DEFAULT, // altitude in centimeters @@ -231,9 +238,13 @@ void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distanc static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome); static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint); static navigationFSMEvent_t nextForNonGeoStates(void); +static bool isWaypointMissionValid(void); void initializeRTHSanityChecker(const fpVector3_t * pos); bool validateRTHSanityChecker(void); +void updateHomePosition(void); + +static bool rthAltControlStickOverrideCheck(unsigned axis); /*************************************************************************************************/ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState); @@ -263,7 +274,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState); @@ -655,11 +665,12 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_PROCESS_NEXT, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP) - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP) + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, } }, @@ -693,19 +704,18 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_PROCESS_NEXT, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME] = NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -784,27 +794,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, - [NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME] = { - .persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOVER_ABOVE_HOME, - .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, - .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, - .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, - .mwState = MW_NAV_STATE_HOVER_ABOVE_HOME, - .mwError = MW_NAV_ERROR_NONE, - .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, // re-process state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_FINISHED, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, - } - }, - /** EMERGENCY LANDING ************************************************/ [NAV_STATE_EMERGENCY_LANDING_INITIALIZE] = { .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE, @@ -1156,8 +1145,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati fpVector3_t targetHoldPos; if (STATE(FIXED_WING_LEGACY)) { - // Airplane - climbout before turning around - calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away + // Airplane - climbout before heading home + if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) { + // Spiral climb centered at xy of RTH activation + calculateInitialHoldPosition(&targetHoldPos); + } else { + calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away Linear climb + } } else { // Multicopter, hover and climb calculateInitialHoldPosition(&targetHoldPos); @@ -1186,6 +1180,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n { UNUSED(previousState); + rthAltControlStickOverrideCheck(PITCH); + if ((posControl.flags.estHeadingStatus == EST_NONE)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -1201,7 +1197,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z)); // If we reached desired initial RTH altitude or we don't want to climb first - if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) { + if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == OFF) || rthAltControlStickOverrideCheck(ROLL)) { // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance if (STATE(FIXED_WING_LEGACY)) { @@ -1232,8 +1228,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n // Climb to safe altitude and turn to correct direction if (STATE(FIXED_WING_LEGACY)) { - tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM; - setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); + if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) { + float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; + updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); + } else { + tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM; + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); + } } else { // Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach // it in a reasonable time. Immediately after we finish this phase - target the original altitude. @@ -1260,6 +1261,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio { UNUSED(previousState); + rthAltControlStickOverrideCheck(PITCH); + if ((posControl.flags.estHeadingStatus == EST_NONE)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -1307,30 +1310,21 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND // If position ok OR within valid timeout - continue if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) { - - // Wait until target heading is reached (with 15 deg margin for error) - if (STATE(FIXED_WING_LEGACY)) { + // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing + if ((ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) { resetLandingDetector(); updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); - return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; + return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land + } + else if (!validateRTHSanityChecker()) { + // Continue to check for RTH sanity during pre-landing hover + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } else { - if (ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) { - resetLandingDetector(); - updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); - return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; - } - else if (!validateRTHSanityChecker()) { - // Continue to check for RTH sanity during pre-landing hover - return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; - } - else { - fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); - setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); - return NAV_FSM_EVENT_NONE; - } + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); + setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return NAV_FSM_EVENT_NONE; } - } else { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -1390,22 +1384,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) { // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown // Do not allow descent velocity slower than -30cm/s so the landing detector works. - descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f); + descentVelLimited = navConfig()->general.land_minalt_vspd; } else { - fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND); // Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm. - float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - tmpHomePos->z - navConfig()->general.land_slowdown_minalt) - / (navConfig()->general.land_slowdown_maxalt - navConfig()->general.land_slowdown_minalt) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt + float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z, + navConfig()->general.land_slowdown_minalt, navConfig()->general.land_slowdown_maxalt, + navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd); - descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f); + descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd); - // Do not allow descent velocity slower than -50cm/s so the landing detector works. - descentVelLimited = MIN(-descentVelScaling * navConfig()->general.land_descent_rate, -50.0f); } - updateClimbRateToAltitudeController(descentVelLimited, ROC_TO_ALT_NORMAL); + updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL); return NAV_FSM_EVENT_NONE; } @@ -1429,7 +1421,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation UNUSED(previousState); if (STATE(ALTITUDE_CONTROL)) { - updateClimbRateToAltitudeController(-0.3f * navConfig()->general.land_descent_rate, ROC_TO_ALT_NORMAL); // FIXME + updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, ROC_TO_ALT_NORMAL); // FIXME } // Prevent I-terms growing when already landed @@ -1457,6 +1449,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav */ setupJumpCounters(); posControl.activeWaypointIndex = 0; + wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE; return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION } } @@ -1526,10 +1519,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav return nextForNonGeoStates(); case NAV_WP_ACTION_RTH: - posControl.rthState.rthInitialDistance = posControl.homeDistance; - initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos); - calculateAndSetActiveWaypointToLocalPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL)); - return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS + return NAV_FSM_EVENT_SWITCH_TO_RTH; }; UNREACHABLE(); @@ -1575,21 +1565,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na case NAV_WP_ACTION_JUMP: case NAV_WP_ACTION_SET_HEAD: case NAV_WP_ACTION_SET_POI: - UNREACHABLE(); - case NAV_WP_ACTION_RTH: - if (isWaypointReached(&posControl.activeWaypoint, true) || isWaypointMissed(&posControl.activeWaypoint)) { - return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED - } - else { - if(navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) - setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING_TAIL_FIRST); - else - setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); - setDesiredPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL), 0, NAV_POS_UPDATE_Z); - return NAV_FSM_EVENT_NONE; // will re-process state in >10ms - } - break; + UNREACHABLE(); } } /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */ @@ -1614,15 +1591,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga case NAV_WP_ACTION_JUMP: case NAV_WP_ACTION_SET_HEAD: case NAV_WP_ACTION_SET_POI: - UNREACHABLE(); - case NAV_WP_ACTION_RTH: - if (posControl.waypointList[posControl.activeWaypointIndex].p1 != 0) { - return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND; - } - else { - return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME; - } + UNREACHABLE(); case NAV_WP_ACTION_LAND: return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND; @@ -1702,14 +1672,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig } } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME(navigationFSMState_t previousState) -{ - UNUSED(previousState); - - const navigationFSMEvent_t hoverEvent = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(previousState); - return hoverEvent; -} - static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState) { // TODO: @@ -2333,26 +2295,42 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void) #if defined(USE_SAFE_HOME) -/******************************************************* - * Is a safehome being used instead of the arming point? - *******************************************************/ -bool isSafeHomeInUse(void) +void checkSafeHomeState(bool shouldBeEnabled) { - return (safehome_used > -1 && safehome_used < MAX_SAFE_HOMES); + if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { + shouldBeEnabled = false; + } else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) { + // if safehomes are only used with failsafe and we're trying to enable safehome + // then enable the safehome only with failsafe + shouldBeEnabled = posControl.flags.forcedRTHActivated; + } + // no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything + if (safehome_distance == 0 || (safehome_applied == shouldBeEnabled)) { + return; + } + if (shouldBeEnabled) { + // set home to safehome + setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + safehome_applied = true; + } else { + // set home to original arming point + setHomePosition(&original_rth_home, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + safehome_applied = false; + } + // if we've changed the home position, update the distance and direction + updateHomePosition(); } /*********************************************************** * See if there are any safehomes near where we are arming. - * If so, use it instead of the arming point for home. - * Select the nearest safehome + * If so, save the nearest one in case we need it later for RTH. **********************************************************/ -bool foundNearbySafeHome(void) +bool findNearestSafeHome(void) { - safehome_used = -1; + safehome_index = -1; uint32_t nearest_safehome_distance = navConfig()->general.safehome_max_distance + 1; uint32_t distance_to_current; fpVector3_t currentSafeHome; - fpVector3_t nearestSafeHome; gpsLocation_t shLLH; shLLH.alt = 0; for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) { @@ -2365,20 +2343,19 @@ bool foundNearbySafeHome(void) distance_to_current = calculateDistanceToDestination(¤tSafeHome); if (distance_to_current < nearest_safehome_distance) { // this safehome is the nearest so far - keep track of it. - safehome_used = i; + safehome_index = i; nearest_safehome_distance = distance_to_current; nearestSafeHome.x = currentSafeHome.x; nearestSafeHome.y = currentSafeHome.y; nearestSafeHome.z = currentSafeHome.z; } } - if (safehome_used >= 0) { + if (safehome_index >= 0) { safehome_distance = nearest_safehome_distance; - setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); - return true; + } else { + safehome_distance = 0; } - safehome_distance = 0; - return false; + return safehome_distance > 0; } #endif @@ -2404,9 +2381,13 @@ void updateHomePosition(void) } if (setHome) { #if defined(USE_SAFE_HOME) - if (!foundNearbySafeHome()) + findNearestSafeHome(); #endif - setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + // save the current location in case it is replaced by a safehome or HOME_RESET + original_rth_home.x = posControl.rthState.homePosition.pos.x; + original_rth_home.y = posControl.rthState.homePosition.pos.y; + original_rth_home.z = posControl.rthState.homePosition.pos.z; } } } @@ -2435,6 +2416,39 @@ void updateHomePosition(void) } } +/* ----------------------------------------------------------- + * Override RTH preset altitude and Climb First option + * using Pitch/Roll stick held for > 1 seconds + * Climb First override limited to Fixed Wing only + *-----------------------------------------------------------*/ +static bool rthAltControlStickOverrideCheck(unsigned axis) +{ + if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated || (axis == ROLL && STATE(MULTIROTOR))) { + return false; + } + static timeMs_t rthOverrideStickHoldStartTime[2]; + + if (rxGetChannelValue(axis) > rxConfig()->maxcheck) { + timeDelta_t holdTime = millis() - rthOverrideStickHoldStartTime[axis]; + + if (!rthOverrideStickHoldStartTime[axis]) { + rthOverrideStickHoldStartTime[axis] = millis(); + } else if (ABS(1500 - holdTime) < 500) { // 1s delay to activate, activation duration limited to 1 sec + if (axis == PITCH) { // PITCH down to override preset altitude reset to current altitude + posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; + posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; + return true; + } else if (axis == ROLL) { // ROLL right to override climb first + return true; + } + } + } else { + rthOverrideStickHoldStartTime[axis] = 0; + } + + return false; +} + /*----------------------------------------------------------- * Update flight statistics *-----------------------------------------------------------*/ @@ -3115,6 +3129,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) const bool canActivatePosHold = canActivatePosHoldMode(); const bool canActivateNavigation = canActivateNavigationModes(); const bool isExecutingRTH = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH; + checkSafeHomeState(isExecutingRTH || posControl.flags.forcedRTHActivated); // Keep canActivateWaypoint flag at FALSE if there is no mission loaded // Also block WP mission if we are executing RTH @@ -3551,6 +3566,7 @@ void activateForcedRTH(void) { abortFixedWingLaunch(); posControl.flags.forcedRTHActivated = true; + checkSafeHomeState(true); navProcessFSMEvents(selectNavEventFromBoxModeInput()); } @@ -3559,6 +3575,7 @@ void abortForcedRTH(void) // Disable failsafe RTH and make sure we back out of navigation mode to IDLE // If any navigation mode was active prior to RTH it will be re-enabled with next RX update posControl.flags.forcedRTHActivated = false; + checkSafeHomeState(false); navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE); } @@ -3578,6 +3595,11 @@ rthState_e getStateOfForcedRTH(void) } } +bool isWaypointMissionRTHActive(void) +{ + return FLIGHT_MODE(NAV_RTH_MODE) && IS_RC_MODE_ACTIVE(BOXNAVWP) && !(IS_RC_MODE_ACTIVE(BOXNAVRTH) || posControl.flags.forcedRTHActivated); +} + bool navigationIsExecutingAnEmergencyLanding(void) { return navGetCurrentStateFlags() & NAV_CTL_EMERG; @@ -3603,9 +3625,12 @@ bool navigationIsFlyingAutonomousMode(void) bool navigationRTHAllowsLanding(void) { - if (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND) - return true; + // WP mission RTH landing setting + if (isWaypointMissionRTHActive() && isWaypointMissionValid()) { + return posControl.waypointList[posControl.waypointCount - 1].p1 > 0; + } + // normal RTH landing setting navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing; return allow == NAV_RTH_ALLOW_LANDING_ALWAYS || (allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE)); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 9fd15582dd..0638e6e844 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -35,6 +35,7 @@ extern gpsLocation_t GPS_home; extern uint32_t GPS_distanceToHome; // distance to home point in meters extern int16_t GPS_directionToHome; // direction to home point in degrees +extern fpVector3_t original_rth_home; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET extern bool autoThrottleManuallyIncreased; @@ -50,14 +51,20 @@ typedef struct { int32_t lon; } navSafeHome_t; +typedef enum { + SAFEHOME_USAGE_OFF = 0, // Don't use safehomes + SAFEHOME_USAGE_RTH = 1, // Default - use safehome for RTH + SAFEHOME_USAGE_RTH_FS = 2, // Use safehomes for RX failsafe only +} safehomeUsageMode_e; + PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig); -extern int8_t safehome_used; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise -extern uint32_t safehome_distance; // distance to the selected safehome +extern int8_t safehome_index; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise +extern uint32_t safehome_distance; // distance to the nearest safehome +extern bool safehome_applied; // whether the safehome has been applied to home. void resetSafeHomes(void); // remove all safehomes -bool isSafeHomeInUse(void); // Are we using a safehome instead of the arming point? -bool foundNearbySafeHome(void); // Did we find a safehome nearby? +bool findNearestSafeHome(void); // Find nearest safehome #endif // defined(USE_SAFE_HOME) @@ -132,6 +139,12 @@ typedef enum { NOMS_ALL_NAV } navOverridesMotorStop_e; +typedef enum { + OFF, + ON, + ON_FW_SPIRAL, +} navRTHClimbFirst_e; + typedef struct positionEstimationConfig_s { uint8_t automatic_mag_declination; uint8_t reset_altitude_type; // from nav_reset_type_e @@ -183,7 +196,9 @@ typedef struct navConfig_s { uint8_t disarm_on_landing; // uint8_t rth_allow_landing; // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e. uint8_t rth_climb_ignore_emerg; // Option to ignore GPS loss on initial climb stage of RTH + uint8_t rth_alt_control_override; // Override RTH Altitude and Climb First settings using Pitch and Roll stick uint8_t nav_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor + uint8_t safehome_usage_mode; // Controls when safehomes are used } flags; uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable) @@ -193,7 +208,8 @@ typedef struct navConfig_s { uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec uint16_t max_manual_speed; // manual velocity control max horizontal speed uint16_t max_manual_climb_rate; // manual velocity control max vertical speed - uint16_t land_descent_rate; // normal RTH landing descent rate + uint16_t land_minalt_vspd; // Final RTH landing descent rate under minalt + uint16_t land_maxalt_vspd; // RTH landing descent rate target at maxalt uint16_t land_slowdown_minalt; // Altitude to stop lowering descent rate during RTH descend uint16_t land_slowdown_maxalt; // Altitude to start lowering descent rate during RTH descend uint16_t emerg_descent_rate; // emergency landing descent rate @@ -504,6 +520,7 @@ bool navigationIsExecutingAnEmergencyLanding(void); * or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active. */ bool navigationRTHAllowsLanding(void); +bool isWaypointMissionRTHActive(void); bool isNavLaunchEnabled(void); bool isFixedWingLaunchDetected(void); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 40f2f49b0b..4ec12bc690 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -171,11 +171,11 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs) if ((posControl.flags.estAltStatus >= EST_USABLE)) { if (posControl.flags.verticalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - // Check if last correction was too log ago - ignore this update - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + // Check if last correction was not too long ago + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { updateAltitudeVelocityAndPitchController_FW(deltaMicrosPositionUpdate); } else { @@ -401,10 +401,10 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs) if ((posControl.flags.estPosStatus >= EST_USABLE)) { // If we have new position - update velocity and acceleration controllers if (posControl.flags.horizontalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { // Calculate virtual position target at a distance of forwardVelocity * HZ2S(POSITION_TARGET_UPDATE_RATE_HZ) // Account for pilot's roll input (move position target left/right at max of max_manual_speed) // POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs @@ -440,10 +440,10 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) if ((posControl.flags.estPosStatus >= EST_USABLE)) { // If we have new position - update velocity and acceleration controllers if (posControl.flags.horizontalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { float velThrottleBoost = (NAV_FW_MIN_VEL_SPEED_BOOST - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate); // If we are in the deadband of 50cm/s - don't update speed boost @@ -473,7 +473,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs) { static timeUs_t previousTimePitchToThrCorr = 0; - const timeDelta_t deltaMicrosPitchToThrCorr = currentTimeUs - previousTimePitchToThrCorr; + const timeDeltaLarge_t deltaMicrosPitchToThrCorr = currentTimeUs - previousTimePitchToThrCorr; previousTimePitchToThrCorr = currentTimeUs; static pt1Filter_t pitchToThrFilterState; diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 66ee070c1f..086d9a378d 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -391,10 +391,6 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state } - if (isLaunchMaxAltitudeReached()) { - return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state - } - if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) { return FW_LAUNCH_EVENT_SUCCESS; // launch timeout. go to FW_LAUNCH_STATE_FINISH } @@ -456,7 +452,7 @@ void resetFixedWingLaunchController(timeUs_t currentTimeUs) bool isFixedWingLaunchDetected(void) { - return fwLaunch.currentState == FW_LAUNCH_STATE_DETECTED; + return fwLaunch.currentState >= FW_LAUNCH_STATE_DETECTED; } void enableFixedWingLaunchController(timeUs_t currentTimeUs) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index a0beb4149c..debec2916c 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -209,11 +209,11 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs) // If we have an update on vertical position data - update velocity and accel targets if (posControl.flags.verticalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - // Check if last correction was too log ago - ignore this update - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + // Check if last correction was not too long ago + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { // If we are preparing for takeoff - start with lowset possible climb rate, adjust alt target and make sure throttle doesn't jump if (prepareForTakeoffOnReset) { const navEstimatedPosVel_t * posToUse = navGetCurrentActualPositionAndVelocity(); @@ -477,8 +477,8 @@ static float computeNormalizedVelocity(const float value, const float maxValue) } static float computeVelocityScale( - const float value, - const float maxValue, + const float value, + const float maxValue, const float attenuationFactor, const float attenuationStart, const float attenuationEnd @@ -491,7 +491,7 @@ static float computeVelocityScale( } static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit, const float maxSpeed) -{ +{ const float measurementX = navGetCurrentActualPositionAndVelocity()->vel.x; const float measurementY = navGetCurrentActualPositionAndVelocity()->vel.y; @@ -539,15 +539,15 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA * Scale down dTerm with 2D speed */ const float setpointScale = computeVelocityScale( - setpointXY, - maxSpeed, + setpointXY, + maxSpeed, multicopterPosXyCoefficients.dTermAttenuation, multicopterPosXyCoefficients.dTermAttenuationStart, multicopterPosXyCoefficients.dTermAttenuationEnd ); const float measurementScale = computeVelocityScale( - posControl.actualState.velXY, - maxSpeed, + posControl.actualState.velXY, + maxSpeed, multicopterPosXyCoefficients.dTermAttenuation, multicopterPosXyCoefficients.dTermAttenuationStart, multicopterPosXyCoefficients.dTermAttenuationEnd @@ -560,23 +560,23 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA // Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit // Thus we don't need to do anything else with calculated acceleration float newAccelX = navPidApply3( - &posControl.pids.vel[X], - setpointX, - measurementX, - US2S(deltaMicros), - accelLimitXMin, - accelLimitXMax, + &posControl.pids.vel[X], + setpointX, + measurementX, + US2S(deltaMicros), + accelLimitXMin, + accelLimitXMax, 0, // Flags 1.0f, // Total gain scale dtermScale // Additional dTerm scale ); float newAccelY = navPidApply3( - &posControl.pids.vel[Y], - setpointY, - measurementY, - US2S(deltaMicros), - accelLimitYMin, - accelLimitYMax, + &posControl.pids.vel[Y], + setpointY, + measurementY, + US2S(deltaMicros), + accelLimitYMin, + accelLimitYMax, 0, // Flags 1.0f, // Total gain scale dtermScale // Additional dTerm scale @@ -638,14 +638,14 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs) if ((posControl.flags.estPosStatus >= EST_USABLE)) { // If we have new position - update velocity and acceleration controllers if (posControl.flags.horizontalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; if (!bypassPositionController) { // Update position controller - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { // Get max speed from generic NAV (waypoint specific), don't allow to move slower than 0.5 m/s - const float maxSpeed = getActiveWaypointSpeed(); + const float maxSpeed = getActiveWaypointSpeed(); updatePositionVelocityController_MC(maxSpeed); updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed); } @@ -761,11 +761,11 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) // Normal sensor data if (posControl.flags.verticalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - // Check if last correction was too log ago - ignore this update - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + // Check if last correction was not too long ago + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL); updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate); updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 9848dfefbe..b26299e707 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -37,6 +37,9 @@ #define INAV_SURFACE_MAX_DISTANCE 40 +#define MAX_POSITION_UPDATE_INTERVAL_US HZ2US(MIN_POSITION_UPDATE_RATE_HZ) // convenience macro +_Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!"); + typedef enum { NAV_POS_UPDATE_NONE = 0, NAV_POS_UPDATE_Z = 1 << 1, @@ -138,7 +141,6 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_3, - NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME, NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD, NAV_FSM_EVENT_SWITCH_TO_CRUISE, @@ -197,7 +199,7 @@ typedef enum { NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35, NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 36, - NAV_PERSISTENT_ID_WAYPOINT_HOVER_ABOVE_HOME = 37, + NAV_PERSISTENT_ID_UNUSED_4 = 37, // was NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME } navigationPersistentId_e; @@ -229,7 +231,6 @@ typedef enum { NAV_STATE_WAYPOINT_NEXT, NAV_STATE_WAYPOINT_FINISHED, NAV_STATE_WAYPOINT_RTH_LAND, - NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, NAV_STATE_EMERGENCY_LANDING_INITIALIZE, NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, diff --git a/src/main/navigation/navigation_rover_boat.c b/src/main/navigation/navigation_rover_boat.c index 1f0a01669a..40c075107b 100644 --- a/src/main/navigation/navigation_rover_boat.c +++ b/src/main/navigation/navigation_rover_boat.c @@ -71,11 +71,11 @@ void applyRoverBoatPositionController(timeUs_t currentTimeUs) static timeUs_t previousTimePositionUpdate; // Occurs @ GPS update rate static timeUs_t previousTimeUpdate; // Occurs @ looptime rate - const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate; + const timeDeltaLarge_t deltaMicros = currentTimeUs - previousTimeUpdate; previousTimeUpdate = currentTimeUs; // If last position update was too long in the past - ignore it (likely restarting altitude controller) - if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + if (deltaMicros > MAX_POSITION_UPDATE_INTERVAL_US) { previousTimeUpdate = currentTimeUs; previousTimePositionUpdate = currentTimeUs; resetFixedWingPositionController(); @@ -86,10 +86,10 @@ void applyRoverBoatPositionController(timeUs_t currentTimeUs) if ((posControl.flags.estPosStatus >= EST_USABLE)) { // If we have new position - update velocity and acceleration controllers if (posControl.flags.horizontalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { update2DPositionHeadingController(currentTimeUs, deltaMicrosPositionUpdate); } else { resetFixedWingPositionController(); @@ -143,4 +143,4 @@ void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, } } -#endif \ No newline at end of file +#endif diff --git a/src/main/rx/fport2.c b/src/main/rx/fport2.c index de2b7d7062..55d4f4349e 100644 --- a/src/main/rx/fport2.c +++ b/src/main/rx/fport2.c @@ -531,7 +531,7 @@ static uint8_t frameStatus(rxRuntimeConfig_t *rxRuntimeConfig) } #endif - if (frameReceivedTimestamp && (cmpTimeUs(currentTimeUs, frameReceivedTimestamp) > FPORT2_MAX_TELEMETRY_AGE_MS * 1000)) { + if (frameReceivedTimestamp && (cmpTimeUs(currentTimeUs, frameReceivedTimestamp) > MS2US(FPORT2_MAX_TELEMETRY_AGE_MS))) { lqTrackerSet(rxRuntimeConfig->lqTracker, 0); frameReceivedTimestamp = 0; } @@ -593,7 +593,7 @@ static bool processFrame(const rxRuntimeConfig_t *rxRuntimeConfig) } - timeUs_t otaResponseTime = cmpTimeUs(micros(), otaFrameEndTimestamp); + timeDelta_t otaResponseTime = cmpTimeUs(micros(), otaFrameEndTimestamp); if (!firmwareUpdateError && (otaResponseTime <= otaMaxResponseTime)) { // We can answer in time (firmwareUpdateStore can take time because it might need to erase flash) writeUplinkFramePhyID(downlinkPhyID, otaResponsePayload); DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT2_OTA_FRAME_RESPONSE_TIME, otaResponseTime); diff --git a/src/main/rx/ghst.c b/src/main/rx/ghst.c index 54d3b6f0f5..4c620de3e5 100644 --- a/src/main/rx/ghst.c +++ b/src/main/rx/ghst.c @@ -119,7 +119,7 @@ void ghstRxWriteTelemetryData(const void *data, int len) } void ghstRxSendTelemetryData(void) -{ +{ // if there is telemetry data to write if (telemetryBufLen > 0) { serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen); @@ -178,7 +178,7 @@ STATIC_UNIT_TESTED void ghstDataReceive(uint16_t c, void *data) static bool shouldSendTelemetryFrame(void) { const timeUs_t now = micros(); - const timeUs_t timeSinceRxFrameEndUs = cmpTimeUs(now, ghstRxFrameEndAtUs); + const timeDelta_t timeSinceRxFrameEndUs = cmpTimeUs(now, ghstRxFrameEndAtUs); return telemetryBufLen > 0 && timeSinceRxFrameEndUs > GHST_RX_TO_TELEMETRY_MIN_US && timeSinceRxFrameEndUs < GHST_RX_TO_TELEMETRY_MAX_US; } @@ -191,7 +191,7 @@ static void ghstIdle(void) static void ghstUpdateFailsafe(unsigned pktIdx) { - // pktIdx is an offset of RC channel packet, + // pktIdx is an offset of RC channel packet, // We'll track arrival time of each of the frame types we ever saw arriving from this receiver if (pktIdx < GHST_UL_RC_CHANS_FRAME_COUNT) { if (ghstFsTracker[pktIdx].onTimePacketCounter < GHST_RC_FRAME_COUNT_THRESHOLD) { @@ -282,7 +282,7 @@ static bool ghstProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig) int startIdx = 0; if ( - ghstValidatedFrame.frame.type >= GHST_UL_RC_CHANS_HS4_FIRST && + ghstValidatedFrame.frame.type >= GHST_UL_RC_CHANS_HS4_FIRST && ghstValidatedFrame.frame.type <= GHST_UL_RC_CHANS_HS4_LAST ) { const ghstPayloadPulses_t* const rcChannels = (ghstPayloadPulses_t*)&ghstValidatedFrame.frame.payload; @@ -300,7 +300,7 @@ static bool ghstProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig) case GHST_UL_RC_CHANS_HS4_RSSI: { const ghstPayloadPulsesRSSI_t* const rssiFrame = (ghstPayloadPulsesRSSI_t*)&ghstValidatedFrame.frame.payload; lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(constrain(rssiFrame->lq, 0, 100), 0, 100, 0, RSSI_MAX_VALUE)); - + break; } diff --git a/src/main/rx/mavlink.c b/src/main/rx/mavlink.c new file mode 100644 index 0000000000..8b0c46a0f3 --- /dev/null +++ b/src/main/rx/mavlink.c @@ -0,0 +1,96 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" +FILE_COMPILE_FOR_SPEED +#ifdef USE_SERIALRX_MAVLINK + +#include "build/debug.h" + +#include "common/utils.h" + +#include "rx/rx.h" +#include "rx/mavlink.h" + +#define MAVLINK_CHANNEL_COUNT 18 +static uint16_t mavlinkChannelData[MAVLINK_CHANNEL_COUNT]; +static bool frameReceived; + +void mavlinkRxHandleMessage(const mavlink_rc_channels_override_t *msg) { + if (msg->chan1_raw != 0 && msg->chan1_raw != UINT16_MAX) mavlinkChannelData[0] = msg->chan1_raw; + if (msg->chan2_raw != 0 && msg->chan2_raw != UINT16_MAX) mavlinkChannelData[1] = msg->chan2_raw; + if (msg->chan3_raw != 0 && msg->chan3_raw != UINT16_MAX) mavlinkChannelData[2] = msg->chan3_raw; + if (msg->chan4_raw != 0 && msg->chan4_raw != UINT16_MAX) mavlinkChannelData[3] = msg->chan4_raw; + if (msg->chan5_raw != 0 && msg->chan5_raw != UINT16_MAX) mavlinkChannelData[4] = msg->chan5_raw; + if (msg->chan6_raw != 0 && msg->chan6_raw != UINT16_MAX) mavlinkChannelData[5] = msg->chan6_raw; + if (msg->chan7_raw != 0 && msg->chan7_raw != UINT16_MAX) mavlinkChannelData[6] = msg->chan7_raw; + if (msg->chan8_raw != 0 && msg->chan8_raw != UINT16_MAX) mavlinkChannelData[7] = msg->chan8_raw; + if (msg->chan9_raw != 0 && msg->chan9_raw < UINT16_MAX - 1) mavlinkChannelData[8] = msg->chan9_raw; + if (msg->chan10_raw != 0 && msg->chan10_raw < UINT16_MAX - 1) mavlinkChannelData[9] = msg->chan10_raw; + if (msg->chan11_raw != 0 && msg->chan11_raw < UINT16_MAX - 1) mavlinkChannelData[10] = msg->chan11_raw; + if (msg->chan12_raw != 0 && msg->chan12_raw < UINT16_MAX - 1) mavlinkChannelData[11] = msg->chan12_raw; + if (msg->chan13_raw != 0 && msg->chan13_raw < UINT16_MAX - 1) mavlinkChannelData[12] = msg->chan13_raw; + if (msg->chan14_raw != 0 && msg->chan14_raw < UINT16_MAX - 1) mavlinkChannelData[13] = msg->chan14_raw; + if (msg->chan15_raw != 0 && msg->chan15_raw < UINT16_MAX - 1) mavlinkChannelData[14] = msg->chan15_raw; + if (msg->chan16_raw != 0 && msg->chan16_raw < UINT16_MAX - 1) mavlinkChannelData[15] = msg->chan16_raw; + if (msg->chan17_raw != 0 && msg->chan17_raw < UINT16_MAX - 1) mavlinkChannelData[16] = msg->chan17_raw; + if (msg->chan18_raw != 0 && msg->chan18_raw < UINT16_MAX - 1) mavlinkChannelData[17] = msg->chan18_raw; + frameReceived = true; +} + +static uint8_t mavlinkFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) +{ + UNUSED(rxRuntimeConfig); + if (frameReceived) { + frameReceived = false; + return RX_FRAME_COMPLETE; + } + return RX_FRAME_PENDING; +} + +static uint16_t mavlinkReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) +{ + UNUSED(rxRuntimeConfig); + // MAVLink values are sent as PWM values in microseconds so no conversion is needed + return mavlinkChannelData[channel]; +} + +bool mavlinkRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +{ + UNUSED(rxConfig); + + frameReceived = false; + + rxRuntimeConfig->channelData = mavlinkChannelData; + rxRuntimeConfig->channelCount = MAVLINK_CHANNEL_COUNT; + rxRuntimeConfig->rxRefreshRate = 11000; + rxRuntimeConfig->rcReadRawFn = mavlinkReadRawRC; + rxRuntimeConfig->rcFrameStatusFn = mavlinkFrameStatus; + + for (int ii = 0; ii < MAVLINK_CHANNEL_COUNT; ++ii) { + mavlinkChannelData[ii] = (16 * PWM_RANGE_MIDDLE) / 10 - 1408; + } + + return true; +} + +#endif diff --git a/src/main/rx/mavlink.h b/src/main/rx/mavlink.h new file mode 100644 index 0000000000..a54e16d76d --- /dev/null +++ b/src/main/rx/mavlink.h @@ -0,0 +1,26 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-function" +#include "common/mavlink.h" +#pragma GCC diagnostic pop + +void mavlinkRxHandleMessage(const mavlink_rc_channels_override_t *msg); +bool mavlinkRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index c8e1184594..f92097befe 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -69,6 +69,7 @@ #include "rx/sumh.h" #include "rx/xbus.h" #include "rx/ghst.h" +#include "rx/mavlink.h" //#define DEBUG_RX_SIGNAL_LOSS @@ -262,6 +263,11 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig case SERIALRX_GHST: enabled = ghstRxInit(rxConfig, rxRuntimeConfig); break; +#endif +#ifdef USE_SERIALRX_MAVLINK + case SERIALRX_MAVLINK: + enabled = mavlinkRxInit(rxConfig, rxRuntimeConfig); + break; #endif default: enabled = false; diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index dcc1804f5d..883d8594fe 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -84,6 +84,7 @@ typedef enum { SERIALRX_FPORT2 = 12, SERIALRX_SRXL2 = 13, SERIALRX_GHST = 14, + SERIALRX_MAVLINK = 15, } rxSerialReceiverType_e; #define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 16 diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index 1e89705e37..2c27a1e95d 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -79,7 +79,7 @@ static void sumdDataReceive(uint16_t c, void *rxCallbackData) static uint8_t sumdIndex; sumdTime = micros(); - if (cmpTimeUs(sumdTime, sumdTimeLast) > 4000) + if (cmpTimeUs(sumdTime, sumdTimeLast) > MS2US(4)) sumdIndex = 0; sumdTimeLast = sumdTime; diff --git a/src/main/rx/sumh.c b/src/main/rx/sumh.c index 6f9af2ec2b..33ffa0e07d 100644 --- a/src/main/rx/sumh.c +++ b/src/main/rx/sumh.c @@ -69,7 +69,7 @@ static void sumhDataReceive(uint16_t c, void *rxCallbackData) sumhTime = micros(); sumhTimeInterval = cmpTimeUs(sumhTime, sumhTimeLast); sumhTimeLast = sumhTime; - if (sumhTimeInterval > 5000) { + if (sumhTimeInterval > MS2US(5)) { sumhFramePosition = 0; } diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 0ad456967c..1a98f37b38 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -56,7 +56,7 @@ typedef enum { TASK_SERIAL, TASK_BATTERY, TASK_TEMPERATURE, -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) TASK_BEEPER, #endif #ifdef USE_LIGHTS diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index fe54342561..3ada6cb134 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -45,6 +45,7 @@ #include "flight/mixer.h" #include "navigation/navigation.h" +#include "navigation/navigation_private.h" #include "config/feature.h" @@ -480,7 +481,16 @@ void currentMeterUpdate(timeUs_t timeDelta) amperage = batteryMetersConfig()->current.offset; if (ARMING_FLAG(ARMED)) { throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC); - int32_t throttleOffset = ((throttleStatus == THROTTLE_LOW) && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000; + navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags(); + bool allNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_ALL_NAV && posControl.navState != NAV_STATE_IDLE; + bool autoNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_AUTO_ONLY && (stateFlags & (NAV_AUTO_RTH | NAV_AUTO_WP)); + int32_t throttleOffset; + + if (allNav || autoNav) { // account for motors running in Nav modes with throttle low + motor stop + throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; + } else { + throttleOffset = ((throttleStatus == THROTTLE_LOW) && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000; + } int32_t throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000; } @@ -560,7 +570,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta) } else { - if (cmpTimeUs(currentTime, recordTimestamp) > 500000) + if (cmpTimeUs(currentTime, recordTimestamp) > MS2US(500)) recordTimestamp = 0; if (!recordTimestamp) { @@ -577,7 +587,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta) if (impedanceFilterState.state) { pt1FilterSetTimeConstant(&impedanceFilterState, impedanceSampleCount > IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH ? 1.2 : 0.5); - pt1FilterApply3(&impedanceFilterState, impedanceSample, timeDelta * 1e-6f); + pt1FilterApply3(&impedanceFilterState, impedanceSample, US2S(timeDelta)); } else { pt1FilterReset(&impedanceFilterState, impedanceSample); } @@ -591,7 +601,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta) uint16_t sagCompensatedVBatSample = MIN(batteryFullVoltage, vbat + (int32_t)powerSupplyImpedance * amperage / 1000); pt1FilterSetTimeConstant(&sagCompVBatFilterState, sagCompensatedVBatSample < pt1FilterGetLastOutput(&sagCompVBatFilterState) ? 40 : 500); - sagCompensatedVBat = lrintf(pt1FilterApply3(&sagCompVBatFilterState, sagCompensatedVBatSample, timeDelta * 1e-6f)); + sagCompensatedVBat = lrintf(pt1FilterApply3(&sagCompVBatFilterState, sagCompensatedVBatSample, US2S(timeDelta))); } DEBUG_SET(DEBUG_SAG_COMP_VOLTAGE, 0, powerSupplyImpedance); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 6a8710bff2..0d32b49bfd 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -104,7 +104,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState; #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 11); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 12); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT, @@ -113,7 +113,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_align = SETTING_ALIGN_GYRO_DEFAULT, .gyroMovementCalibrationThreshold = SETTING_MORON_THRESHOLD_DEFAULT, .looptime = SETTING_LOOPTIME_DEFAULT, - .gyroSync = SETTING_GYRO_SYNC_DEFAULT, #ifdef USE_DUAL_GYRO .gyro_to_use = SETTING_GYRO_TO_USE_DEFAULT, #endif @@ -318,7 +317,7 @@ bool gyroInit(void) gyroDev[0].initFn(&gyroDev[0]); // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value - gyro.targetLooptime = gyroConfig()->gyroSync ? gyroDev[0].sampleRateIntervalUs : gyroConfig()->looptime; + gyro.targetLooptime = gyroDev[0].sampleRateIntervalUs; // At this poinrt gyroDev[0].gyroAlign was set up by the driver from the busDev record // If configuration says different - override @@ -518,19 +517,6 @@ int16_t gyroRateDps(int axis) return lrintf(gyro.gyroADCf[axis]); } -bool gyroSyncCheckUpdate(void) -{ - if (!gyro.initialized) { - return false; - } - - if (!gyroDev[0].intStatusFn) { - return false; - } - - return gyroDev[0].intStatusFn(&gyroDev[0]); -} - void gyroUpdateDynamicLpf(float cutoffFreq) { if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 6aa3c11414..b84477d686 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -61,7 +61,6 @@ extern gyro_t gyro; typedef struct gyroConfig_s { sensor_align_e gyro_align; // gyro alignment uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. - bool gyroSync; // Enable interrupt based loop uint16_t looptime; // imu loop time in us uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint8_t gyro_soft_lpf_hz; @@ -95,5 +94,4 @@ bool gyroIsCalibrationComplete(void); bool gyroReadTemperature(void); int16_t gyroGetTemperature(void); int16_t gyroRateDps(int axis); -bool gyroSyncCheckUpdate(void); void gyroUpdateDynamicLpf(float cutoffFreq); diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index cec9b00faf..8b26c586d8 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -227,7 +227,7 @@ timeDelta_t rangefinderUpdate(void) rangefinder.dev.update(&rangefinder.dev); } - return rangefinder.dev.delayMs * 1000; // to microseconds + return MS2US(rangefinder.dev.delayMs); } /** diff --git a/src/main/target/FALCORE/config.c b/src/main/target/FALCORE/config.c index d862359d49..0428b0a266 100755 --- a/src/main/target/FALCORE/config.c +++ b/src/main/target/FALCORE/config.c @@ -59,7 +59,6 @@ void targetConfiguration(void) gyroConfigMutable()->looptime = 1000; - gyroConfigMutable()->gyroSync = 1; gyroConfigMutable()->gyro_lpf = 0; // 256 Hz gyroConfigMutable()->gyro_soft_lpf_hz = 90; gyroConfigMutable()->gyro_notch_hz = 150; diff --git a/src/main/target/ZEEZF7/CMakeLists.txt b/src/main/target/ZEEZF7/CMakeLists.txt index 513e023060..eb14cb8802 100644 --- a/src/main/target/ZEEZF7/CMakeLists.txt +++ b/src/main/target/ZEEZF7/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f722xe(ZEEZF7) +target_stm32f722xe(ZEEZF7V2) diff --git a/src/main/target/ZEEZF7/config.c b/src/main/target/ZEEZF7/config.c index ffbe3639f0..bde6f06f21 100644 --- a/src/main/target/ZEEZF7/config.c +++ b/src/main/target/ZEEZF7/config.c @@ -25,6 +25,8 @@ void targetConfiguration(void) { +#ifndef ZEEZF7V2 pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; // VTX power switcher //pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +#endif } diff --git a/src/main/target/ZEEZF7/target.c b/src/main/target/ZEEZF7/target.c index 86573bcc1c..b0ede6a513 100755 --- a/src/main/target/ZEEZF7/target.c +++ b/src/main/target/ZEEZF7/target.c @@ -25,10 +25,21 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { +#ifdef ZEEZF7V2 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), // S2 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR, 0, 0), // S3 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR, 0, 0), // S4 + DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0), // S5 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR, 0, 0), // S6 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR, 0, 0), // S7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // S8 +#else DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR, 0, 0), // S1 DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S2 DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), // S3 DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR, 0, 0), // S4 +#endif DEF_TIM(TIM1, CH2N, PB0, TIM_USE_LED , 0, 0), // LED STRIP }; diff --git a/src/main/target/ZEEZF7/target.h b/src/main/target/ZEEZF7/target.h index 2d6e4ab51f..9de6c3359e 100755 --- a/src/main/target/ZEEZF7/target.h +++ b/src/main/target/ZEEZF7/target.h @@ -17,9 +17,13 @@ #pragma once +#ifdef ZEEZF7V2 +#define TARGET_BOARD_IDENTIFIER "ZEF7V2" +#define USBD_PRODUCT_STRING "ZEEZF7V2" +#else #define TARGET_BOARD_IDENTIFIER "ZEF7" - #define USBD_PRODUCT_STRING "ZEEZF7" +#endif #define LED0 PC14 #define LED1 PC15 @@ -29,6 +33,21 @@ // *************** Gyro & ACC ********************** #define USE_SPI + +#ifdef ZEEZF7V2 + +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 +#define GYRO_INT_EXTI PC4 +#define IMU_MPU6000_ALIGN CW0_DEG + +#else + #define USE_SPI_DEVICE_2 #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 @@ -36,15 +55,44 @@ #define MPU6000_CS_PIN PB12 #define MPU6000_SPI_BUS BUS_SPI2 +#define GYRO_INT_EXTI PC9 -#define USE_EXTI -#define GYRO_INT_EXTI PC9 -#define USE_MPU_DATA_READY_SIGNAL - -#define USE_IMU_MPU6000 #define IMU_MPU6000_ALIGN CW180_DEG +#endif + +#define USE_EXTI +#define USE_MPU_DATA_READY_SIGNAL +#define USE_IMU_MPU6000 + // *************** I2C/Baro/Mag ********************* +#ifdef ZEEZF7V2 + +#define USE_I2C +#define USE_BARO +#define USE_BARO_BMP280 +#define BARO_I2C_BUS BUS_I2C1 + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +// External I2C Pads -- I2C2 +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PA8 +#define I2C2_SDA PC9 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL +#define USE_MAG_AK8975 + +#else // Target has no I2C but can use MSP devices, such as those provided on Mateksys MQ8-CAN/MSP // Which contains: GPS SAM-M8Q, Compass QMC5883L, Barometer DPS310 // See: http://www.mateksys.com/?portfolio=m8q-can @@ -55,18 +103,34 @@ #define USE_MAG #define USE_MAG_QMC5883 +#endif + // *************** Flash **************************** -#define USE_SPI_DEVICE_1 +#ifdef ZEEZF7V2 +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN PB12 + +#else + +#define USE_SPI_DEVICE_1 #define SPI1_SCK_PIN PA5 #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 -#define USE_FLASHFS -#define USE_FLASH_M25P16 #define M25P16_SPI_BUS BUS_SPI1 #define M25P16_CS_PIN PA4 + +#endif + +#define USE_FLASHFS +#define USE_FLASH_M25P16 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT // *************** OSD ***************************** @@ -82,14 +146,16 @@ #define MAX7456_CS_PIN PA15 // *************** PINIO *************************** +#ifdef ZEEZF7 + #define USE_PINIO #define USE_PINIOBOX #define PINIO1_PIN PB11 // VTX power switcher +#endif + // *************** UART ***************************** #define USE_VCP -//#define VBUS_SENSING_PIN PB12 -//#define VBUS_SENSING_ENABLED #define USE_UART1 #define UART1_RX_PIN PA10 @@ -111,12 +177,20 @@ #define UART5_RX_PIN PD2 #define UART5_TX_PIN PC12 +#ifdef ZEEZF7V2 + +#define SERIAL_PORT_COUNT 6 + +#else + #define USE_UART6 #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 #define SERIAL_PORT_COUNT 7 +#endif + #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART4 @@ -125,15 +199,24 @@ #define USE_ADC #define ADC_INSTANCE ADC1 #define ADC1_DMA_STREAM DMA2_Stream0 +#ifdef ZEEZF7V2 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#else #define ADC_CHANNEL_1_PIN PC2 #define ADC_CHANNEL_2_PIN PC3 +#endif #define VBAT_ADC_CHANNEL ADC_CHN_1 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 -#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY| FEATURE_VBAT | FEATURE_OSD ) +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_LED_STRIP) + +#ifdef ZEEZF7V2 +#define CURRENT_METER_SCALE 250 +#endif #define USE_LED_STRIP -#define WS2811_PIN PB0 +#define WS2811_PIN PB0 #define USE_SERIAL_4WAY_BLHELI_INTERFACE @@ -142,7 +225,11 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) -#define MAX_PWM_OUTPUT_PORTS 4 +#ifdef ZEEZF7V2 +#define MAX_PWM_OUTPUT_PORTS 8 +#else +#define MAX_PWM_OUTPUT_PORTS 4 +#endif #define USE_DSHOT #define USE_ESC_SENSOR #define USE_SERIALSHOT diff --git a/src/main/target/common.h b/src/main/target/common.h index 7344fd21f3..41fc44229f 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -86,6 +86,7 @@ #define USE_DYNAMIC_FILTERS #define USE_GYRO_KALMAN +#define USE_SMITH_PREDICTOR #define USE_EXTENDED_CMS_MENUS #define USE_HOTT_TEXTMODE @@ -151,6 +152,7 @@ #define USE_SERIALRX_SUMH #define USE_SERIALRX_XBUS #define USE_SERIALRX_JETIEXBUS +#define USE_SERIALRX_MAVLINK #define USE_TELEMETRY_SRXL #define USE_SPEKTRUM_CMS_TELEMETRY //#define USE_SPEKTRUM_VTX_CONTROL //Some functions from betaflight still not implemented diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index b5725a19b2..6c9ded6869 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -67,6 +67,7 @@ #include "navigation/navigation_private.h" #include "rx/rx.h" +#include "rx/mavlink.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" @@ -86,10 +87,8 @@ #include "scheduler/scheduler.h" -// mavlink library uses unnames unions that's causes GCC to complain if -Wpedantic is used -// until this is resolved in mavlink library - ignore -Wpedantic for mavlink code #pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" +#pragma GCC diagnostic ignored "-Wunused-function" #include "common/mavlink.h" #pragma GCC diagnostic pop @@ -97,6 +96,11 @@ #define TELEMETRY_MAVLINK_MAXRATE 50 #define TELEMETRY_MAVLINK_DELAY ((1000 * 1000) / TELEMETRY_MAVLINK_MAXRATE) +/** + * MAVLink requires angles to be in the range -Pi..Pi. + * This converts angles from a range of 0..Pi to -Pi..Pi + */ +#define RADIANS_TO_MAVLINK_RANGE(angle) (angle > M_PIf) ? angle - (2 * M_PIf) : angle /** @brief A mapping of plane flight modes for custom_mode field of heartbeat. */ typedef enum APM_PLANE_MODE @@ -322,6 +326,7 @@ void checkMAVLinkTelemetryState(void) static void mavlinkSendMessage(void) { uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN]; + if (telemetryConfig()->mavlink.version == 1) mavSendMsg.magic = MAVLINK_STX_MAVLINK1; int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavSendMsg); for (int i = 0; i < msgLength; i++) { @@ -333,7 +338,7 @@ void mavlinkSendSystemStatus(void) { // Receiver is assumed to be always present uint32_t onboard_control_sensors_present = (MAV_SYS_STATUS_SENSOR_RC_RECEIVER); - // GYRO and RC are assumed as minimium requirements + // GYRO and RC are assumed as minimum requirements uint32_t onboard_control_sensors_enabled = (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_RC_RECEIVER); uint32_t onboard_control_sensors_health = 0; @@ -539,7 +544,19 @@ void mavlinkSendPosition(timeUs_t currentTimeUs) // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 gpsSol.groundCourse * 10, // satellites_visible Number of satellites visible. If unknown, set to 255 - gpsSol.numSat); + gpsSol.numSat, + // alt_ellipsoid Altitude (above WGS84, EGM96 ellipsoid). Positive for up + 0, + // h_acc Position uncertainty in mm, + gpsSol.eph * 10, + // v_acc Altitude uncertainty in mm, + gpsSol.epv * 10, + // vel_acc Speed uncertainty in mm (??) + 0, + // hdg_acc Heading uncertainty in degE5 + 0, + // yaw Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + 0); mavlinkSendMessage(); @@ -577,7 +594,10 @@ void mavlinkSendPosition(timeUs_t currentTimeUs) // longitude Longitude (WGS84), expressed as * 1E7 GPS_home.lon, // altitude Altitude(WGS84), expressed as * 1000 - GPS_home.alt * 10); // FIXME + GPS_home.alt * 10, // FIXME + // time_usec Timestamp (microseconds since system boot) + // Use millis() * 1000 as micros() will overflow after 1.19 hours. + ((uint64_t) millis()) * 1000); mavlinkSendMessage(); } @@ -589,17 +609,17 @@ void mavlinkSendAttitude(void) // time_boot_ms Timestamp (milliseconds since system boot) millis(), // roll Roll angle (rad) - DECIDEGREES_TO_RADIANS(attitude.values.roll), + RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.roll)), // pitch Pitch angle (rad) - DECIDEGREES_TO_RADIANS(-attitude.values.pitch), + RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(-attitude.values.pitch)), // yaw Yaw angle (rad) - DECIDEGREES_TO_RADIANS(attitude.values.yaw), + RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.yaw)), // rollspeed Roll angular speed (rad/s) - 0, + gyro.gyroADCf[FD_ROLL], // pitchspeed Pitch angular speed (rad/s) - 0, + gyro.gyroADCf[FD_PITCH], // yawspeed Yaw angular speed (rad/s) - 0); + gyro.gyroADCf[FD_YAW]); mavlinkSendMessage(); } @@ -738,12 +758,18 @@ void mavlinkSendHUDAndHeartbeat(void) void mavlinkSendBatteryTemperatureStatusText(void) { uint16_t batteryVoltages[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN]; + uint16_t batteryVoltagesExt[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN]; memset(batteryVoltages, UINT16_MAX, sizeof(batteryVoltages)); + memset(batteryVoltagesExt, 0, sizeof(batteryVoltagesExt)); if (feature(FEATURE_VBAT)) { uint8_t batteryCellCount = getBatteryCellCount(); if (batteryCellCount > 0) { - for (int cell=0; (cell < batteryCellCount) && (cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN); cell++) { - batteryVoltages[cell] = getBatteryAverageCellVoltage() * 10; + for (int cell=0; cell < batteryCellCount && cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN + MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; cell++) { + if (cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN) { + batteryVoltages[cell] = getBatteryAverageCellVoltage() * 10; + } else { + batteryVoltagesExt[cell] = getBatteryAverageCellVoltage() * 10; + } } } else { @@ -772,7 +798,17 @@ void mavlinkSendBatteryTemperatureStatusText(void) // energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate isAmperageConfigured() ? getMWhDrawn()*36 : -1, // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery); - feature(FEATURE_VBAT) ? calculateBatteryPercentage() : -1); + feature(FEATURE_VBAT) ? calculateBatteryPercentage() : -1, + // time_remaining Remaining battery time, 0: autopilot does not provide remaining battery time estimate + 0, // TODO this could easily be implemented + // charge_state State for extent of discharge, provided by autopilot for warning or external reactions + 0, + // voltages_ext Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead. + batteryVoltagesExt, + // mode Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode. + 0, + // fault_bitmask Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported). + 0); mavlinkSendMessage(); @@ -783,7 +819,8 @@ void mavlinkSendBatteryTemperatureStatusText(void) millis(), 0, 0, - temperature * 10); + temperature * 10, + 0); mavlinkSendMessage(); @@ -802,7 +839,9 @@ void mavlinkSendBatteryTemperatureStatusText(void) mavlink_msg_statustext_pack(mavSystemId, mavComponentId, &mavSendMsg, (uint8_t)severity, - buff); + buff, + 0, + 0); mavlinkSendMessage(); } @@ -850,7 +889,7 @@ static bool handleIncoming_MISSION_CLEAR_ALL(void) // Check if this message is for us if (msg.target_system == mavSystemId) { resetWaypointList(); - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } @@ -872,17 +911,17 @@ static bool handleIncoming_MISSION_COUNT(void) if (msg.count <= NAV_MAX_WAYPOINTS) { incomingMissionWpCount = msg.count; // We need to know how many items to request incomingMissionWpSequence = 0; - mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence); + mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } else if (ARMING_FLAG(ARMED)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_NO_SPACE); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_NO_SPACE, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } @@ -900,19 +939,19 @@ static bool handleIncoming_MISSION_ITEM(void) if (msg.target_system == mavSystemId) { // Check supported values first if (ARMING_FLAG(ARMED)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } if ((msg.autocontinue == 0) || (msg.command != MAV_CMD_NAV_WAYPOINT && msg.command != MAV_CMD_NAV_RETURN_TO_LAUNCH)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } if ((msg.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) && !(msg.frame == MAV_FRAME_MISSION && msg.command == MAV_CMD_NAV_RETURN_TO_LAUNCH)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } @@ -934,22 +973,22 @@ static bool handleIncoming_MISSION_ITEM(void) if (incomingMissionWpSequence >= incomingMissionWpCount) { if (isWaypointListValid()) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); } } else { - mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence); + mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); } } else { // Wrong sequence number received - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); } @@ -966,7 +1005,7 @@ static bool handleIncoming_MISSION_REQUEST_LIST(void) // Check if this message is for us if (msg.target_system == mavSystemId) { - mavlink_msg_mission_count_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, getWaypointCount()); + mavlink_msg_mission_count_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, getWaypointCount(), MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } @@ -996,11 +1035,12 @@ static bool handleIncoming_MISSION_REQUEST(void) 0, 0, 0, 0, wp.lat / 1e7f, wp.lon / 1e7f, - wp.alt / 100.0f); + wp.alt / 100.0f, + MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); } @@ -1010,6 +1050,14 @@ static bool handleIncoming_MISSION_REQUEST(void) return false; } +static bool handleIncoming_RC_CHANNELS_OVERRIDE(void) { + mavlink_rc_channels_override_t msg; + mavlink_msg_rc_channels_override_decode(&mavRecvMsg, &msg); + // Don't check system ID because it's not configurable with systems like Crossfire + mavlinkRxHandleMessage(&msg); + return true; +} + static bool processMAVLinkIncomingTelemetry(void) { while (serialRxBytesWaiting(mavlinkPort) > 0) { @@ -1030,6 +1078,8 @@ static bool processMAVLinkIncomingTelemetry(void) return handleIncoming_MISSION_REQUEST_LIST(); case MAVLINK_MSG_ID_MISSION_REQUEST: return handleIncoming_MISSION_REQUEST(); + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: + return handleIncoming_RC_CHANNELS_OVERRIDE(); default: return false; } @@ -1064,8 +1114,6 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs) lastMavlinkMessage = currentTimeUs; incomingRequestServed = false; } - - } #endif diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 6246c3f47f..add40a3b0f 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -90,7 +90,8 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .position_rate = SETTING_MAVLINK_POS_RATE_DEFAULT, .extra1_rate = SETTING_MAVLINK_EXTRA1_RATE_DEFAULT, .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT, - .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT + .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT, + .version = SETTING_MAVLINK_VERSION_DEFAULT } ); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 242f5cc83e..51e79b89b5 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -87,6 +87,7 @@ typedef struct telemetryConfig_s { uint8_t extra1_rate; uint8_t extra2_rate; uint8_t extra3_rate; + uint8_t version; } mavlink; } telemetryConfig_t; diff --git a/src/utils/settings.rb b/src/utils/settings.rb index acae823773..877e325fcd 100755 --- a/src/utils/settings.rb +++ b/src/utils/settings.rb @@ -301,6 +301,7 @@ class Generator check_member_default_values_presence sanitize_fields + resolv_min_max_and_default_values_if_possible initialize_name_encoder initialize_value_encoder validate_default_values @@ -359,6 +360,7 @@ class Generator @data = YAML.load_file(@settings_file) initialize_tables + initialize_constants check_conditions end @@ -415,6 +417,11 @@ class Generator buf << "extern const char * const #{table_variable_name(name)}[];\n" end + # Write setting constants from settings file + @constants.each do |name, value| + buf << "#define SETTING_CONSTANT_#{name.upcase} #{value.inspect}\n" + end + # Write #define'd constants for referencing each setting ii = 0 foreach_enabled_member do |group, member| @@ -715,6 +722,10 @@ class Generator end end + def initialize_constants + @constants = @data["constants"] + end + def ordered_table_names @enabled_tables.to_a().sort() end @@ -877,6 +888,8 @@ class Generator foreach_enabled_member do |_, member| name = member["name"] type = member["type"] + min = member["min"] || 0 + max = member["max"] default_value = member["default_value"] next if %i[ zero target ].include? default_value @@ -894,13 +907,17 @@ class Generator unsigned = !$~[:unsigned].empty? bitsize = $~[:bitsize].to_i type_range = unsigned ? 0..(2**bitsize-1) : (-2**(bitsize-1)+1)..(2**(bitsize-1)-1) - raise "Numeric member #{name} doesn't have maximum value defined" unless member.has_key? 'max' + min = type_range.min if min =~ /\AU?INT\d+_MIN\Z/ + max = type_range.max if max =~ /\AU?INT\d+_MAX\Z/ raise "Member #{name} default value has an invalid type, integer or symbol expected" unless default_value.is_a? Integer or default_value.is_a? Symbol raise "Member #{name} default value is outside type's storage range, min #{type_range.min}, max #{type_range.max}" unless default_value.is_a? Symbol or type_range === default_value + raise "Numeric member #{name} doesn't have maximum value defined" unless member.has_key? 'max' + raise "Member #{name} default value is outside of the allowed range" if default_value.is_a? Numeric and min.is_a? Numeric and max.is_a? Numeric and not (min..max) === default_value when type == "float" - raise "Numeric member #{name} doesn't have maximum value defined" unless member.has_key? 'max' raise "Member #{name} default value has an invalid type, numeric or symbol expected" unless default_value.is_a? Numeric or default_value.is_a? Symbol + raise "Numeric member #{name} doesn't have maximum value defined" unless member.has_key? 'max' + raise "Member #{name} default value is outside of the allowed range" if default_value.is_a? Numeric and min.is_a? Numeric and max.is_a? Numeric and not (min..max) === default_value when type == "string" max = member["max"].to_i @@ -971,6 +988,8 @@ class Generator types.each do |idx, type| member = members[idx] case type + when /^bool/ + typ = "bool" when /^int8_t/ # {aka signed char}" typ = "int8_t" when /^uint8_t/ # {aka unsigned char}" @@ -988,7 +1007,7 @@ class Generator member["max"] = $1.to_i - 1; typ = "string" else - raise "Unknown type #{m[1]} when resolving type for setting #{member["name"]}" + raise "Unknown type #{type} when resolving type for setting #{member["name"]}" end dputs "#{member["name"]} type is #{typ}" member["type"] = typ @@ -1039,6 +1058,18 @@ class Generator raise "Missing default value for #{missing_default_value_names.count} member#{"s" unless missing_default_value_names.one?}: #{missing_default_value_names * ", "}" unless missing_default_value_names.empty? end + def resolv_min_max_and_default_values_if_possible + foreach_member do |_, member| + %w[ min max default_value ].each do |value_type| + member_value = member[value_type] + if member_value.is_a? String + constant_value = @constants[member_value] + member[value_type] = constant_value unless constant_value.nil? + end + end + end + end + def resolve_constants(constants) return nil unless constants.length > 0 s = Set.new diff --git a/src/utils/update_cli_docs.py b/src/utils/update_cli_docs.py index f7f7d8a24b..f685d908a8 100755 --- a/src/utils/update_cli_docs.py +++ b/src/utils/update_cli_docs.py @@ -22,6 +22,16 @@ DEFAULTS_BLACKLIST = [ 'serialrx_provider', ] +MIN_MAX_REPLACEMENTS = { + 'INT16_MIN': -32768, + 'INT16_MAX': 32767, + 'INT32_MIN': -2147483648, + 'INT32_MAX': 2147483647, + 'UINT8_MAX': 255, + 'UINT16_MAX': 65535, + 'UINT32_MAX': 4294967295, +} + def parse_settings_yaml(): """Parse the YAML settings specs""" @@ -32,42 +42,59 @@ def generate_md_table_from_yaml(settings_yaml): """Generate a sorted markdown table with description & default value for each setting""" params = {} - # Extract description and default value of each setting from the YAML specs (if present) + # Extract description, default/min/max values of each setting from the YAML specs (if present) for group in settings_yaml['groups']: for member in group['members']: - if not any(key in member for key in ["description", "default_value"]) and not options.quiet: - print("Setting \"{}\" has no description or default value specified".format(member['name'])) - # Handle edge cases of YAML autogeneration - if "default_value" in member: - # Replace booleans with "ON"/"OFF" - if type(member["default_value"]) == bool: - member["default_value"] = "ON" if member["default_value"] else "OFF" - # Replace zero placeholder with actual zero - elif member["default_value"] == ":zero": - member["default_value"] = 0 - # Replace target-default placeholder with extended definition - elif member["default_value"] == ":target": - member["default_value"] = "_target default_" - # Replace empty strings with more evident marker - elif member["default_value"] == "": - member["default_value"] = "_empty_" - # Reformat direct code references - elif str(member["default_value"])[0] == ":": - member["default_value"] = f'`{member["default_value"][1:]}`' + if not any(key in member for key in ["description", "default_value", "min", "max"]) and not options.quiet: + print("Setting \"{}\" has an incomplete specification".format(member['name'])) + + # Handle default/min/max fields for each setting + for key in ["default_value", "min", "max"]: + # Basing on the check above, not all fields may be present + if key in member: + ### Fetch actual values from the `constants` block if present + if ('constants' in settings_yaml) and (member[key] in settings_yaml['constants']): + member[key] = settings_yaml['constants'][member[key]] + ### Fetch actual values from hardcoded min/max replacements + elif member[key] in MIN_MAX_REPLACEMENTS: + member[key] = MIN_MAX_REPLACEMENTS[member[key]] + + ### Handle edge cases of YAML autogeneration and prettify some values + # Replace booleans with "ON"/"OFF" + if type(member[key]) == bool: + member[key] = "ON" if member[key] else "OFF" + # Replace zero placeholder with actual zero + elif member[key] == ":zero": + member[key] = 0 + # Replace target-default placeholder with extended definition + elif member[key] == ":target": + member[key] = "_target default_" + # Replace empty strings with more evident marker + elif member[key] == "": + member[key] = "_empty_" + # Reformat direct code references + elif str(member[key])[0] == ":": + member[key] = f'`{member[key][1:]}`' + + params[member['name']] = { "description": member["description"] if "description" in member else "", - "default": member["default_value"] if "default_value" in member else "" + "default": member["default_value"] if "default_value" in member else "", + "min": member["min"] if "min" in member else "", + "max": member["max"] if "max" in member else "" } # MD table header md_table_lines = [ - "| Variable Name | Default Value | Description |\n", - "| ------------- | ------------- | ----------- |\n", + "| Variable Name | Default Value | Min | Max | Description |\n", + "| ------------- | ------------- | --- | --- | ----------- |\n", ] # Sort the settings by name and build the rows of the table for param in sorted(params.items()): - md_table_lines.append("| {} | {} | {} |\n".format(param[0], param[1]['default'], param[1]['description'])) + md_table_lines.append("| {} | {} | {} | {} | {} |\n".format( + param[0], param[1]['default'], param[1]['min'], param[1]['max'], param[1]['description'] + )) # Return the assembled table return md_table_lines