diff --git a/.clang-format b/.clang-format index ae8019dc..8e39f5a4 100644 --- a/.clang-format +++ b/.clang-format @@ -42,6 +42,7 @@ IndentCaseLabels: true IndentPPDirectives: None IndentWidth: 2 KeepEmptyLinesAtTheStartOfBlocks: true +LineEnding: LF MaxEmptyLinesToKeep: 1 NamespaceIndentation: None ObjCSpaceAfterProperty: false diff --git a/.gitignore b/.gitignore index 1cca735c..c82d706c 100644 --- a/.gitignore +++ b/.gitignore @@ -19,6 +19,8 @@ cmake-build* *.map scripts/parameter-descriptions.md +CMakeFiles/ +CMakeCache.txt *.aux *.fdb_latexmk diff --git a/boards/varmint_h7/.clang-format b/boards/varmint_h7/.clang-format deleted file mode 100644 index 68220e74..00000000 --- a/boards/varmint_h7/.clang-format +++ /dev/null @@ -1,68 +0,0 @@ -# ROS2 code style standards -# https://docs.ros.org/en/humble/The-ROS2-Project/Contributing/Code-Style-Language-Versions.html#id3 -BasedOnStyle: LLVM -AccessModifierOffset: -2 -AlignAfterOpenBracket: Align -AlignConsecutiveAssignments: None -AlignOperands: DontAlign -AllowAllArgumentsOnNextLine: false -AllowAllConstructorInitializersOnNextLine: false -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortBlocksOnASingleLine: Always -AllowShortCaseLabelsOnASingleLine: false -AllowShortFunctionsOnASingleLine: All -AllowShortIfStatementsOnASingleLine: AllIfsAndElse -AllowShortLambdasOnASingleLine: All -AllowShortLoopsOnASingleLine: true -AlwaysBreakAfterReturnType: None -AlwaysBreakTemplateDeclarations: Yes -BreakBeforeBraces: Custom -BraceWrapping: - AfterCaseLabel: false - AfterClass: true - AfterControlStatement: Never - AfterEnum: true - AfterFunction: true - AfterNamespace: true - AfterStruct: true - AfterUnion: true - BeforeCatch: false - BeforeElse: false - IndentBraces: false - SplitEmptyFunction: false - SplitEmptyRecord: true -BreakBeforeBinaryOperators: NonAssignment -BreakBeforeTernaryOperators: true -BreakConstructorInitializers: BeforeComma -BreakInheritanceList: BeforeColon -ColumnLimit: 120 -CompactNamespaces: false -ContinuationIndentWidth: 2 -IndentCaseLabels: true -IndentPPDirectives: None -IndentWidth: 2 -KeepEmptyLinesAtTheStartOfBlocks: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceAfterProperty: false -ObjCSpaceBeforeProtocolList: false -PointerAlignment: Middle -ReflowComments: false -SpaceAfterCStyleCast: true -SpaceAfterLogicalNot: false -SpaceAfterTemplateKeyword: false -SpaceBeforeAssignmentOperators: true -SpaceBeforeCpp11BracedList: false -SpaceBeforeCtorInitializerColon: true -SpaceBeforeInheritanceColon: true -SpaceBeforeParens: ControlStatements -SpaceBeforeRangeBasedForLoopColon: true -SpaceInEmptyParentheses: false -SpacesBeforeTrailingComments: 1 -SpacesInAngles: false -SpacesInCStyleCastParentheses: false -SpacesInContainerLiterals: false -SpacesInParentheses: false -SpacesInSquareBrackets: false -TabWidth: 2 -UseTab: Never diff --git a/boards/varmint_h7/.gitignore b/boards/varmint_h7/.gitignore index d0b09811..c9bf32f7 100644 --- a/boards/varmint_h7/.gitignore +++ b/boards/varmint_h7/.gitignore @@ -16,3 +16,4 @@ cmake_install.cmake varmint_11x/ varmint_11x_test/ varmint_12x/ +Liv4f.* diff --git a/boards/varmint_h7/common/STM32H7LinkerScript.ld b/boards/varmint_h7/common/STM32H7LinkerScript.ld index c831d3dc..53f82593 100644 --- a/boards/varmint_h7/common/STM32H7LinkerScript.ld +++ b/boards/varmint_h7/common/STM32H7LinkerScript.ld @@ -37,8 +37,8 @@ ENTRY(Reset_Handler) /* Highest address of the user mode stack */ _estack = ORIGIN(RAM_D1) + LENGTH(RAM_D1); /* end of RAM */ /* Generate a link error if heap and stack don't fit into RAM */ -_Min_Heap_Size = 0x2000; /* required amount of heap */ -_Min_Stack_Size = 0x2000; /* required amount of stack */ +_Min_Heap_Size = 0x10000; /* required amount of heap note _user_heap_stack is one location */ +_Min_Stack_Size = 0x20000; /* required amount of stack */ /* Specify the memory areas */ MEMORY diff --git a/boards/varmint_h7/common/Varmint.cpp b/boards/varmint_h7/common/Varmint.cpp index 24871bb7..5720e896 100644 --- a/boards/varmint_h7/common/Varmint.cpp +++ b/boards/varmint_h7/common/Varmint.cpp @@ -148,7 +148,7 @@ uint16_t Varmint::sensors_init_message(char * message, uint16_t size, uint16_t i bool Varmint::imu_read(rosflight_firmware::ImuStruct * imu) { ImuPacket p; - if (imu0_.rxFifoReadMostRecent((uint8_t *) &p, sizeof(p))) { + if (imu0_.read((uint8_t *) &p, sizeof(p))) { imu->header = p.header; imu->accel[0] = p.accel[0]; imu->accel[1] = p.accel[1]; @@ -167,7 +167,7 @@ bool Varmint::imu_read(rosflight_firmware::ImuStruct * imu) bool Varmint::mag_read(rosflight_firmware::MagStruct * mag) { MagPacket p; - if (mag_.rxFifoReadMostRecent((uint8_t *) &p, sizeof(p))) { + if (mag_.read((uint8_t *) &p, sizeof(p))) { mag->header = p.header; mag->flux[0] = p.flux[0]; mag->flux[1] = p.flux[1]; @@ -183,7 +183,7 @@ bool Varmint::mag_read(rosflight_firmware::MagStruct * mag) bool Varmint::baro_read(rosflight_firmware::PressureStruct * baro) { PressurePacket p; - if (baro_.rxFifoReadMostRecent((uint8_t *) &p, sizeof(p))) { + if (baro_.read((uint8_t *) &p, sizeof(p))) { baro->header = p.header; baro->pressure = p.pressure; baro->temperature = p.temperature; @@ -197,7 +197,7 @@ bool Varmint::baro_read(rosflight_firmware::PressureStruct * baro) bool Varmint::diff_pressure_read(rosflight_firmware::PressureStruct * diff_pressure) { PressurePacket p; - if (pitot_.rxFifoReadMostRecent((uint8_t *) &p, sizeof(p))) { + if (pitot_.read((uint8_t *) &p, sizeof(p))) { diff_pressure->header = p.header; diff_pressure->pressure = p.pressure; diff_pressure->temperature = p.temperature; @@ -219,7 +219,7 @@ bool Varmint::sonar_read(rosflight_firmware::RangeStruct * sonar) bool Varmint::battery_read(rosflight_firmware::BatteryStruct * batt) { AdcPacket p; - if (adc_.rxFifoReadMostRecent((uint8_t *) &p, sizeof(p))) { + if (adc_.read((uint8_t *) &p, sizeof(p))) { batt->header = p.header; batt->current = p.volts[ADC_BATTERY_CURRENT]; batt->voltage = p.volts[ADC_BATTERY_VOLTS]; @@ -245,57 +245,24 @@ bool Varmint::gnss_read(rosflight_firmware::GnssStruct * gnss) { UbxPacket p; - if (gps_.rxFifoReadMostRecent((uint8_t *) &p, sizeof(p))) { + if (gps_.read((uint8_t *) &p, sizeof(p))) { gnss->header = p.header; gnss->pps = p.pps; gnss->unix_seconds = p.unix_seconds; // Unix time - gnss->t_acc = p.pvt.tAcc; - gnss->time_of_week = p.pvt.iTOW; - gnss->year = p.pvt.year; - gnss->month = p.pvt.month; - gnss->day = p.pvt.day; - gnss->hour = p.pvt.hour; - gnss->min = p.pvt.min; - gnss->sec = p.pvt.sec; - gnss->nano = p.pvt.nano; - gnss->lon = p.pvt.lon; - gnss->lat = p.pvt.lat; - gnss->height_ellipsoid = p.pvt.height; - gnss->height_msl = p.pvt.hMSL; - gnss->h_acc = p.pvt.hAcc; - gnss->v_acc = p.pvt.vAcc; - gnss->vel_n = p.pvt.velN; - gnss->vel_e = p.pvt.velE; - gnss->vel_d = p.pvt.velD; - gnss->mag_var = p.pvt.magDec; - gnss->ground_speed = p.pvt.gSpeed; - gnss->course = p.pvt.headMot; - gnss->course_accy = p.pvt.headAcc; - gnss->vel_n = p.pvt.velN; - gnss->vel_e = p.pvt.velE; - gnss->vel_d = p.pvt.velD; - gnss->speed_accy = p.pvt.sAcc; - gnss->mag_var = p.pvt.magDec; - gnss->valid = ((p.pvt.flags & 0x01) != 0); - gnss->num_sat = p.pvt.numSV; - gnss->dop = p.pvt.pDOP; + gnss->unix_nanos = p.unix_nanos; gnss->fix_type = p.pvt.fixType; - - // These are not available from standard NMEA messages - // from ublox Class 0x01, ID 0x20 - gnss->ecef.x = p.ecefp.ecefX; - gnss->ecef.y = p.ecefp.ecefY; - gnss->ecef.z = p.ecefp.ecefZ; - gnss->ecef.p_acc = p.ecefp.pAcc; - // from ublox Class 0x01, ID 0x11 - gnss->ecef.vx = p.ecefv.ecefVX; - gnss->ecef.vy = p.ecefv.ecefVY; - gnss->ecef.vz = p.ecefv.ecefVZ; - gnss->ecef.s_acc = p.ecefv.sAcc; - + gnss->num_sat = p.pvt.numSV; + gnss->lon = (double)p.pvt.lon* 1e-7; // Convert 100's of nanodegs into deg (DDS format) + gnss->lat = (double)p.pvt.lat* 1e-7; // Convert 100's of nanodegs into deg (DDS format) + gnss->height_msl = (float)p.pvt.hMSL* 1e-3; //mm to m + gnss->vel_n = (float)p.pvt.velN* 1e-3; // mm/s to m/s + gnss->vel_e = (float)p.pvt.velE* 1e-3; // mm/s to m/s + gnss->vel_d = (float)p.pvt.velD* 1e-3; // mm/s to m/s + gnss->h_acc = (float)p.pvt.hAcc* 1e-3; //mm to m + gnss->v_acc = (float)p.pvt.vAcc* 1e-3; //mm to m + gnss->speed_accy = (float)p.pvt.sAcc* 1e-3; // mm/s to m/s return true; } - return false; } @@ -306,7 +273,7 @@ bool Varmint::rc_read(rosflight_firmware::RcStruct * rc_struct) { RcPacket p; - if (rc_.rxFifoReadMostRecent((uint8_t *) &p, sizeof(p))) { + if (rc_.read((uint8_t *) &p, sizeof(p))) { rc_struct->header = p.header; uint16_t len = RC_STRUCT_CHANNELS < RC_PACKET_CHANNELS ? RC_STRUCT_CHANNELS : RC_PACKET_CHANNELS; for (uint16_t i = 0; i < len; i++) { rc_struct->chan[i] = p.chan[i]; } diff --git a/boards/varmint_h7/common/drivers/Adc.cpp b/boards/varmint_h7/common/drivers/Adc.cpp index 7f013d9f..c5c2612f 100644 --- a/boards/varmint_h7/common/drivers/Adc.cpp +++ b/boards/varmint_h7/common/drivers/Adc.cpp @@ -49,7 +49,8 @@ extern Time64 time64; #define ADC_DMA_BUF_SIZE_EXT (ADC_CHANNELS_EXT * sizeof(uint32_t)) #define ADC_DMA_BUF_SIZE_MAX (16 * sizeof(uint32_t)) // 16 channels is max for the ADC sequencer -DTCM_RAM uint8_t adc_fifo_rx_buffer[ADC_FIFO_BUFFERS * sizeof(AdcPacket)]; +DTCM_RAM uint8_t adc_double_buffer[2 * sizeof(AdcPacket)]; + DTCM_RAM uint32_t adc_counts[ADC_CHANNELS]; ADC_EXT_DMA_RAM uint32_t adc_dma_buf_ext[ADC_DMA_BUF_SIZE_MAX / 4]; @@ -70,9 +71,7 @@ uint32_t Adc::init(uint16_t sample_rate_hz, ADC_HandleTypeDef * hadc_ext, hadcInt_ = hadc_int; cfg_ = adc_cfg; - groupDelay_ = 1000000 / sampleRateHz_; - - rxFifo_.init(ADC_FIFO_BUFFERS, sizeof(AdcPacket), adc_fifo_rx_buffer); + double_buffer_.init(adc_double_buffer, sizeof(adc_double_buffer)); if (DRIVER_OK != configAdc(hadcExt_, adc_instance_ext, cfg_, ADC_CHANNELS_EXT)) { initializationStatus_ = DRIVER_HAL_ERROR; @@ -189,14 +188,12 @@ void Adc::endDma(ADC_HandleTypeDef * hadc) double vcc = p.vRef; #endif for (int i = 0; i < ADC_CHANNELS; i++) { - // p.volts[i] = ((double)(adc_counts[i]&0xFFFF)/65535.0 - cfg_[i].offset) * p.vRef * cfg_[i].scaleFactor; - p.volts[i] = ((double) (adc_counts[i] & 0xFFFF) / 65535.0 * p.vRef - vcc * cfg_[i].offset) * cfg_[i].scaleFactor; + p.volts[i] = ((double) (adc_counts[i] & 0xFFFF) / 65535.0 * vcc - cfg_[i].offset) * cfg_[i].scaleFactor; } - p.header.timestamp = time64.Us(); - p.drdy = drdy_; - p.groupDelay = groupDelay_; - rxFifo_.write((uint8_t *) &p, sizeof(p)); + p.header.complete = time64.Us(); + p.header.timestamp = (drdy_+p.header.complete)/2; + write((uint8_t *) &p, sizeof(p)); ext_read = 0; int_read = 0; } @@ -207,8 +204,8 @@ bool Adc::display(void) AdcPacket p; char name[] = "Adc (adc)"; - if (rxFifo_.readMostRecent((uint8_t *) &p, sizeof(p))) { - misc_header(name, p.drdy, p.header.timestamp, p.groupDelay); + if (read((uint8_t *) &p, sizeof(p))) { + misc_header(name, p.header); misc_printf("\n"); misc_printf(" %-8s : ", "STM"); diff --git a/boards/varmint_h7/common/drivers/Adc.h b/boards/varmint_h7/common/drivers/Adc.h index 2c0fa888..945d7cea 100644 --- a/boards/varmint_h7/common/drivers/Adc.h +++ b/boards/varmint_h7/common/drivers/Adc.h @@ -38,8 +38,9 @@ #ifndef ADC_H_ #define ADC_H_ +#include "DoubleBuffer.h" #include "BoardConfig.h" -#include "Driver.h" + typedef struct __attribute__((__packed__)) { @@ -52,7 +53,7 @@ typedef struct __attribute__((__packed__)) /* * */ -class Adc : public Driver +class Adc : public Status { public: uint32_t init(uint16_t sample_rate_hz, ADC_HandleTypeDef * hadc_ext, @@ -64,11 +65,19 @@ class Adc : public Driver bool poll(uint64_t poll_offset); void endDma(ADC_HandleTypeDef * hadc); - bool display(void) override; + bool display(void); bool isMy(ADC_HandleTypeDef * hadc) { return (hadcExt_ == hadc) || (hadcInt_ == hadc); } void setScaleFactor(uint16_t n, float scale_factor); + bool read(uint8_t * data, uint16_t size) { return double_buffer_.read(data, size)==DoubleBufferStatus::OK; } + private: + bool write(uint8_t * data, uint16_t size) { return double_buffer_.write(data, size)==DoubleBufferStatus::OK; } + DoubleBuffer double_buffer_; + uint16_t sampleRateHz_; + uint64_t drdy_; + + uint32_t configChan(ADC_HandleTypeDef * hadc, ADC_ChannelConfTypeDef * sConfig, AdcChannelCfg * cfg); uint32_t configAdc(ADC_HandleTypeDef * hadc, ADC_TypeDef * adc_instance, AdcChannelCfg * cfg, uint16_t cfg_channels); ADC_HandleTypeDef *hadcExt_, *hadcInt_; // The shared SPI handle diff --git a/boards/varmint_h7/common/drivers/Adis165xx.cpp b/boards/varmint_h7/common/drivers/Adis165xx.cpp index c40cca9d..06429539 100644 --- a/boards/varmint_h7/common/drivers/Adis165xx.cpp +++ b/boards/varmint_h7/common/drivers/Adis165xx.cpp @@ -60,7 +60,7 @@ extern Time64 time64; DMA_RAM uint8_t adis165xx_dma_txbuf[SPI_DMA_MAX_BUFFER_SIZE]; DMA_RAM uint8_t adis165xx_dma_rxbuf[SPI_DMA_MAX_BUFFER_SIZE]; -DTCM_RAM uint8_t adis165xx_fifo_rx_buffer[ADIS165XX_FIFO_BUFFERS * sizeof(ImuPacket)]; +DTCM_RAM uint8_t adis165xx_double_buffer[2* sizeof(ImuPacket)]; uint32_t Adis165xx::init( // Driver initializers @@ -80,7 +80,7 @@ uint32_t Adis165xx::init( snprintf(name_, STATUS_NAME_MAX_LEN, "%s", "Adis165xx"); initializationStatus_ = DRIVER_OK; sampleRateHz_ = sample_rate_hz; - drdyPort_ = drdy_port; + drdyPin_ = drdy_pin; spi_.init(hspi, adis165xx_dma_txbuf, adis165xx_dma_rxbuf, cs_port, cs_pin); @@ -92,13 +92,13 @@ uint32_t Adis165xx::init( htim_ = htim; htimChannel_ = htim_channel; - groupDelay_ = (uint64_t) 1510 - + (uint64_t) 500000 / sampleRateHz_; // us, Approximate, Accel is 1.57ms, Gyro x&y are 1.51ms, and Gyro z is 1.29ms. + groupDelay_ = (uint64_t) 1510 + (uint64_t) 500000 / sampleRateHz_- 250; + // us, Approximate, Accel is 1.57ms, Gyro x&y are 1.51ms, and Gyro z is 1.29ms. HAL_GPIO_WritePin(spi_.port_, spi_.pin_, GPIO_PIN_SET); HAL_GPIO_WritePin(resetPort_, resetPin_, GPIO_PIN_SET); - rxFifo_.init(ADIS165XX_FIFO_BUFFERS, sizeof(ImuPacket), adis165xx_fifo_rx_buffer); + double_buffer_.init(adis165xx_double_buffer, sizeof(adis165xx_double_buffer)); // Startup the external clock @@ -224,6 +224,8 @@ bool Adis165xx::startDma(void) // called to start dma read void Adis165xx::endDma(void) // called when DMA data is ready { uint8_t * rx = spi_.endDma(); + ImuPacket p = {0}; + if (sampleRateHz_ == 2000) { // compute checksum uint16_t sum = 0; @@ -233,10 +235,6 @@ void Adis165xx::endDma(void) // called when DMA data is ready for (int i = 0; i < ADIS_BUFFBYTES16 / 2; i++) data[i] = (int16_t) rx[2 * i] << 8 | ((int16_t) rx[2 * i + 1] & 0x00FF); if (sum == data[10]) { - ImuPacket p; - p.header.timestamp = time64.Us(); - p.drdy = drdy_; - p.groupDelay = groupDelay_; p.header.status = (uint16_t) data[1]; p.gyro[0] = -(double) data[2] * 0.001745329251994; // rad/s, or use 0.1 deg/s p.gyro[1] = -(double) data[3] * 0.001745329251994; // rad/s, or use 0.1 deg/s @@ -246,7 +244,6 @@ void Adis165xx::endDma(void) // called when DMA data is ready p.accel[2] = (double) data[7] * 0.01225; // m/s^2 p.temperature = (double) data[8] * 0.1 + 273.15; // K p.dataTime = (double) ((uint16_t) data[9]) / sampleRateHz_; - if (p.header.status == ADIS_OK) rxFifo_.write((uint8_t *) &p, sizeof(p)); } } else { // compute checksum @@ -258,10 +255,6 @@ void Adis165xx::endDma(void) // called when DMA data is ready data[i] = (int16_t) rx[2 * i] << 8 | ((int16_t) rx[2 * i + 1] & 0x00FF); if (sum == data[16]) { - ImuPacket p; - p.header.timestamp = time64.Us(); - p.drdy = drdy_; - p.groupDelay = groupDelay_; p.header.status = (uint16_t) data[1]; p.gyro[0] = val(rx + 4) * 0.001745329251994; // rad/s, or use 0.1 deg/s p.gyro[1] = val(rx + 8) * 0.001745329251994; // rad/s, or use 0.1 deg/s @@ -271,14 +264,17 @@ void Adis165xx::endDma(void) // called when DMA data is ready p.accel[2] = val(rx + 24) * 0.01225; // m/s^2 p.temperature = (double) data[14] * 0.1 + 273.15; // K p.dataTime = (double) ((uint16_t) data[15]) / sampleRateHz_; - if (p.header.status == ADIS_OK) - { - rotate(p.gyro); - rotate(p.accel); - rxFifo_.write((uint8_t *) &p, sizeof(p)); - } } } + if (p.header.status == ADIS_OK) + { + p.header.timestamp = drdy_-groupDelay_; + rotate(p.gyro); + rotate(p.accel); + p.header.complete = time64.Us(); + write((uint8_t *) &p, sizeof(p)); + } + } void Adis165xx::writeRegister(uint8_t address, uint16_t value) @@ -312,8 +308,8 @@ bool Adis165xx::display(void) { ImuPacket p; char name[] = "Adis165xx (imu)"; - if (rxFifo_.readMostRecent((uint8_t *) &p, sizeof(p))) { - misc_header(name, p.drdy, p.header.timestamp, p.groupDelay); + if (read((uint8_t *) &p, sizeof(p))) { + misc_header(name, p.header ); misc_f32(nan(""), nan(""), p.accel[0] / 9.80665, "ax", "%6.2f", "g"); misc_f32(nan(""), nan(""), p.accel[1] / 9.80665, "ay", "%6.2f", "g"); misc_f32(nan(""), nan(""), p.accel[2] / 9.80665, "az", "%6.2f", "g"); diff --git a/boards/varmint_h7/common/drivers/Adis165xx.h b/boards/varmint_h7/common/drivers/Adis165xx.h index c63695f7..a75bf8a4 100644 --- a/boards/varmint_h7/common/drivers/Adis165xx.h +++ b/boards/varmint_h7/common/drivers/Adis165xx.h @@ -38,13 +38,15 @@ #ifndef ADIS165XX_H_ #define ADIS165XX_H_ +#include "DoubleBuffer.h" #include "BoardConfig.h" -#include "Driver.h" + #include "Spi.h" +#include "misc.h" #define ADIS_OK (0x0000) -class Adis165xx : public Driver +class Adis165xx : public Status , public MiscRotatable { public: uint32_t init( @@ -63,15 +65,25 @@ class Adis165xx : public Driver void endDma(void); bool startDma(void); - bool display(void) override; + bool display(void); bool isMy(uint16_t exti_pin) { return drdyPin_ == exti_pin; } bool isMy(SPI_HandleTypeDef * hspi) { return hspi == spi_.hspi(); } SPI_HandleTypeDef * hspi(void) { return spi_.hspi(); } void set_rotation(double rotation[9]) { memcpy(rotation_,&rotation, 9*sizeof(double));} + bool read(uint8_t * data, uint16_t size) { return (uint16_t)(double_buffer_.read(data, size)==DoubleBufferStatus::OK); } private: + bool write(uint8_t * data, uint16_t size) { return (uint16_t)(double_buffer_.write(data, size)==DoubleBufferStatus::OK); } + + DoubleBuffer double_buffer_; + + uint16_t sampleRateHz_; + uint64_t groupDelay_; // SPI Stuff Spi spi_; + uint16_t drdyPin_; + uint64_t drdy_; + uint16_t timeoutMs_; // ADIS165xx Stuff GPIO_TypeDef * resetPort_; diff --git a/boards/varmint_h7/common/drivers/Auav.cpp b/boards/varmint_h7/common/drivers/Auav.cpp index cbf67f97..d373e917 100644 --- a/boards/varmint_h7/common/drivers/Auav.cpp +++ b/boards/varmint_h7/common/drivers/Auav.cpp @@ -47,8 +47,8 @@ extern Time64 time64; DMA_RAM uint8_t auav_dma_txbuf[SPI_DMA_MAX_BUFFER_SIZE]; DMA_RAM uint8_t auav_dma_rxbuf[SPI_DMA_MAX_BUFFER_SIZE]; -DTCM_RAM uint8_t auav_pitot_fifo_rx_buffer[AUAV_FIFO_BUFFERS * sizeof(PressurePacket)]; -DTCM_RAM uint8_t auav_baro_fifo_rx_buffer[AUAV_FIFO_BUFFERS * sizeof(PressurePacket)]; +DTCM_RAM uint8_t auav_pitot_double_buffer[2 * sizeof(PressurePacket)]; +DTCM_RAM uint8_t auav_baro_double_buffer[2 * sizeof(PressurePacket)]; #define ROLLOVER 10000 @@ -99,7 +99,8 @@ uint32_t Auav::init(uint16_t sample_rate_hz, // { spi_[AUAV_PITOT].init(hspi, auav_dma_txbuf, auav_dma_rxbuf, pitot_cs_port, pitot_cs_pin); - rxFifo_[AUAV_PITOT].init(AUAV_FIFO_BUFFERS, sizeof(PressurePacket), auav_pitot_fifo_rx_buffer); + double_buffer_[AUAV_PITOT].init(auav_pitot_double_buffer, sizeof(auav_pitot_double_buffer)); + char name[] = "Auav (pitot)"; memset(name_local_[AUAV_PITOT], '\0', sizeof(name_local_[AUAV_PITOT])); strcpy(name_local_[AUAV_PITOT], name); @@ -126,7 +127,8 @@ uint32_t Auav::init(uint16_t sample_rate_hz, // { spi_[AUAV_BARO].init(hspi, auav_dma_txbuf, auav_dma_rxbuf, baro_cs_port, baro_cs_pin); - rxFifo_[AUAV_BARO].init(AUAV_FIFO_BUFFERS, sizeof(PressurePacket), auav_baro_fifo_rx_buffer); + double_buffer_[AUAV_BARO].init(auav_baro_double_buffer, sizeof(auav_baro_double_buffer)); + char name[] = "Auav (baro) "; memset(name_local_[AUAV_BARO], '\0', sizeof(name_local_[AUAV_BARO])); strcpy(name_local_[AUAV_BARO], name); @@ -176,7 +178,6 @@ uint32_t Auav::init(uint16_t sample_rate_hz, // } for (int i = 0; i < 2; i++) { - launchUs_[i] = 0; groupDelay_[i] = 0; // Read Status @@ -260,7 +261,6 @@ bool Auav::poll(uint64_t poll_counter) spiState_ = STATUS_IDLE; if ((dmaRunning_ = (HAL_OK == spi_[AUAV_BARO].startDma(cmdBytes_[AUAV_BARO], AUAV_CMD_BYTES)))) { spiState_ = STATUS_BARO_START; - launchUs_[AUAV_BARO] = time64.Us(); } } return false; @@ -273,7 +273,6 @@ void Auav::endDma(void) spiState_ = STATUS_IDLE; if ((dmaRunning_ = (HAL_OK == spi_[AUAV_PITOT].startDma(cmdBytes_[AUAV_PITOT], AUAV_CMD_BYTES)))) { spiState_ = STATUS_PITOT_START; - launchUs_[AUAV_PITOT] = time64.Us(); } } else if (spiState_ == STATUS_PITOT_START) { // Done starting Pitot, Wait for Pitot Read @@ -288,19 +287,21 @@ void Auav::endDma(void) PressurePacket p; makePacket(&p, inbuf, AUAV_PITOT); spiState_ = STATUS_IDLE; - //if(p.header.status==sensor_status_ready_[AUAV_PITOT]) // PTT uncomment if needed + if(p.header.status==sensor_status_ready_[AUAV_PITOT]) // PTT uncomment if needed { - rxFifo_[AUAV_PITOT].write((uint8_t *) &p, sizeof(p)); + p.header.complete = time64.Us(); + write2((uint8_t *) &p, sizeof(p), AUAV_PITOT); } } else if (spiState_ == STATUS_BARO_READ) { // Done starting Baro uint8_t * inbuf = spi_[AUAV_BARO].endDma(); // close chip select, data ignored PressurePacket p; makePacket(&p, inbuf, AUAV_BARO); spiState_ = STATUS_IDLE; - // if(p.header.status==sensor_status_ready_[AUAV_BARO]) // PTT uncomment this when we fix the sensor. + if(p.header.status==sensor_status_ready_[AUAV_BARO]) // PTT uncomment this when we fix the sensor. { - rxFifo_[AUAV_BARO].write((uint8_t *) &p, sizeof(p)); - } + p.header.complete = time64.Us(); + write2((uint8_t *) &p, sizeof(p), AUAV_BARO); + } } else { spiState_ = STATUS_IDLE; } @@ -382,21 +383,19 @@ void Auav::makePacket(PressurePacket * p, uint8_t * inbuf, uint8_t device) uint32_t Pdig = (uint32_t) (PCorrt * (double) 0xFFFFFF); #endif - p->drdy = drdy_[device]; - p->groupDelay = (drdy_[device] - launchUs_[device]) / 2; p->header.status = (uint16_t) inbuf[0]; p->temperature = (double) iTemp * 155.0 / 16777216.0 - 45.0 + 273.15; p->pressure = off_[device] + 1.25 * ((double) Pdig / 16777216.0 - osDig_[device]) * fss_[device]; - p->header.timestamp = time64.Us(); + p->header.timestamp = drdy_[device]; } bool Auav::display(void) { PressurePacket p; - if (rxFifoReadMostRecent((uint8_t *) &p, sizeof(p), AUAV_BARO)) { - misc_header(name_local_[AUAV_BARO], p.drdy, p.header.timestamp, p.groupDelay); + if (read2((uint8_t *) &p, sizeof(p), AUAV_BARO)) { + misc_header(name_local_[AUAV_BARO], p.header); misc_f32(99, 101, p.pressure / 1000., "Press", "%6.2f", "kPa"); misc_f32(18, 50, p.temperature - 273.15, "Temp", "%5.1f", "C"); @@ -407,8 +406,8 @@ bool Auav::display(void) misc_printf("%s\n", name_local_[AUAV_BARO]); } - if (rxFifoReadMostRecent((uint8_t *) &p, sizeof(p), AUAV_PITOT)) { - misc_header(name_local_[AUAV_PITOT], p.drdy, p.header.timestamp, p.groupDelay); + if (read2((uint8_t *) &p, sizeof(p), AUAV_PITOT)) { + misc_header(name_local_[AUAV_PITOT], p.header ); misc_f32(-5.0, 5.0, p.pressure, "Press", "%6.2f", "Pa"); misc_f32(18, 50, p.temperature - 273.15, "Temp", "%5.1f", "C"); misc_x16(sensorOk(AUAV_PITOT), p.header.status, "Status"); diff --git a/boards/varmint_h7/common/drivers/Auav.h b/boards/varmint_h7/common/drivers/Auav.h index aaa8ab5a..22d32433 100644 --- a/boards/varmint_h7/common/drivers/Auav.h +++ b/boards/varmint_h7/common/drivers/Auav.h @@ -40,8 +40,8 @@ #ifndef AUAV_H_ #define AUAV_H_ +#include "DoubleBuffer.h" #include "BoardConfig.h" -#include "Driver.h" #include "Packets.h" #include "Spi.h" @@ -50,7 +50,7 @@ #define AUAV_CMD_BYTES 3 -class Auav : public Driver +class Auav : public Status { /** * \brief @@ -77,18 +77,15 @@ class Auav : public Driver void drdyIsr(uint64_t timestamp, uint16_t exti_pin); - uint16_t rxFifoReadMostRecent(uint8_t * data, uint16_t size, uint8_t id) - { - return rxFifo_[id].readMostRecent(data, size); - } + bool read2(uint8_t * data, uint16_t size, uint8_t id) { return double_buffer_[id].read(data, size)==DoubleBufferStatus::OK; } + bool write2(uint8_t * data, uint16_t size, uint8_t id) { return double_buffer_[id].write(data, size)==DoubleBufferStatus::OK; } - uint16_t rxFifoReadMostRecent(uint8_t * data, uint16_t size) - { - return rxFifo_[AUAV_PITOT].readMostRecent(data, size); - } uint8_t sensorOk(uint8_t id) { return sensor_status_ready_[id]; } + bool read(uint8_t * data, uint16_t size) { return read2(data,size,AUAV_PITOT); } private: + bool write(uint8_t * data, uint16_t size) { return write2(data, size,AUAV_PITOT); } + void makePacket(PressurePacket * p, uint8_t * inbuff, uint8_t device); int32_t readCfg(uint8_t address, Spi * spi); // SPI Stuff @@ -102,11 +99,12 @@ class Auav : public Driver uint8_t sensor_status_ready_[2]; char name_local_[2][16]; // for display - PacketFifo rxFifo_[2]; + DoubleBuffer double_buffer_[2]; GPIO_TypeDef * drdyPort_[2]; uint16_t drdyPin_[2]; uint16_t sampleRateHz_; - uint64_t drdy_[2], timeout_[2], launchUs_[2]; + + uint64_t drdy_[2], timeout_[2]; uint64_t groupDelay_[2]; bool dmaRunning_; }; diff --git a/boards/varmint_h7/common/drivers/Bmi088.cpp b/boards/varmint_h7/common/drivers/Bmi088.cpp index e9389e73..f7d5e665 100644 --- a/boards/varmint_h7/common/drivers/Bmi088.cpp +++ b/boards/varmint_h7/common/drivers/Bmi088.cpp @@ -59,7 +59,7 @@ extern Time64 time64; DMA_RAM uint8_t bmi088_dma_txbuf[SPI_DMA_MAX_BUFFER_SIZE]; DMA_RAM uint8_t bmi088_dma_rxbuf[SPI_DMA_MAX_BUFFER_SIZE]; -DTCM_RAM uint8_t bmi088_fifo_rx_buffer[BMI088_FIFO_BUFFERS * sizeof(ImuPacket)]; +DTCM_RAM uint8_t bmi088_double_buffer[2 * sizeof(ImuPacket)]; uint32_t Bmi088::init( // Driver initializers @@ -80,7 +80,7 @@ uint32_t Bmi088::init( snprintf(name_, STATUS_NAME_MAX_LEN, "%s", "Bmi088"); initializationStatus_ = DRIVER_OK; sampleRateHz_ = sample_rate_hz; - drdyPort_ = drdy_port; + drdyPin_ = drdy_pin; spiA_.init(hspi, bmi088_dma_txbuf, bmi088_dma_rxbuf, cs_port_a, cs_pin_a); @@ -91,7 +91,8 @@ uint32_t Bmi088::init( rangeA_ = range_a; rangeG_ = range_g; - rxFifo_.init(BMI088_FIFO_BUFFERS, sizeof(ImuPacket), bmi088_fifo_rx_buffer); + double_buffer_.init( bmi088_double_buffer, sizeof(bmi088_double_buffer) ); + if (sampleRateHz_ <= 400) { sampleRateHz_ = 400; @@ -329,14 +330,12 @@ void Bmi088::endDma(void) // DMA complete routine data = (int16_t) rx[6] << 8 | (int16_t) rx[5]; p.gyro[2] = scale_factor * (double) data; - p.drdy = drdy_; - p.groupDelay = groupDelay_; - - p.header.timestamp = time64.Us(); + p.header.complete = time64.Us(); + p.header.timestamp = drdy_-groupDelay_; rotate(p.gyro); rotate(p.accel); - rxFifo_.write((uint8_t *) &p, sizeof(p)); + write((uint8_t *) &p, sizeof(p)); seqCount_ = 0; @@ -383,8 +382,8 @@ bool Bmi088::display(void) { ImuPacket p; char name[] = "Bmi088 (imu)"; - if (rxFifo_.readMostRecent((uint8_t *) &p, sizeof(p))) { - misc_header(name, p.drdy, p.header.timestamp, p.groupDelay); + if (read((uint8_t *) &p, sizeof(p))) { + misc_header(name, p.header ); misc_f32(nan(""), nan(""), p.accel[0] / 9.80665, "ax", "%6.2f", "g"); misc_f32(nan(""), nan(""), p.accel[1] / 9.80665, "ay", "%6.2f", "g"); misc_f32(nan(""), nan(""), p.accel[2] / 9.80665, "az", "%6.2f", "g"); diff --git a/boards/varmint_h7/common/drivers/Bmi088.h b/boards/varmint_h7/common/drivers/Bmi088.h index 26a66553..a3c1be1b 100644 --- a/boards/varmint_h7/common/drivers/Bmi088.h +++ b/boards/varmint_h7/common/drivers/Bmi088.h @@ -38,13 +38,14 @@ #ifndef BMI088_H_ #define BMI088_H_ -#include "Driver.h" +#include "DoubleBuffer.h" #include "Spi.h" +#include "misc.h" /* * */ -class Bmi088 : public Driver +class Bmi088 : public Status, public MiscRotatable { /** * \brief @@ -69,16 +70,25 @@ class Bmi088 : public Driver void endDma(void); bool startDma(void); - bool display(void) override; + bool display(void); bool isMy(uint16_t exti_pin) { return drdyPin_ == exti_pin; } bool isMy(SPI_HandleTypeDef * hspi) { return hspi == spiA_.hspi(); } SPI_HandleTypeDef * hspi(void) { return spiA_.hspi(); } + bool read(uint8_t * data, uint16_t size) { return double_buffer_.read(data, size)==DoubleBufferStatus::OK; } + private: + bool write(uint8_t * data, uint16_t size) { return double_buffer_.write(data, size)==DoubleBufferStatus::OK; } + DoubleBuffer double_buffer_; + uint16_t sampleRateHz_; + uint64_t groupDelay_; // SPI Stuff Spi spiA_; Spi spiG_; + uint16_t drdyPin_; + uint64_t drdy_; + uint16_t timeoutMs_; uint16_t seqCount_; // BMI088 Stuff diff --git a/boards/varmint_h7/common/drivers/DlhrL20G.cpp b/boards/varmint_h7/common/drivers/DlhrL20G.cpp index 3e0368a0..89160d7f 100644 --- a/boards/varmint_h7/common/drivers/DlhrL20G.cpp +++ b/boards/varmint_h7/common/drivers/DlhrL20G.cpp @@ -43,7 +43,7 @@ extern Time64 time64; //#define DLHRL20G_OK (0x40) DMA_RAM uint8_t dlhr_i2c_dma_buf[I2C_DMA_MAX_BUFFER_SIZE]; -DTCM_RAM uint8_t dlhr_fifo_rx_buffer[DLHRL20G_FIFO_BUFFERS * sizeof(PressurePacket)]; +DTCM_RAM uint8_t dlhr_double_buffer[2 * sizeof(PressurePacket)]; #define DLHR_I2C_STATUS_SIZE 1 #define DLHR_I2C_DMA_SIZE 7 @@ -63,10 +63,8 @@ uint32_t DlhrL20G::init( hi2c_ = hi2c; address_ = i2c_address << 1; - launchUs_ = 0; - // groupDelay_ = 0; //Computed later based on launchUs_ and drdy_ timestamps. - rxFifo_.init(DLHRL20G_FIFO_BUFFERS, sizeof(PressurePacket), dlhr_fifo_rx_buffer); + double_buffer_.init(dlhr_double_buffer, sizeof(dlhr_double_buffer) ); dtMs_ = 1000. / (double) sampleRateHz_; @@ -142,7 +140,6 @@ bool DlhrL20G::poll(uint64_t poll_counter) { dlhr_i2c_dma_buf[0] = cmdByte_; dlhr_i2c_dma_buf[1] = 0x00; - launchUs_ = time64.Us(); status = (HAL_OK == HAL_I2C_Master_Transmit_DMA(hi2c_, address_, dlhr_i2c_dma_buf, 1)); } else if (!previous_drdy && current_drdy) // drdy triggers a read. { @@ -160,9 +157,8 @@ void DlhrL20G::endDma(void) PressurePacket p; p.header.status = dlhr_i2c_dma_buf[0]; if (p.header.status == 0x0040) { - p.header.timestamp = time64.Us(); - p.drdy = drdy_; - p.groupDelay = (p.drdy - launchUs_) / 2; + p.header.timestamp = drdy_; + p.header.complete = time64.Us(); uint32_t i_pressure = (uint32_t) dlhr_i2c_dma_buf[1] << 16 | (uint32_t) dlhr_i2c_dma_buf[2] << 8 | (uint32_t) dlhr_i2c_dma_buf[3]; uint32_t i_temperature = @@ -174,7 +170,7 @@ void DlhrL20G::endDma(void) p.pressure = 1.25 * FS * ((double) i_pressure / 16777216.0 - OSdig); // Pa p.temperature = 125.0 * (double) i_temperature / 16777216.0 - 40.0 + 273.15; // K - if (p.header.status == DLHRL20G_OK) rxFifo_.write((uint8_t *) &p, sizeof(p)); + if (p.header.status == DLHRL20G_OK) write((uint8_t *) &p, sizeof(p)); } } @@ -182,9 +178,9 @@ bool DlhrL20G::display(void) { PressurePacket p; char name[] = "DlhrL20G (pitot)"; - if (rxFifo_.readMostRecent((uint8_t *) &p, sizeof(p))) { - misc_header(name, p.drdy, p.header.timestamp, p.groupDelay); - misc_f32(-2.5, 2.5, p.pressure, "Press", "%6.2f", "Pa"); + if (read((uint8_t *) &p, sizeof(p))) { + misc_header(name, p.header ); + misc_f32(-5, 5, p.pressure, "Press", "%6.2f", "Pa"); misc_f32(18, 50, p.temperature - 273.15, "Temp", "%5.1f", "C"); misc_x16(DLHRL20G_OK, p.header.status, "Status"); misc_printf("\n"); diff --git a/boards/varmint_h7/common/drivers/DlhrL20G.h b/boards/varmint_h7/common/drivers/DlhrL20G.h index 9a52fa49..e44bb0c3 100644 --- a/boards/varmint_h7/common/drivers/DlhrL20G.h +++ b/boards/varmint_h7/common/drivers/DlhrL20G.h @@ -38,8 +38,9 @@ #ifndef DLHRL20G_H_ #define DLHRL20G_H_ +#include "DoubleBuffer.h" #include "BoardConfig.h" -#include "Driver.h" + #include "Packets.h" #include "Time64.h" @@ -49,7 +50,7 @@ /* * */ -class DlhrL20G : public Driver +class DlhrL20G : public Status { /** * \brief @@ -69,11 +70,19 @@ class DlhrL20G : public Driver // I2C_HandleTypeDef* hi2c(void) {return hi2c_;} bool isMy(I2C_HandleTypeDef * hi2c) { return hi2c_ == hi2c; } + bool read(uint8_t * data, uint16_t size) { return double_buffer_.read(data, size)==DoubleBufferStatus::OK; } + private: + bool write(uint8_t * data, uint16_t size) { return double_buffer_.write(data, size)==DoubleBufferStatus::OK; } + DoubleBuffer double_buffer_; I2C_HandleTypeDef * hi2c_; uint16_t address_; uint8_t cmdByte_; double dtMs_; + GPIO_TypeDef * drdyPort_; + uint16_t drdyPin_; + uint64_t drdy_; + uint16_t sampleRateHz_; }; #endif /* DLHRL20G_H_ */ diff --git a/boards/varmint_h7/common/drivers/DoubleBuffer.h b/boards/varmint_h7/common/drivers/DoubleBuffer.h new file mode 100644 index 00000000..84b5fcbb --- /dev/null +++ b/boards/varmint_h7/common/drivers/DoubleBuffer.h @@ -0,0 +1,135 @@ +/** + ****************************************************************************** + * File : DoubleBuffer.h + * Date : June 6, 2025 + ****************************************************************************** + * + * Copyright (c) 2025, AeroVironment, Inc. + * 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 copyright holder 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 COPYRIGHT HOLDERS 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 COPYRIGHT HOLDER 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. + * + ****************************************************************************** + **/ + +#ifndef DRIVERS_DOUBLEBUFFER_H_ +#define DRIVERS_DOUBLEBUFFER_H_ + +#include "Status.h" + +#include +#include +#include + +enum class DoubleBufferStatus +{ + ERROR=-1, + EMPTY=0, + OK=1, +// OVERWRITE=2, +}; + +class DoubleBuffer : public Status +{ + public: + + uint32_t init(uint8_t * packet_buffer, size_t packet_sizeX2 ) + { + snprintf(name_, STATUS_NAME_MAX_LEN, "%s", "DoubleBuffer"); + initializationStatus_ = DRIVER_OK; + + size_ = packet_sizeX2/2; + + if (packet_buffer==NULL) + { + size_ = 0; + packet_[0] = NULL; + packet_[1] = NULL; + initializationStatus_ = DRIVER_NOT_INITIALIZED; + } + + packet_[0] = packet_buffer; + packet_[1] = packet_buffer + size_; + + reset(); + + return initializationStatus_; + } + + void reset(void) + { + rx_ = 0; + tx_ = 0; + memset(packet_[0],0,size_); + memset(packet_[1],0,size_); + } + + DoubleBufferStatus write(uint8_t * packet, size_t size) + { + if ((initializationStatus_ == DRIVER_NOT_INITIALIZED) || (packet==NULL) || (size != size_) ) return DoubleBufferStatus::ERROR; + memcpy(packet_[tx_], packet, size_); + if(rx_ != tx_) return DoubleBufferStatus::OK; //DoubleBufferStatus::OVERWRITE; // buffer not empty overwrote next value, still a good write. + tx_ = !tx_; + return DoubleBufferStatus::OK; + } + + DoubleBufferStatus read(uint8_t * packet, size_t size) + { + if ((initializationStatus_ == DRIVER_NOT_INITIALIZED) || (packet==NULL) || (size != size_) ) return DoubleBufferStatus::ERROR; + if(rx_ == tx_) return DoubleBufferStatus::EMPTY; // buffer empty + memcpy(packet, packet_[rx_], size_); + rx_ = !rx_; + return DoubleBufferStatus::OK; + } + +private: + volatile uint32_t rx_, tx_; + size_t size_; + uint8_t * packet_[2]; +}; + + + + + + + + + + + + + + + + + + + + + + +#endif /* DRIVERS_DOUBLEBUFFER_H_ */ diff --git a/boards/varmint_h7/common/drivers/Dps310.cpp b/boards/varmint_h7/common/drivers/Dps310.cpp index 35203815..11fbaaed 100644 --- a/boards/varmint_h7/common/drivers/Dps310.cpp +++ b/boards/varmint_h7/common/drivers/Dps310.cpp @@ -72,7 +72,7 @@ extern Time64 time64; DMA_RAM uint8_t dps310_dma_txbuf[SPI_DMA_MAX_BUFFER_SIZE]; DMA_RAM uint8_t dps310_dma_rxbuf[SPI_DMA_MAX_BUFFER_SIZE]; -DTCM_RAM uint8_t dps310_fifo_rx_buffer[DPS310_FIFO_BUFFERS * sizeof(PressurePacket)]; +DTCM_RAM uint8_t dps310_double_buffer[2 * sizeof(PressurePacket)]; #define ROLLOVER 20000 #define DPS310_CMD_P 0 @@ -122,8 +122,9 @@ uint32_t Dps310::init( initializationStatus_ = DRIVER_OK; sampleRateHz_ = sample_rate_hz; - drdyPort_ = drdy_port; + drdyPin_ = drdy_pin; + drdy_ = 0; spi_.init(hspi, dps310_dma_txbuf, dps310_dma_rxbuf, cs_port, cs_pin); spiState_ = DPS310_IDLE_STATE; @@ -133,7 +134,7 @@ uint32_t Dps310::init( // groupDelay_ = 1000000/sampleRateHz_; HAL_GPIO_WritePin(spi_.port_, spi_.pin_, GPIO_PIN_SET); - rxFifo_.init(DPS310_FIFO_BUFFERS, sizeof(PressurePacket), dps310_fifo_rx_buffer); + double_buffer_.init(dps310_double_buffer, sizeof(dps310_double_buffer) ); #define RESET 0x0C writeRegister(RESET, 0x09); @@ -313,8 +314,8 @@ bool Dps310::poll(uint64_t poll_counter) // Start P measurement sequence if (poll_state == DPS310_CMD_P) // Command Pressure Read { + drdy_ = time64.Us(); uint8_t cmd[2] = {MEAS_CFG | SPI_WRITE, 0x01}; - launchUs_ = time64.Us(); if ((dmaRunning_ = (HAL_OK == spi_.startDma(cmd, 2)))) spiState_ = DPS310_CMD_P; else spiState_ = DPS310_STATE_ERROR; } @@ -362,7 +363,7 @@ void Dps310::endDma(void) { if (rx[1] & 0x10) { p.header.status |= (uint16_t) rx[1]; - p.drdy = time64.Us(); + p.header.complete = time64.Us(); } } else if (spiState_ == DPS310_DRDY_T) // Temperature DRDY { @@ -378,9 +379,9 @@ void Dps310::endDma(void) double Praw = (double) praw / KP; p.pressure = C00_ + Praw * (C10_ + Praw * (C20_ + Praw * C30_)) + Traw * (C01_ + Praw * (C11_ + Praw * C21_)); // Pa - p.header.timestamp = time64.Us(); - p.groupDelay = p.header.timestamp - (p.drdy + launchUs_) / 2; - if (p.header.status == DPS310_OK) rxFifo_.write((uint8_t *) &p, sizeof(p)); + p.header.timestamp = drdy_; + p.header.complete = time64.Us(); + if (p.header.status == DPS310_OK) write((uint8_t *) &p, sizeof(p)); p.header.status = 0; } @@ -392,8 +393,8 @@ bool Dps310::display(void) { PressurePacket p; - if (rxFifo_.readMostRecent((uint8_t *) &p, sizeof(p))) { - misc_header(name_, p.drdy, p.header.timestamp, p.groupDelay); + if (read((uint8_t *) &p, sizeof(p))) { + misc_header(name_, p.header ); misc_f32(98, 101, p.pressure / 1000., "Press", "%6.2f", "kPa"); misc_f32(18, 50, p.temperature - 273.15, "Temp", "%5.1f", "C"); misc_x16(DPS310_OK, p.header.status, "Status"); diff --git a/boards/varmint_h7/common/drivers/Dps310.h b/boards/varmint_h7/common/drivers/Dps310.h index 60f56509..51b1feb9 100644 --- a/boards/varmint_h7/common/drivers/Dps310.h +++ b/boards/varmint_h7/common/drivers/Dps310.h @@ -38,17 +38,19 @@ #ifndef DPS310_H_ #define DPS310_H_ +#include "DoubleBuffer.h" #include "BoardConfig.h" -#include "Driver.h" + #include "Spi.h" #include "Time64.h" +#include "Polling.h" #define DPS310_OK (0xE0D0) /* * */ -class Dps310 : public Driver +class Dps310 : public Status { /** * \brief @@ -84,16 +86,25 @@ class Dps310 : public Driver // void endTxDma(void); - bool display(void) override; + bool display(void); bool isMy(uint16_t exti_pin) { return drdyPin_ == exti_pin; } bool isMy(SPI_HandleTypeDef * hspi) { return hspi == spi_.hspi(); } SPI_HandleTypeDef * hspi(void) { return spi_.hspi(); } + bool read(uint8_t * data, uint16_t size) { return double_buffer_.read(data, size)==DoubleBufferStatus::OK; } + private: + bool write(uint8_t * data, uint16_t size) { return double_buffer_.write(data, size)==DoubleBufferStatus::OK; } + uint64_t drdy_; + DoubleBuffer double_buffer_; + uint16_t sampleRateHz_; // SPI Stuff Spi spi_; PollingState spiState_; + uint16_t drdyPin_; + bool dmaRunning_; + uint16_t timeoutMs_; // Dps310 Stuff diff --git a/boards/varmint_h7/common/drivers/Driver.h b/boards/varmint_h7/common/drivers/Driver.h deleted file mode 100644 index 1543aff5..00000000 --- a/boards/varmint_h7/common/drivers/Driver.h +++ /dev/null @@ -1,82 +0,0 @@ -/** - ****************************************************************************** - * File : Driver.h - * Date : Sep 20, 2023 - ****************************************************************************** - * - * Copyright (c) 2023, AeroVironment, Inc. - * 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 copyright holder 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 COPYRIGHT HOLDERS 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 COPYRIGHT HOLDER 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. - * - ****************************************************************************** - **/ - -#ifndef DRIVER_H_ -#define DRIVER_H_ - -#include "stm32h7xx_hal.h" - -#include "BoardConfig.h" -#include "PacketFifo.h" -#include "Polling.h" -#include "Status.h" - -#include - -class Driver : public Status -{ -public: - Driver() { initializationStatus_ = DRIVER_NOT_INITIALIZED; } - // bool initGood(void) { return initializationStatus_ == DRIVER_OK; } - - virtual bool display(void) = 0; - - uint16_t rxFifoCount(void) { return rxFifo_.packetCount(); } - uint16_t rxFifoRead(uint8_t * data, uint16_t size) { return rxFifo_.read(data, size); } - uint16_t rxFifoReadMostRecent(uint8_t * data, uint16_t size) { return rxFifo_.readMostRecent(data, size); } - bool drdy(void) { return HAL_GPIO_ReadPin(drdyPort_, drdyPin_); } - bool dmaRunning(void) { return dmaRunning_; } - void rotate(double *x) { - double y[3]; - y[0] = x[0]*rotation_[0] + x[1]*rotation_[1] + x[2]*rotation_[2]; - y[1] = x[0]*rotation_[3] + x[1]*rotation_[4] + x[2]*rotation_[5]; - y[2] = x[0]*rotation_[6] + x[1]*rotation_[7] + x[2]*rotation_[8]; - memcpy(x,y,sizeof(double)*3); - } - -protected: - PacketFifo rxFifo_; - GPIO_TypeDef * drdyPort_; - uint16_t drdyPin_; - uint16_t sampleRateHz_; - uint64_t drdy_, timeout_, launchUs_; - uint64_t groupDelay_ = 0; - bool dmaRunning_ = 0; - double rotation_[9]; -}; - -#endif /* DRIVER_H_ */ diff --git a/boards/varmint_h7/common/drivers/Iis2mdc.cpp b/boards/varmint_h7/common/drivers/Iis2mdc.cpp index c3386ea8..9b365962 100644 --- a/boards/varmint_h7/common/drivers/Iis2mdc.cpp +++ b/boards/varmint_h7/common/drivers/Iis2mdc.cpp @@ -64,7 +64,7 @@ extern Time64 time64; DMA_RAM uint8_t iis2mdc_dma_txbuf[SPI_DMA_MAX_BUFFER_SIZE]; DMA_RAM uint8_t iis2mdc_dma_rxbuf[SPI_DMA_MAX_BUFFER_SIZE]; -DTCM_RAM uint8_t iis2mdc_fifo_rx_buffer[IIS2MDC_FIFO_BUFFERS * sizeof(MagPacket)]; +DTCM_RAM uint8_t iis2mdc_double_buffer[2 * sizeof(MagPacket)]; #define ROLLOVER 10000 #define IIS2MDC_CMD 0 @@ -87,14 +87,15 @@ uint32_t Iis2mdc::init( snprintf(name_, STATUS_NAME_MAX_LEN, "%s", "Iis2mdc"); initializationStatus_ = DRIVER_OK; sampleRateHz_ = sample_rate_hz; - drdyPort_ = drdy_port; - drdyPin_ = drdy_pin; + drdyPin_ = drdy_pin; + drdy_ = 0; spi_.init(hspi, iis2mdc_dma_txbuf, iis2mdc_dma_rxbuf, cs_port, cs_pin); HAL_GPIO_WritePin(spi_.port_, spi_.pin_, GPIO_PIN_SET); - rxFifo_.init(IIS2MDC_FIFO_BUFFERS, sizeof(MagPacket), iis2mdc_fifo_rx_buffer); + + double_buffer_.init(iis2mdc_double_buffer, sizeof(iis2mdc_double_buffer)); spiState_ = IIS2MDC_IDLE_STATE; dmaRunning_ = false; @@ -192,7 +193,7 @@ bool Iis2mdc::poll(uint64_t poll_counter) // Start measurement sequence if (poll_state == IIS2MDC_CMD) // Command Pressure Read { - launchUs_ = time64.Us(); + drdy_ = time64.Us(); uint8_t cmd[2] = {CFG_REG_A | SPI_WRITE, 0x81}; // CFG_REG_A if ((dmaRunning_ = (HAL_OK == spi_.startDma(cmd, 2)))) spiState_ = poll_state; else spiState_ = IIS2MDC_STATE_ERROR; @@ -219,7 +220,7 @@ void Iis2mdc::endDma(void) if (spiState_ == IIS2MDC_RX_H) // Flux Data and DRDY { memset(&p, 0, sizeof(p)); // clear p - p.drdy = time64.Us(); + p.header.complete = time64.Us(); p.header.status = rx[1]; int16_t data = (rx[3] << 8) | rx[2]; @@ -238,13 +239,12 @@ void Iis2mdc::endDma(void) int16_t data = (rx[2] << 8) | rx[1]; p.temperature = (double) data / 8.0 + 25.0 + 273.15; // K - p.header.timestamp = time64.Us(); - // 5 ms is added to group delay because of the set/reset averaging used in the flux measurement above. - p.groupDelay = p.header.timestamp - (p.drdy + launchUs_) / 2 + 5000; + p.header.timestamp = drdy_; + p.header.complete = time64.Us(); if (p.header.status == IIS2MDC_OK) { rotate(p.flux); - rxFifo_.write((uint8_t *) &p, sizeof(p)); + write((uint8_t *) &p, sizeof(p)); } } @@ -256,17 +256,17 @@ bool Iis2mdc::display() { MagPacket p; char name[] = "Iis2mdc (mag)"; - if (rxFifo_.readMostRecent((uint8_t *) &p, sizeof(p))) { + if (read((uint8_t *) &p, sizeof(p))) { float total_flux = sqrt(p.flux[0] * p.flux[0] + p.flux[1] * p.flux[1] + p.flux[2] * p.flux[2]); - misc_header(name, p.drdy, p.header.timestamp, p.groupDelay); + misc_header(name, p.header ); misc_f32(NAN, NAN, p.flux[0] * 1e6, "hx", "%6.2f", "uT"); misc_f32(NAN, NAN, p.flux[1] * 1e6, "hy", "%6.2f", "uT"); misc_f32(NAN, NAN, p.flux[2] * 1e6, "hz", "%6.2f", "uT"); - misc_f32(20, 70, total_flux * 1e6, "|h|", "%6.2f", "uT"); + misc_f32(20, 100, total_flux * 1e6, "|h|", "%6.2f", "uT"); misc_f32(18, 50, p.temperature - 273.15, "Temp", "%5.1f", "C"); misc_x16(IIS2MDC_OK, p.header.status, "Status"); misc_printf("\n"); diff --git a/boards/varmint_h7/common/drivers/Iis2mdc.h b/boards/varmint_h7/common/drivers/Iis2mdc.h index 44339f54..419a8edc 100644 --- a/boards/varmint_h7/common/drivers/Iis2mdc.h +++ b/boards/varmint_h7/common/drivers/Iis2mdc.h @@ -38,16 +38,19 @@ #ifndef IIS2MDC_H_ #define IIS2MDC_H_ +#include "DoubleBuffer.h" #include "BoardConfig.h" -#include "Driver.h" + #include "Spi.h" +#include "misc.h" +#include "Polling.h" #define IIS2MDC_OK (0x0F) /* * */ -class Iis2mdc : public Driver +class Iis2mdc : public Status, public MiscRotatable { /** * \brief @@ -68,19 +71,27 @@ class Iis2mdc : public Driver bool poll(uint64_t poll_counter); void endDma(void); - bool display(void) override; + bool display(void); bool isMy(uint16_t exti_pin) { return drdyPin_ == exti_pin; } bool isMy(SPI_HandleTypeDef * hspi) { return hspi == spi_.hspi(); } SPI_HandleTypeDef * hspi(void) { return spi_.hspi(); } + bool read(uint8_t * data, uint16_t size) { return double_buffer_.read(data, size)==DoubleBufferStatus::OK; } + private: + bool write(uint8_t * data, uint16_t size) { return double_buffer_.write(data, size)==DoubleBufferStatus::OK; } + DoubleBuffer double_buffer_; + uint16_t sampleRateHz_; // SPI Stuff Spi spi_; PollingState spiState_; - + uint16_t drdyPin_; + uint64_t drdy_; + bool dmaRunning_; void writeRegister(uint8_t address, uint8_t value); uint8_t readRegister(uint8_t address); + }; #endif /* IIS2MDC_H_ */ diff --git a/boards/varmint_h7/common/drivers/Ist8308.cpp b/boards/varmint_h7/common/drivers/Ist8308.cpp index ab7312aa..886b307a 100644 --- a/boards/varmint_h7/common/drivers/Ist8308.cpp +++ b/boards/varmint_h7/common/drivers/Ist8308.cpp @@ -49,7 +49,7 @@ extern Time64 time64; #define IST8308_IDLE_STATE 0xFFFF DMA_RAM uint8_t ist8308_i2c_dma_buf[I2C_DMA_MAX_BUFFER_SIZE]; -DTCM_RAM uint8_t ist8308_fifo_rx_buffer[IST8308_FIFO_BUFFERS * sizeof(MagPacket)]; +DTCM_RAM uint8_t ist8308_double_buffer[2 * sizeof(MagPacket)]; #define WAI_REG 0x0 #define DEVICE_ID 0x08 @@ -118,13 +118,13 @@ uint32_t Ist8308::init( hi2c_ = hi2c; address_ = i2c_address << 1; - launchUs_ = 0; - // groupDelay_ = 0; //Computed later based on launchUs_ and drdy_ timestamps. i2cState_ = IST8308_IDLE_STATE; dmaRunning_ = false; - rxFifo_.init(IST8308_FIFO_BUFFERS, sizeof(MagPacket), ist8308_fifo_rx_buffer); + double_buffer_.init(ist8308_double_buffer, sizeof(ist8308_double_buffer)); + + drdy_ = 0; dtMs_ = 1000. / (double) sampleRateHz_; @@ -201,7 +201,7 @@ bool Ist8308::poll(uint64_t poll_counter) { PollingState poll_state = (PollingState) (poll_counter % (ROLLOVER / POLLING_PERIOD_US)); if (poll_state == IST8308_CMD) { - launchUs_ = time64.Us(); +// drdy_ = time64.Us(); ist8308_i2c_dma_buf[0] = CNTL2_REG; ist8308_i2c_dma_buf[1] = CNTL2_VAL_SINGLE_MODE; @@ -230,9 +230,8 @@ void Ist8308::endDma(void) // else if (i2cState_ == IST8308_RX) { MagPacket p; - p.header.timestamp = time64.Us(); - p.drdy = drdy_; - p.groupDelay = p.header.timestamp - (launchUs_ + drdy_) / 2; + p.header.timestamp = drdy_; + p.header.complete = time64.Us(); p.header.status = ist8308_i2c_dma_buf[0]; if (p.header.status == STAT1_VAL_DRDY) @@ -249,7 +248,7 @@ void Ist8308::endDma(void) p.flux[2] = -(double) iflux * 1.1515e-7; // Tesla rotate(p.flux); - rxFifo_.write((uint8_t *) &p, sizeof(p)); + write((uint8_t *) &p, sizeof(p)); } } i2cState_ = IST8308_STATE_ERROR; @@ -259,8 +258,8 @@ bool Ist8308::display() { MagPacket p; char name[] = "Ist8308 (mag)"; - if (rxFifo_.readMostRecent((uint8_t *) &p, sizeof(p))) { - misc_header(name, p.drdy, p.header.timestamp, p.groupDelay); + if (read((uint8_t *) &p, sizeof(p))) { + misc_header(name, p.header ); misc_printf("%10.3f %10.3f %10.3f uT ", p.flux[0] * 1e6 + 10.9, p.flux[1] * 1e6 + 45.0, p.flux[2] * 1e6 - 37.5); misc_printf(" | "); diff --git a/boards/varmint_h7/common/drivers/Ist8308.h b/boards/varmint_h7/common/drivers/Ist8308.h index 7b78b459..160cdd30 100644 --- a/boards/varmint_h7/common/drivers/Ist8308.h +++ b/boards/varmint_h7/common/drivers/Ist8308.h @@ -38,12 +38,15 @@ #ifndef DRIVERS_IST8308_H_ #define DRIVERS_IST8308_H_ +#include "DoubleBuffer.h" #include "BoardConfig.h" -#include "Driver.h" + #include "Packets.h" #include "Time64.h" +#include "misc.h" +#include "Polling.h" -class Ist8308 : public Driver +class Ist8308 : public Status, public MiscRotatable { public: @@ -60,7 +63,15 @@ class Ist8308 : public Driver // I2C_HandleTypeDef* hi2c(void) {return hi2c_;} bool isMy(I2C_HandleTypeDef * hi2c) { return hi2c_ == hi2c; } + bool read(uint8_t * data, uint16_t size) { return double_buffer_.read(data, size)==DoubleBufferStatus::OK; } + private: + bool write(uint8_t * data, uint16_t size) { return double_buffer_.write(data, size)==DoubleBufferStatus::OK; } + DoubleBuffer double_buffer_; + uint16_t sampleRateHz_; + uint64_t drdy_; + bool dmaRunning_; + I2C_HandleTypeDef * hi2c_; PollingState i2cState_; uint16_t address_; diff --git a/boards/varmint_h7/common/drivers/Ms4525.cpp b/boards/varmint_h7/common/drivers/Ms4525.cpp index aa42e0d9..c088dfe7 100644 --- a/boards/varmint_h7/common/drivers/Ms4525.cpp +++ b/boards/varmint_h7/common/drivers/Ms4525.cpp @@ -43,7 +43,7 @@ extern Time64 time64; #define MS4525_OK (0x0000) DMA_RAM uint8_t ms4525_i2c_dma_buf[I2C_DMA_MAX_BUFFER_SIZE]; -DTCM_RAM uint8_t ms4525_fifo_rx_buffer[MS4525_FIFO_BUFFERS * sizeof(PressurePacket)]; +DTCM_RAM uint8_t ms4525_double_buffer[2 * sizeof(PressurePacket)]; #define MS4525_I2C_DMA_SIZE (4) @@ -71,12 +71,11 @@ uint32_t Ms4525::init( hi2c_ = hi2c; address_ = i2c_address << 1; - launchUs_ = 0; - // groupDelay_ = 0; //Computed later based on launchUs_ and drdy_ timestamps. - rxFifo_.init(MS4525_FIFO_BUFFERS, sizeof(PressurePacket), ms4525_fifo_rx_buffer); + double_buffer_.init(ms4525_double_buffer, sizeof(ms4525_double_buffer)); dtMs_ = 1000. / (double) sampleRateHz_; + drdy_ = 0; // Read the status register uint8_t sensor_status[2]; @@ -100,11 +99,8 @@ bool Ms4525::poll(uint64_t poll_counter) PollingState poll_state = (PollingState) (poll_counter % (ROLLOVER / POLLING_PERIOD_US)); if ((poll_state == MS4525_CMDRXSTART) || (poll_state == MS4525_CMDRX1) || (poll_state == MS4525_CMDRX2) || (poll_state == MS4525_CMDRX3) || (poll_state == MS4525_CMDRX4) || (poll_state == MS4525_CMDRXSEND)) { - if (poll_state == MS4525_CMDRXSTART) launchUs_ = time64.Us(); - - if ((dmaRunning_ = (HAL_OK - == HAL_I2C_Master_Receive_DMA(hi2c_, address_, ms4525_i2c_dma_buf, - MS4525_I2C_DMA_SIZE)))) // Receive 7 bytes of data over I2C + drdy_ = time64.Us(); + if (HAL_OK == HAL_I2C_Master_Receive_DMA(hi2c_, address_, ms4525_i2c_dma_buf, MS4525_I2C_DMA_SIZE)) // Receive 7 bytes of data over I2C i2cState_ = poll_state; else i2cState_ = MS4525_STATE_ERROR; } @@ -138,25 +134,23 @@ void Ms4525::endDma(void) p.temperature = (double) i_temperature * 200.0 / 2047.0 - 50.0 + 273.15; // K p.header.status = ms4525_i2c_dma_buf[0] & 0xC0; - p.header.timestamp = time64.Us(); - p.drdy = p.header.timestamp; - p.groupDelay = (p.header.timestamp - launchUs_) / 2; + p.header.timestamp = drdy_; + p.header.complete = time64.Us(); - rxFifo_.write((uint8_t *) &p, sizeof(p)); + write((uint8_t *) &p, sizeof(p)); } } } i2cState_ = MS4525_IDLE_STATE; - dmaRunning_ = false; } bool Ms4525::display(void) { PressurePacket p; char name[] = "MS4525 (pitot)"; - if (rxFifo_.readMostRecent((uint8_t *) &p, sizeof(p))) { - misc_header(name, p.drdy, p.header.timestamp, p.groupDelay); + if (read((uint8_t *) &p, sizeof(p))) { + misc_header(name, p.header ); misc_printf("%10.3f Pa | | " "%7.1f C | " " | 0x%04X", diff --git a/boards/varmint_h7/common/drivers/Ms4525.h b/boards/varmint_h7/common/drivers/Ms4525.h index 0e9d3c26..9a6435f6 100644 --- a/boards/varmint_h7/common/drivers/Ms4525.h +++ b/boards/varmint_h7/common/drivers/Ms4525.h @@ -38,16 +38,17 @@ #ifndef MS4525_H_ #define MS4525_H_ +#include "DoubleBuffer.h" #include "BoardConfig.h" -#include "Driver.h" #include "Packets.h" #include "Time64.h" +#include "Polling.h" #define MS4525_I2C_ADDRESS (0x28) /* * */ -class Ms4525 : public Driver +class Ms4525 : public Status { /** * \brief @@ -69,11 +70,17 @@ class Ms4525 : public Driver // I2C_HandleTypeDef* hi2c(void) {return hi2c_;} bool isMy(I2C_HandleTypeDef * hi2c) { return hi2c_ == hi2c; } + bool read(uint8_t * data, uint16_t size) { return double_buffer_.read(data, size)==DoubleBufferStatus::OK; } + private: + bool write(uint8_t * data, uint16_t size) { return double_buffer_.write(data, size)==DoubleBufferStatus::OK; } + DoubleBuffer double_buffer_; + uint16_t sampleRateHz_; I2C_HandleTypeDef * hi2c_; PollingState i2cState_; uint16_t address_; double dtMs_; + uint64_t drdy_; }; #endif /* DLHRL20G_H_ */ diff --git a/boards/varmint_h7/common/drivers/Packets.h b/boards/varmint_h7/common/drivers/Packets.h index bbf45b49..22540a06 100644 --- a/boards/varmint_h7/common/drivers/Packets.h +++ b/boards/varmint_h7/common/drivers/Packets.h @@ -57,8 +57,6 @@ typedef struct //__attribute__((__packed__)) typedef struct //__attribute__((__packed__)) { rosflight_firmware::PacketHeader header; - uint64_t drdy; // us, time of drdy signal (group delay is often known relative to this time) - uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) double temperature; double vBku; double vRef; @@ -68,9 +66,6 @@ typedef struct //__attribute__((__packed__)) typedef struct //__attribute__((__packed__)) { rosflight_firmware::PacketHeader header; - uint64_t drdy; // us, time of drdy signal (group delay is often known relative to this time) - uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) - // double gyro[3]; // rad/s double accel[3]; // rad/s double temperature; // K @@ -80,9 +75,6 @@ typedef struct //__attribute__((__packed__)) typedef struct //__attribute__((__packed__)) { rosflight_firmware::PacketHeader header; - uint64_t drdy; // us, time of drdy signal (group delay is often known relative to this time) - uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) - // double pressure; // Pa double temperature; // K } PressurePacket; @@ -90,9 +82,6 @@ typedef struct //__attribute__((__packed__)) typedef struct //__attribute__((packed)) { rosflight_firmware::PacketHeader header; - uint64_t drdy; // us, time of drdy signal (group delay is often known relative to this time) - uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) - // double flux[3]; // T, magnetic flux density double temperature; // K } MagPacket; @@ -101,13 +90,33 @@ typedef struct //__attribute__((packed)) typedef struct //__attribute__((__packed__)) { rosflight_firmware::PacketHeader header; - uint64_t drdy; // us, time of drdy signal (group delay is often known relative to this time) - uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) - // uint8_t nChan; float chan[RC_PACKET_CHANNELS]; bool frameLost; bool failsafeActivated; } RcPacket; + +//typedef struct GnssStruct GnssPacket; + +typedef struct //__attribute__((__packed__)) +{ + rosflight_firmware::PacketHeader header; + uint64_t pps; // most recent pps timestamp (us) + int64_t unix_seconds; // Unix time, in seconds + int32_t unix_nanos; + uint8_t fix_type; + uint8_t num_sat; + double lon; + double lat; + float height_msl; + float vel_n; + float vel_e; + float vel_d; + float h_acc; + float v_acc; + float speed_accy; +} GnssPacket; + + #endif /* DRIVERPACKETS_H_ */ diff --git a/boards/varmint_h7/common/drivers/Pwm.cpp b/boards/varmint_h7/common/drivers/Pwm.cpp index 3395ca1f..1638f509 100644 --- a/boards/varmint_h7/common/drivers/Pwm.cpp +++ b/boards/varmint_h7/common/drivers/Pwm.cpp @@ -37,7 +37,8 @@ #include "Pwm.h" #include "BoardConfig.h" -#include "Driver.h" + +#include // Notes on DSHOT // DSHOT Bitrate T1H T0H Bit(µs) Frame (µs) diff --git a/boards/varmint_h7/common/drivers/Pwm.h b/boards/varmint_h7/common/drivers/Pwm.h index 4b4beb90..394240ee 100644 --- a/boards/varmint_h7/common/drivers/Pwm.h +++ b/boards/varmint_h7/common/drivers/Pwm.h @@ -63,7 +63,7 @@ typedef enum PWM_DSHOT } pwm_type; -typedef struct __attribute__((__packed__)) +typedef struct // __attribute__((__packed__)) { TIM_HandleTypeDef * htim; pwm_type type; diff --git a/boards/varmint_h7/common/drivers/Sbus.cpp b/boards/varmint_h7/common/drivers/Sbus.cpp index 8bc938f2..fffe8933 100644 --- a/boards/varmint_h7/common/drivers/Sbus.cpp +++ b/boards/varmint_h7/common/drivers/Sbus.cpp @@ -75,7 +75,7 @@ typedef struct __attribute__((__packed__)) #define SBUS_DMA_BUFFER_SIZE (sizeof(SbusPacket) * 4) DMA_RAM uint8_t sbus_dma_rxbuf[SBUS_DMA_BUFFER_SIZE]; -DTCM_RAM uint8_t sbus_fifo_rx_buffer[SBUS_FIFO_BUFFERS * sizeof(RcPacket)]; +DTCM_RAM uint8_t sbus_double_buffer[2 * sizeof(RcPacket)]; uint32_t Sbus::init( // Driver initializers @@ -86,11 +86,9 @@ uint32_t Sbus::init( snprintf(name_, STATUS_NAME_MAX_LEN, "%s", "Sbus"); initializationStatus_ = DRIVER_OK; sampleRateHz_ = sample_rate_hz; - drdyPort_ = 0; // do not use - drdyPin_ = 0; // do not use dtimeout_ = 100000; // 0.1 seconds timeout_ = 0; - + drdy_ = 0; huart_ = huart; hdmaUartRx_ = hdma_uart_rx; @@ -126,7 +124,7 @@ uint32_t Sbus::init( } // USART initialization end - rxFifo_.init(SBUS_FIFO_BUFFERS, sizeof(RcPacket), sbus_fifo_rx_buffer); + double_buffer_.init(sbus_double_buffer, sizeof(sbus_double_buffer)); __HAL_UART_CLEAR_IDLEFLAG(huart_); __HAL_UART_DISABLE_IT(huart_, UART_IT_IDLE); @@ -150,6 +148,7 @@ bool Sbus::poll(void) bool Sbus::startDma(void) { + drdy_ = time64.Us(); timeout_ = time64.Us() + dtimeout_; // 0.1 second timeout HAL_StatusTypeDef hal_status = HAL_UART_Receive_DMA(huart_, sbus_dma_rxbuf, SBUS_DMA_BUFFER_SIZE); // start next read return HAL_OK == hal_status; @@ -157,7 +156,7 @@ bool Sbus::startDma(void) void Sbus::endDma(void) { - drdy_ = time64.Us(); +// drdy_ = time64.Us(); RcPacket p; SbusPacket * sbus = (SbusPacket *) sbus_dma_rxbuf; @@ -201,14 +200,13 @@ void Sbus::endDma(void) p.failsafeActivated = sbus->dig_chan3; for (int n = 0; n < RC_PACKET_CHANNELS; n++) p.chan[n] = (p.chan[n] - 172) / 1639.0; - p.header.timestamp = time64.Us(); // usTime(); - p.drdy = drdy_; - p.groupDelay = 9000; // Use one packet time latency for now. + p.header.timestamp = drdy_; + p.header.complete = time64.Us(); p.header.status = !(p.frameLost | p.failsafeActivated); lol_ = p.frameLost | p.failsafeActivated; - rxFifo_.write((uint8_t *) &p, sizeof(p)); + write((uint8_t *) &p, sizeof(p)); timeout_ = drdy_ + dtimeout_; // Give it a second before we say it's lost } startDma(); @@ -218,17 +216,17 @@ bool Sbus::display(void) { RcPacket p; char name[] = "Sbus (rc)"; - if (rxFifo_.readMostRecent((uint8_t *) &p, sizeof(p))) { - misc_header(name, p.drdy, p.header.timestamp, p.groupDelay); + if (read((uint8_t *) &p, sizeof(p))) { + misc_header(name, p.header ); for (int i = 0; i < 8; i++) misc_printf("[%2u]:%4.0f, ", i + 1, (p.chan[i] * 100.0)); misc_printf("%\n"); - misc_header(name, p.drdy, p.header.timestamp, p.groupDelay); + misc_header(name, p.header ); for (int i = 8; i < 16; i++) misc_printf("[%2u]:%4.0f, ", i + 1, (p.chan[i] * 100.0)); misc_printf("%\n"); - misc_header(name, p.drdy, p.header.timestamp, p.groupDelay); + misc_header(name, p.header ); for (int i = 16; i < 24; i++) misc_printf("[%2u]:%4.0f, ", i + 1, (p.chan[i] * 100.0)); misc_printf("%\n"); - misc_header(name, p.drdy, p.header.timestamp, p.groupDelay); + misc_header(name, p.header ); misc_printf("Frame Lost: %1u, Failsafe Activated: %1u, Status: %1u\n", p.frameLost, p.failsafeActivated, p.header.status); return 1; diff --git a/boards/varmint_h7/common/drivers/Sbus.h b/boards/varmint_h7/common/drivers/Sbus.h index 3d6c4d48..f9a67528 100644 --- a/boards/varmint_h7/common/drivers/Sbus.h +++ b/boards/varmint_h7/common/drivers/Sbus.h @@ -38,13 +38,13 @@ #ifndef SBUS_H_ #define SBUS_H_ +#include "DoubleBuffer.h" #include "BoardConfig.h" -#include "Driver.h" /* * */ -class Sbus : public Driver +class Sbus : public Status { /** * \brief @@ -61,14 +61,22 @@ class Sbus : public Driver bool poll(void); void endDma(void); bool startDma(void); - bool display(void) override; + bool display(void); bool lol(void) { return lol_; } UART_HandleTypeDef * huart(void) { return huart_; } bool isMy(UART_HandleTypeDef * huart) { return huart_ == huart; } + bool read(uint8_t * data, uint16_t size) { return double_buffer_.read(data, size)==DoubleBufferStatus::OK; } + private: + bool write(uint8_t * data, uint16_t size) { return double_buffer_.write(data, size)==DoubleBufferStatus::OK; } + DoubleBuffer double_buffer_; + uint16_t sampleRateHz_; bool lol_; + uint64_t drdy_; + uint64_t timeout_; + uint64_t dtimeout_; UART_HandleTypeDef * huart_; DMA_HandleTypeDef * hdmaUartRx_; diff --git a/boards/varmint_h7/common/drivers/Sd.cpp b/boards/varmint_h7/common/drivers/Sd.cpp index fca7da04..056b4b8d 100644 --- a/boards/varmint_h7/common/drivers/Sd.cpp +++ b/boards/varmint_h7/common/drivers/Sd.cpp @@ -40,6 +40,7 @@ #include "misc.h" #include +#include extern Time64 time64; diff --git a/boards/varmint_h7/common/drivers/Sd.h b/boards/varmint_h7/common/drivers/Sd.h index a65d567f..6b1d63f1 100644 --- a/boards/varmint_h7/common/drivers/Sd.h +++ b/boards/varmint_h7/common/drivers/Sd.h @@ -39,7 +39,8 @@ #define SD_H_ #include "BoardConfig.h" -#include "Driver.h" +#include "Status.h" + /* * */ diff --git a/boards/varmint_h7/common/drivers/Spi.h b/boards/varmint_h7/common/drivers/Spi.h index 1e9ba830..ac989fed 100644 --- a/boards/varmint_h7/common/drivers/Spi.h +++ b/boards/varmint_h7/common/drivers/Spi.h @@ -41,6 +41,7 @@ #include "stm32h7xx_hal.h" #include // for memset +#include #include "BoardConfig.h" #include "Status.h" @@ -135,21 +136,6 @@ class Spi : public Status return hal_status; } - // HAL_StatusTypeDef startTxDma(uint8_t tx_byte, uint16_t size) - // { - // bool software_nss = !(hspi_->Init.NSS == SPI_NSS_HARD_OUTPUT); - // - // memset(txBuffer_,0,SPI_DMA_MAX_BUFFER_SIZE); - // txBuffer_[0] = tx_byte; - // - // if(software_nss) HAL_GPIO_WritePin(port_, pin_, GPIO_PIN_RESET); - // HAL_StatusTypeDef hal_status = HAL_SPI_Transmit_DMA(hspi_, txBuffer_, size); - // if((HAL_OK!=hal_status) && (software_nss)) HAL_GPIO_WritePin(port_, pin_, GPIO_PIN_SET); - // time64.dUs(2); - // - // return hal_status; - // } - HAL_StatusTypeDef startTxDma(uint8_t * tx_bytes, uint16_t size) { bool software_nss = !(hspi_->Init.NSS == SPI_NSS_HARD_OUTPUT); diff --git a/boards/varmint_h7/common/drivers/Status.h b/boards/varmint_h7/common/drivers/Status.h index c0c7efd9..861882c4 100644 --- a/boards/varmint_h7/common/drivers/Status.h +++ b/boards/varmint_h7/common/drivers/Status.h @@ -38,6 +38,9 @@ #ifndef STATUS_H_ #define STATUS_H_ +#include +#include + #define STATUS_LIST_MAX_LEN 16 #define STATUS_NAME_MAX_LEN 16 diff --git a/boards/varmint_h7/common/drivers/Telem.cpp b/boards/varmint_h7/common/drivers/Telem.cpp index 693b13c3..8a0579ef 100644 --- a/boards/varmint_h7/common/drivers/Telem.cpp +++ b/boards/varmint_h7/common/drivers/Telem.cpp @@ -46,9 +46,11 @@ extern Time64 time64; -#define TELEM_RX_BUFFER_SIZE (4096) // Use a multiple of 32! +//#define TELEM_RX_BUFFER_SIZE (4096-32) // Use a multiple of 32! +//DTCM_RAM uint8_t telem_fifo_rx_buffer[TELEM_RX_BUFFER_SIZE]; -DTCM_RAM uint8_t telem_fifo_rx_buffer[TELEM_RX_BUFFER_SIZE]; +#define TELEM_RX_BUFFER_SIZE (8*1024) +static uint8_t telem_fifo_rx_buffer[TELEM_RX_BUFFER_SIZE]; #define TELEM_DMA_TX_BUFFER_SIZE (SERIAL_MAX_PAYLOAD_SIZE) DMA_RAM uint8_t telem_dma_txbuf[TELEM_DMA_TX_BUFFER_SIZE]; @@ -62,8 +64,8 @@ DTCM_RAM uint8_t telem_fifo_tx_buffer1[SERIAL_TX_FIFO_BUFFERS1 * sizeof(SerialTx DTCM_RAM uint8_t telem_fifo_tx_buffer2[SERIAL_TX_FIFO_BUFFERS2 * sizeof(SerialTxPacket)]; DTCM_RAM PacketFifo telem_tx_fifos[SERIAL_QOS_FIFOS]; -DATA_RAM uint8_t * telem_fifo_tx_buffer[SERIAL_QOS_FIFOS] = {telem_fifo_tx_buffer0, telem_fifo_tx_buffer1, - telem_fifo_tx_buffer2}; +DATA_RAM uint8_t * telem_fifo_tx_buffer[SERIAL_QOS_FIFOS] = + {telem_fifo_tx_buffer0, telem_fifo_tx_buffer1, telem_fifo_tx_buffer2}; uint32_t Telem::init( // Driver initializers diff --git a/boards/varmint_h7/common/drivers/Telem.h b/boards/varmint_h7/common/drivers/Telem.h index f121b8bd..7fae30f6 100644 --- a/boards/varmint_h7/common/drivers/Telem.h +++ b/boards/varmint_h7/common/drivers/Telem.h @@ -40,9 +40,9 @@ #include "BoardConfig.h" #include "ByteFifo.h" -#include "Driver.h" #include "Packets.h" #include "Time64.h" +#include "PacketFifo.h" extern Time64 time64; enum DmaItType diff --git a/boards/varmint_h7/common/drivers/Time64.h b/boards/varmint_h7/common/drivers/Time64.h index 0c759917..0b8fee26 100644 --- a/boards/varmint_h7/common/drivers/Time64.h +++ b/boards/varmint_h7/common/drivers/Time64.h @@ -39,9 +39,10 @@ #define TIME64_H_ #include "BoardConfig.h" -#include "Driver.h" #include "Status.h" #include +#include +#include class Time64 : public Status { diff --git a/boards/varmint_h7/common/drivers/Ubx.cpp b/boards/varmint_h7/common/drivers/Ubx.cpp index 62404ca3..fc4237f2 100644 --- a/boards/varmint_h7/common/drivers/Ubx.cpp +++ b/boards/varmint_h7/common/drivers/Ubx.cpp @@ -49,21 +49,28 @@ extern Time64 time64; #define UBX_DMA_BUFFER_SIZE 16 * 2 // must be multiple of 16 DMA_RAM uint8_t ubx_dma_rxbuf[UBX_DMA_BUFFER_SIZE]; -DTCM_RAM uint8_t ubx_fifo_rx_buffer[UBX_FIFO_BUFFERS * sizeof(UbxPacket)]; +DTCM_RAM uint8_t ubx_double_buffer[2 * sizeof(UbxPacket)]; + +void Ubx::pps(uint64_t pps_timestamp) +{ + static bool first_time = true; + if(!first_time) ubx_.pps = pps_timestamp; + first_time = false; +} + uint32_t Ubx::init( // Driver initializers - uint16_t sample_rate_hz, GPIO_TypeDef * drdy_port, uint16_t drdy_pin, bool has_pps, + uint16_t sample_rate_hz, GPIO_TypeDef * pps_port, uint16_t pps_pin, // UART initializers - UART_HandleTypeDef * huart, USART_TypeDef * huart_instance, DMA_HandleTypeDef * hdma_uart_rx, uint32_t baud, - UbxProtocol ubx_protocol) + UART_HandleTypeDef * huart, USART_TypeDef * huart_instance, DMA_HandleTypeDef * hdma_uart_rx, uint32_t baud) { snprintf(name_, STATUS_NAME_MAX_LEN, "%s", "Ubx"); initializationStatus_ = DRIVER_OK; sampleRateHz_ = sample_rate_hz; - drdyPort_ = drdy_port; - drdyPin_ = drdy_pin; - hasPps_ = has_pps; + + ppsPin_ = pps_pin; + ppsHz_ = 1; // To match top of second. dtimeout_ = 1000000; // 1 seconds timeout_ = 0; @@ -74,15 +81,10 @@ uint32_t Ubx::init( baud_initial_ = 9600; baud_ = baud; - ubxProtocol_ = ubx_protocol; - - groupDelay_ = 0; + ubx_.pps = 0; // gotNav_ = false; gotPvt_ = 0; - gotTime_ = 0; - gotEcefP_ = 0; - gotEcefV_ = 0; // USART initialization huart_->Instance = huart_instance; @@ -118,7 +120,7 @@ uint32_t Ubx::init( } // baud_initial_ = 100000000/huart_->Instance->BRR; - uint32_t bauds[] = {9600, 230400, 57600, 460800}; // { 9600, 19200, 38400, 57600, 115200, 230400}; + uint32_t bauds[] = {9600, 115200, 38400, 57600, 230400}; // { 9600, 19200, 38400, 57600, 115200, 230400, 460800}; unsigned int i, retry; HAL_Delay(1000); @@ -133,8 +135,7 @@ uint32_t Ubx::init( return initializationStatus_; } // Set UBLOX Baud - if (ubxProtocol_ == UBX_M8) cfgPrt(baud_); - else cfgM9(baud_, sampleRateHz_); + cfgPrt(baud_); HAL_Delay(2); } @@ -147,8 +148,7 @@ uint32_t Ubx::init( HAL_Delay(100); // Give the UBLOX some time to get there // Check if we have acquired the baud rate - if (ubxProtocol_ == UBX_M8) ubx_baud = pollBaud(); - else ubx_baud = pollBaudM9(); + ubx_baud = pollBaud(); if (ubx_baud == baud_) break; misc_printf("%6lu, %u retries\n", 100000000 / huart_->Instance->BRR, retry); @@ -167,7 +167,7 @@ uint32_t Ubx::init( return initializationStatus_; } - rxFifo_.init(UBX_FIFO_BUFFERS, sizeof(UbxPacket), ubx_fifo_rx_buffer); + double_buffer_.init(ubx_double_buffer, sizeof(ubx_double_buffer) ); // Disable these messages to get rid of clutter uint16_t error = 0; @@ -181,19 +181,21 @@ uint32_t Ubx::init( error |= (uint16_t) cfgMsg(0x01, 0x20, 0); // NAV-TIMEGPS error |= (uint16_t) cfgMsg(0x01, 0x01, 0); // NAV-POSECEF (length 20) error |= (uint16_t) cfgMsg(0x01, 0x11, 0); // NAV-VELECEF (length 20) + error |= (uint16_t) cfgMsg(0x01, 0x20, 0); // NAV-TIMEGPS (length 16) + error |= (uint16_t) cfgMsg(0x01, 0x01, 0); // NAV-POSECEF (length 20) + error |= (uint16_t) cfgMsg(0x01, 0x11, 0); // NAV-VELECEF (length 20) // Enable these messages - error |= (uint16_t) cfgMsg(0x01, 0x20, 1); // NAV-TIMEGPS (length 16) + //error |= (uint16_t) cfgMsg(0x01, 0x20, 1); // NAV-TIMEGPS (length 16) error |= (uint16_t) cfgMsg(0x01, 0x07, 1); // NAV-PVT (length 92) - error |= (uint16_t) cfgMsg(0x01, 0x01, 1); // NAV-POSECEF (length 20) - error |= (uint16_t) cfgMsg(0x01, 0x11, 1); // NAV-VELECEF (length 20) + //error |= (uint16_t) cfgMsg(0x01, 0x01, 1); // NAV-POSECEF (length 20) + //error |= (uint16_t) cfgMsg(0x01, 0x11, 1); // NAV-VELECEF (length 20) // Set GPS Configuration (already done in pollCfgPrtM9() for UBX_M9) - if (ubxProtocol_ == UBX_M8) { - error |= (uint16_t) cfgRate(sampleRateHz_); // Nav rate 0x06 0x08 - error |= (uint16_t) cfgTp5(sampleRateHz_); // PPS rate 0x06 0x31 - error |= (uint16_t) cfgNav5(); // airplane mode 0x06 0x24 - } + error |= (uint16_t) cfgRate(sampleRateHz_); // Nav rate 0x06 0x08 + error |= (uint16_t) cfgTp5(ppsHz_); // PPS rate 0x06 0x31 + error |= (uint16_t) cfgNav5(); // airplane mode 0x06 0x24 + __HAL_UART_CLEAR_IDLEFLAG(huart_); __HAL_UART_DISABLE_IT(huart_, UART_IT_IDLE); @@ -233,7 +235,6 @@ void Ubx::endDma(void) if (found) { if ((p.cl == 0x01) && (p.id == 0x07)) { - gotPvt_ = time64.Us(); memcpy((uint8_t *) &(ubx_.pvt), p.payload, sizeof(ubx_.pvt)); struct tm tm; tm.tm_sec = ubx_.pvt.sec; @@ -243,37 +244,31 @@ void Ubx::endDma(void) tm.tm_mon = ubx_.pvt.month - 1; tm.tm_year = ubx_.pvt.year - 1900; ubx_.unix_seconds = mktime(&tm); + ubx_.unix_nanos = ubx_.pvt.nano; + if (ubx_.pvt.nano<0) { - ubx_.unix_seconds -= 1; - ubx_.pvt.nano += 1000000000; + ubx_.unix_seconds--; + ubx_.unix_nanos += 1000000000; } - } else if ((p.cl == 0x01) && (p.id == 0x20)) { - gotTime_ = time64.Us(); - memcpy((uint8_t *) (&ubx_.time), p.payload, sizeof(ubx_.time)); - } else if ((p.cl == 0x01) && (p.id == 0x01)) { - gotEcefP_ = time64.Us(); - memcpy((uint8_t *) (&ubx_.ecefp), p.payload, sizeof(ubx_.ecefp)); - } else if ((p.cl == 0x01) && (p.id == 0x11)) { - gotEcefV_ = time64.Us(); - memcpy((uint8_t *) (&ubx_.ecefv), p.payload, sizeof(ubx_.ecefv)); - } - - if (gotPvt_ && gotTime_ && gotEcefP_ && gotEcefV_) { - ubx_.header.timestamp = time64.Us(); // usTime(); - ubx_.drdy = drdy_; - if (!hasPps_) ubx_.pps = 0; //ubx_.drdy - 25000; // fake number if we don't have PPS hooked up + gotPvt_ = time64.Us(); + if ( (ubx_.pps !=0) && (ubx_.pps < gotPvt_) && ((ubx_.pvt.valid & 0x07)== 0x07) && ((ubx_.pvt.flags & 0x01)==0x01) ) + { + if(ubx_.pvt.nano<0) + { + ubx_.header.timestamp = ubx_.pps - (uint64_t)(-ubx_.pvt.nano/1000); + } else { + ubx_.header.timestamp = ubx_.pps + (uint64_t)(ubx_.pvt.nano/1000); + } + } else { + ubx_.header.timestamp = gotPvt_-22000; // + } - if (ubx_.pps > ubx_.drdy) ubx_.pps -= 1000000 / sampleRateHz_; - ubx_.groupDelay = ubx_.drdy - ubx_.pps; + ubx_.header.complete = gotPvt_; - rxFifo_.write((uint8_t *) &ubx_, sizeof(ubx_)); + write((uint8_t *) &ubx_, sizeof(ubx_)); gotPvt_ = 0; - gotTime_ = 0; - gotEcefP_ = 0; - gotEcefV_ = 0; - drdy_ = 0; } } } @@ -315,7 +310,6 @@ bool Ubx::parseByte(uint8_t c, UbxFrame * p) p->A += c; p->B += p->A; - if ((p->cl == 0x01) && (p->id == 0x07)) drdy_ = time64.Us() - UBX_DMA_BUFFER_SIZE * 10000000 / baud_; } else if (n == 4) // length LSB { p->length = (uint16_t) c; @@ -349,50 +343,7 @@ bool Ubx::parseByte(uint8_t c, UbxFrame * p) return false; } -void Ubx::pps(uint64_t pps_timestamp) { ubx_.pps = pps_timestamp; } - -bool Ubx::display(void) -{ - UbxPacket p; - - char name_pvt[] = "Ubx (pvt)"; - char name_time[] = "Ubx (time)"; - char name_ecefp[] = "Ubx (ecefp)"; - char name_ecefv[] = "Ubx (ecefv)"; - - if (rxFifo_.read((uint8_t *) &p, sizeof(p))) { - misc_header(name_pvt, p.drdy, p.header.timestamp, p.groupDelay); - misc_printf("%10.3f ms | ", (double) (p.header.timestamp - p.pps) / 1000.); - misc_printf(" iTOW %10u | ", p.pvt.iTOW); - misc_printf("%02u/%02u/%04u ", p.pvt.month, p.pvt.day, p.pvt.year); - misc_printf("%02u:%02u:%09.6f", p.pvt.hour, p.pvt.min, (double) p.pvt.sec + (double) p.pvt.nano * 1e-9); - misc_printf("%14.8f deg %14.8f deg | ", (double) p.pvt.lat * 1e-7, (double) p.pvt.lon * 1e-7); - misc_printf("numSV %02d | ", p.pvt.numSV); - misc_printf("Fix %02d\n", p.pvt.fixType); - - misc_header(name_time, p.drdy, p.header.timestamp, p.groupDelay); - misc_printf("%10.3f ms | ", (double) (p.header.timestamp - p.pps) / 1000.); - misc_printf(" iTOW %10u | ", p.time.iTOW); - misc_printf(" TOW %14.3f ms | valid 0x%02X\n", (double) p.time.iTOW + (double) p.time.fTOW / 1000, p.time.valid); - - misc_header(name_ecefp, p.drdy, p.header.timestamp, p.groupDelay); - misc_printf("%10.3f ms | ", (double) (p.header.timestamp - p.pps) / 1000.); - misc_printf(" iTOW %10u | ", p.ecefp.iTOW); - misc_printf(" %10d %10d %10d %10u cm\n", p.ecefp.ecefX, p.ecefp.ecefY, p.ecefp.ecefZ, p.ecefp.pAcc); - - misc_header(name_ecefv, p.drdy, p.header.timestamp, p.groupDelay); - misc_printf("%10.3f ms | ", (double) (p.header.timestamp - p.pps) / 1000.); - misc_printf(" iTOW %10u | ", p.ecefv.iTOW); - misc_printf(" %10d %10d %10d %10u cm/s\n", p.ecefv.ecefVX, p.ecefv.ecefVY, p.ecefv.ecefVZ, p.ecefv.sAcc); - } else { - misc_printf("%s\n", name_pvt); - misc_printf("%s\n", name_time); - misc_printf("%s\n", name_ecefp); - misc_printf("%s\n", name_ecefv); - } - return 1; -} /////////////////////////////////////////////////////////////////////////////// // Packet Stuff @@ -510,22 +461,22 @@ uint16_t Ubx::cfgTp5(uint32_t hz) #define SFG_TP5_LENGTH 32 uint8_t message[SFG_TP5_LENGTH + 8] = {0}; uint8_t * payload = message + 6; - uint32_t period_us = 1000000 / hz; +// uint32_t period_us = 1000000 / hz; uint32_t pulse_len_us = 1000; header(message, 0x06, 0x31, SFG_TP5_LENGTH); payload[0] = 0x00; // Timepulse pin 0 - payload[1] = 0x01; // Version 1 + payload[1] = 0x00; // Version 0 payload[2] = 0x00; // reserved payload[3] = 0x00; // reserved SET(payload + 4, 0x0000, int16_t); // antenna delay SET(payload + 6, 0x0000, int16_t); // rf delay - SET(payload + 8, period_us, uint32_t); - SET(payload + 12, period_us / 2, uint32_t); - SET(payload + 16, pulse_len_us, uint32_t); // no pulse when not locked =0 ? - SET(payload + 20, pulse_len_us / 2, uint32_t); + SET(payload + 8, hz, uint32_t); // frequency when not locked + SET(payload + 12, hz, uint32_t); // frequency when locked + SET(payload + 16, 0, uint32_t); // pulse length when not locked (1ms) + SET(payload + 20, pulse_len_us, uint32_t); // pulse length when locked (1ms) SET(payload + 24, 0x0000, uint32_t); // delay - SET(payload + 28, 0x01F7, uint32_t); // 0001 1111 1111 0111 = 0x01F7 + SET(payload + 28, 0x007F, uint32_t); // 0x0111 1111 return tx(message, SFG_TP5_LENGTH); } @@ -556,99 +507,43 @@ uint16_t Ubx::cfgMsg(uint8_t cl, uint8_t id, uint8_t decimation_rate) return tx(message, CFG_MSG_LENGTH); } -////////////////////////////////////////////////////////////////////////////////////////// -// M9 Protocol +bool Ubx::display(void) +{ + UbxPacket p; -//#define SET(buf,data,type) *((type*)(buf))=data + char name_pvt[] = "Ubx (pvt)"; -// M9 parameters setting functions -#define SETVAL(ptr, value, type) \ - { \ - *((type *) (ptr)) = value; \ - ptr += sizeof(type); \ - } + if (read((uint8_t *) &p, sizeof(p))) { -#define SETKV(ptr, key, value, type) \ - { \ - *(uint32_t *) (ptr) = key; \ - ptr += 4; \ - *((type *) (ptr)) = value; \ - ptr += sizeof(type); \ - } + static double lag = 0; + if(p.header.complete>p.header.timestamp) + { + lag = (lag*0.99 + 0.01*(double)(p.header.complete-p.header.timestamp)); + } + struct tm *gmt; + time_t seconds = p.unix_seconds; + gmt = gmtime(&seconds); -uint16_t Ubx::cfgM9(uint32_t baud, uint16_t sampleRateHz) -{ - uint8_t cfg_message[64] = {0}; - uint16_t length = 0; - uint8_t * p = cfg_message; - // Header - SETVAL(p, 0xB5, uint8_t); // mu (1 bytes) - SETVAL(p, 0x62, uint8_t); // b (1 bytes) - SETVAL(p, 0x06, uint8_t); // class CFG (1 bytes) - SETVAL(p, 0x8A, uint8_t); // id Write Value (1 bytes) - p += 2; // space for length (2 bytes) - // Message - SETVAL(p, 0x00, uint8_t); // Message Version (1 bytes) - SETVAL(p, 0x01, uint8_t); // Write to RAM bit 1 is ram, 2 is bbr layer, 3 is flash (1 bytes) - p += 2; // reserved (2 bytes) - // Key value pairs - SETKV(p, 0x40520001, baud, uint32_t); // baud rate (8 bytes) - SETKV(p, 0x30210001, 1000 / sampleRateHz, uint16_t); // output rate in milliseconds (6 bytes) - SETKV(p, 0x30210002, 1, uint16_t); // 1 data output per nav measurement (6 bytes) - SETKV(p, 0x20110021, 8, uint8_t); // CFG-NAVSPG-DYNMODEL 8 = 4G Airborne (5 bytes) - SETKV(p, 0x20110011, 3, uint8_t); // CFG-NAVSPG-FIXMODE 3 = Auto 2/3D (5 bytes) - - // compute and insert message length length - length = p - cfg_message - 6; - SET(cfg_message + 4, length, uint16_t); - - return tx(cfg_message, length); -} -uint32_t Ubx::pollBaudM9(void) -{ -#define CFG_MESSAGE_SIZE 64 - uint8_t cfg_message[CFG_MESSAGE_SIZE] = {0}; - uint16_t length = 0; - uint8_t * p = cfg_message; - // Header - SETVAL(p, 0xB5, uint8_t); // mu (1 bytes) - SETVAL(p, 0x62, uint8_t); // b (1 bytes) - SETVAL(p, 0x06, uint8_t); // class CFG (1 bytes) - SETVAL(p, 0x8B, uint8_t); // id Write Value (1 bytes) - p += 2; // space for length (2 bytes) - // Message - SETVAL(p, 0x00, uint8_t); // Message Version (1 bytes) - SETVAL(p, 0x00, uint8_t); // Read to RAM - SETVAL(p, 0x0000, uint16_t); // Offset (position) - - SETVAL(p, 0x40520001, uint32_t); // baud - - // compute and insert message length length - length = p - cfg_message - 6; - SET(cfg_message + 4, length, uint16_t); - - if (tx(cfg_message, length) != HAL_OK) return 0; + misc_header(name_pvt, p.header ); + misc_printf("| pps %10.6f s | ", (double)p.pps * 1e-6); + misc_printf(" iTOW %10.3f s | ", (double)p.pvt.iTOW/1000); - UbxFrame ubx; - memset(&ubx, 0, sizeof(ubx)); +// misc_printf("%02u/%02u/%04u ", p.pvt.month, p.pvt.day, p.pvt.year); +// misc_printf("%02u:%02u:%02u%+011.9lf | ", p.pvt.hour, p.pvt.min, p.pvt.sec, (double)p.pvt.nano*1e-9); - for (int i = 0; 256; i++) { - uint8_t ch; - HAL_StatusTypeDef hal_status = HAL_UART_Receive(huart_, &ch, 1, 1000); + misc_printf("%02u/%02u/%04u ", gmt->tm_mon+1, gmt->tm_mday, gmt->tm_year+1900); + misc_printf("%02u:%02u:%02u.%09u | ", gmt->tm_hour, gmt->tm_min, gmt->tm_sec,p.unix_nanos); - if (hal_status == HAL_OK) - if (parseByte(ch, &ubx)) { - if ((ubx.cl == 0x06) && (ubx.id == 0x8B)) { - uint32_t key = (uint32_t) ubx.payload[7] << 24 | (uint32_t) ubx.payload[6] << 16 - | (uint32_t) ubx.payload[5] << 8 | (uint32_t) ubx.payload[4]; - if (key == 0x40520001) - return (uint32_t) ubx.payload[11] << 24 | (uint32_t) ubx.payload[10] << 16 | (uint32_t) ubx.payload[9] << 8 - | (uint32_t) ubx.payload[8]; - ; // baud key - } else { - memset(&ubx, 0, sizeof(ubx)); - } - } + misc_printf("%14.8f deg %14.8f deg | ", (double) p.pvt.lat * 1e-7, (double) p.pvt.lon * 1e-7); + misc_printf("numSV %02u | ", p.pvt.numSV); + misc_printf("Fix %02u | ", p.pvt.fixType); + misc_printf("valid 0x%02X | ", p.pvt.valid); + misc_printf("flags 0x%02X | ", p.pvt.flags); + misc_printf("dt %9.0lf us\n", lag); + } else { + misc_printf("%s\n", name_pvt); } - return 0; + + return 1; } + diff --git a/boards/varmint_h7/common/drivers/Ubx.h b/boards/varmint_h7/common/drivers/Ubx.h index 0971d4ae..e29320e4 100644 --- a/boards/varmint_h7/common/drivers/Ubx.h +++ b/boards/varmint_h7/common/drivers/Ubx.h @@ -38,15 +38,10 @@ #ifndef UBX_H_ #define UBX_H_ +#include "DoubleBuffer.h" #include "BoardConfig.h" -#include "Driver.h" #include "Packets.h" - -typedef enum -{ - UBX_M8, - UBX_M9 -} UbxProtocol; +#include "Status.h" // Class 0x01, ID 0x07 typedef struct __attribute__((__packed__)) // This matches the Ubx packet, do not modify @@ -93,39 +88,8 @@ typedef struct __attribute__((__packed__)) // This matches the Ubx packet, do no uint16_t magAcc; // degx 10^-2 } UbxPvt; -// Class 0x01, ID 0x20 -typedef struct __attribute__((__packed__)) // This matches the Ubx packet, do not modify -{ - uint32_t iTOW; // ms, GPS time of week - int32_t fTOW; // ns, (iTOW * 1e-3) + (fTOW * 1e-9) seconds - int16_t week; // GPS week number - int8_t leapS; - uint8_t valid; - uint32_t tAcc; -} UbxTime; - -// Class 0x01, ID 0x01 -typedef struct __attribute__((__packed__)) // This matches the Ubx packet, do not modify -{ - uint32_t iTOW; // ms, GPS time of week - int32_t ecefX; - int32_t ecefY; - int32_t ecefZ; - uint32_t pAcc; -} UbxEcefPos; - -// Class 0x01, ID 0x11 -typedef struct __attribute__((__packed__)) // This matches the Ubx packet, do not modify -{ - uint32_t iTOW; // ms, GPS time of week - int32_t ecefVX; - int32_t ecefVY; - int32_t ecefVZ; - uint32_t sAcc; -} UbxEcefVel; - #define UBX_MAX_PAYLOAD_BYTES (256) -typedef struct __attribute__((__packed__)) +typedef struct //__attribute__((__packed__)) { uint8_t cl, id; uint16_t length; @@ -133,17 +97,13 @@ typedef struct __attribute__((__packed__)) uint8_t payload[UBX_MAX_PAYLOAD_BYTES]; } UbxFrame; -typedef struct __attribute__((__packed__)) // This matches the Ubx packet, do not modify +typedef struct //__attribute__((__packed__)) // This matches the Ubx packet, do not modify { - rosflight_firmware::PacketHeader header; + rosflight_firmware::PacketHeader header; // time of validity, status int64_t unix_seconds; // computed from pvt time values - uint64_t drdy; - uint64_t groupDelay; // us, time from measurement to drdy, (approximate!) + int32_t unix_nanos; uint64_t pps; UbxPvt pvt; - UbxTime time; - UbxEcefPos ecefp; - UbxEcefVel ecefv; } UbxPacket; /** @@ -152,7 +112,7 @@ typedef struct __attribute__((__packed__)) // This matches the Ubx packet, do no * */ -class Ubx : public Driver +class Ubx : public Status { /** * \brief @@ -162,32 +122,38 @@ class Ubx : public Driver public: uint32_t init( // Driver initializers - uint16_t sample_rate_hz, GPIO_TypeDef * drdy_port, uint16_t drdy_pin, bool has_pps, + uint16_t sample_rate_hz, GPIO_TypeDef * pps_port, uint16_t pps_pin, // UART initializers - UART_HandleTypeDef * huart, USART_TypeDef * huart_instance, DMA_HandleTypeDef * hdma_uart_rx, uint32_t baud_desired, - UbxProtocol ubx_protocol); + UART_HandleTypeDef * huart, USART_TypeDef * huart_instance, DMA_HandleTypeDef * hdma_uart_rx, uint32_t baud_desired); bool poll(void); void endDma(void); bool startDma(void); - bool display(void) override; + bool display(void); bool parseByte(uint8_t c, UbxFrame * p); UART_HandleTypeDef * huart(void) { return huart_; } - bool isMy(uint16_t exti_pin) { return drdyPin_ == exti_pin; } + bool isMy(uint16_t exti_pin) { return ppsPin_ == exti_pin; } bool isMy(UART_HandleTypeDef * huart) { return huart_ == huart; } void pps(uint64_t pps_timestamp); + bool read(uint8_t * data, uint16_t size) { return double_buffer_.read(data, size)==DoubleBufferStatus::OK; } + private: + bool write(uint8_t * data, uint16_t size) { return double_buffer_.write(data, size)==DoubleBufferStatus::OK; } + DoubleBuffer double_buffer_; + uint16_t sampleRateHz_; UbxPacket ubx_; - uint64_t gotPvt_, gotTime_, gotEcefP_, gotEcefV_; //, gotNav_; + uint16_t ppsPin_; + uint64_t timeout_; + + uint64_t gotPvt_; uint64_t dtimeout_; UART_HandleTypeDef * huart_; DMA_HandleTypeDef * hdmaUartRx_; uint32_t baud_, baud_initial_; - UbxProtocol ubxProtocol_; - bool hasPps_; + uint16_t ppsHz_; void checksum(uint8_t * buffer); void header(uint8_t * buffer, uint8_t cl, uint8_t id, uint16_t length); diff --git a/boards/varmint_h7/common/drivers/Vcp.cpp b/boards/varmint_h7/common/drivers/Vcp.cpp index b0d45eae..6eee094e 100644 --- a/boards/varmint_h7/common/drivers/Vcp.cpp +++ b/boards/varmint_h7/common/drivers/Vcp.cpp @@ -47,12 +47,14 @@ extern Time64 time64; -//#define VCP_TX_FIFO_BUFFERS SERIAL_TX_FIFO_BUFFERS #define VCP_TX_FIFO_BUFFERS 256 DTCM_RAM uint8_t vcp_fifo_tx_buffer[VCP_TX_FIFO_BUFFERS * sizeof(SerialTxPacket)]; -#define VCP_RX_FIFO_BUFFER_BYTES 4096 -DTCM_RAM uint8_t vcp_fifo_rx_buffer[VCP_RX_FIFO_BUFFER_BYTES]; +//#define VCP_RX_FIFO_BUFFER_BYTES 4096 +//DTCM_RAM uint8_t vcp_fifo_rx_buffer[VCP_RX_FIFO_BUFFER_BYTES]; + +#define VCP_RX_FIFO_BUFFER_BYTES (8*1024) +static uint8_t vcp_fifo_rx_buffer[VCP_RX_FIFO_BUFFER_BYTES]; uint32_t Vcp::init(uint16_t sample_rate_hz) { diff --git a/boards/varmint_h7/common/drivers/misc.cpp b/boards/varmint_h7/common/drivers/misc.cpp index 2abb8b35..c464f2e3 100644 --- a/boards/varmint_h7/common/drivers/misc.cpp +++ b/boards/varmint_h7/common/drivers/misc.cpp @@ -265,10 +265,12 @@ size_t misc_getline(uint8_t * line, size_t len) return len; } -void misc_header(char * name, uint64_t drdy, uint64_t timestamp, uint64_t delay) +void misc_header(char * name, rosflight_firmware::PacketHeader &header) { - misc_printf("%-16s [%8.2f s %8.2f ms %8.3f ms] ", name, (double) drdy / 1e6, (double) (timestamp - drdy) / 1000., - (double) delay / 1000.); + int64_t dt=0; + if (header.timestamp>header.complete) dt = -(header.timestamp-header.complete); + else dt = header.complete-header.timestamp; + misc_printf("%-16s [t:%12.6f s dt:%10d us] ", name, (double) header.timestamp / 1e6, dt); } uint16_t misc_bytes_in_dma(DMA_HandleTypeDef * hdma_uart_rx, uint16_t dma_buffer_size) @@ -296,3 +298,4 @@ void misc_exit_status(uint32_t status) if (status & VOLTAGE_SET_FAIL) misc_printf(" VOLTAGE_SET_FAIL"); misc_printf("\033[0m\n"); } + diff --git a/boards/varmint_h7/common/drivers/misc.h b/boards/varmint_h7/common/drivers/misc.h index 0e56dcca..d6fb5dfd 100644 --- a/boards/varmint_h7/common/drivers/misc.h +++ b/boards/varmint_h7/common/drivers/misc.h @@ -38,9 +38,9 @@ #ifndef MISC_H_ #define MISC_H_ -#include #include #include +#include "rosflight_structs.h" #ifdef __cplusplus extern "C" { @@ -81,12 +81,29 @@ void misc_u32(uint32_t lo, uint32_t hi, uint32_t x, const char * pre, const char void misc_printf(const char * format, ...); size_t misc_getline(uint8_t * line, size_t len); -void misc_header(char * name, uint64_t drdy, uint64_t timestamp, uint64_t delay); +void misc_header(char * name, rosflight_firmware::PacketHeader &header); uint16_t misc_bytes_in_dma(DMA_HandleTypeDef * hdma_uart_rx, uint16_t dma_buffer_size); void misc_exit_status(uint32_t status); + #ifdef __cplusplus } #endif +class MiscRotatable +{ +public: + void rotate(double *x) { + double y[3]; + y[0] = x[0]*rotation_[0] + x[1]*rotation_[1] + x[2]*rotation_[2]; + y[1] = x[0]*rotation_[3] + x[1]*rotation_[4] + x[2]*rotation_[5]; + y[2] = x[0]*rotation_[6] + x[1]*rotation_[7] + x[2]*rotation_[8]; + x[0] = y[0]; + x[1] = y[1]; + x[2] = y[2]; + } +protected: + double rotation_[9]; +}; + #endif /* MISC_H_ */ diff --git a/boards/varmint_h7/pixracer_pro/.cproject b/boards/varmint_h7/pixracer_pro/.cproject index 4e7ce38d..82ee7852 100644 --- a/boards/varmint_h7/pixracer_pro/.cproject +++ b/boards/varmint_h7/pixracer_pro/.cproject @@ -106,10 +106,11 @@ + - + @@ -260,4 +261,4 @@ - + \ No newline at end of file diff --git a/boards/varmint_h7/pixracer_pro/.settings/stm32cubeide.project.prefs b/boards/varmint_h7/pixracer_pro/.settings/stm32cubeide.project.prefs index a79366d4..15d53424 100644 --- a/boards/varmint_h7/pixracer_pro/.settings/stm32cubeide.project.prefs +++ b/boards/varmint_h7/pixracer_pro/.settings/stm32cubeide.project.prefs @@ -1,5 +1,5 @@ 635E684B79701B039C64EA45C3F84D30=B11C66302F2E416818C1000D71988D3E 66BE74F758C12D739921AEA421D593D3=0 -DC22A860405A8BF2F2C095E5B6529F12=DA7C13B0DA5FE8332A3BF9C561394D56 +DC22A860405A8BF2F2C095E5B6529F12=BA3347A6BE47BDAA8BF9358F89019ED0 ED565EBB1256083B085CE599FB387C0B=D46F9D1F9869D76E9AC6B7CBD05E6A65 eclipse.preferences.version=1 diff --git a/boards/varmint_h7/pixracer_pro/Core/Inc/main.h b/boards/varmint_h7/pixracer_pro/Core/Inc/main.h index 57f5ca60..07110d95 100644 --- a/boards/varmint_h7/pixracer_pro/Core/Inc/main.h +++ b/boards/varmint_h7/pixracer_pro/Core/Inc/main.h @@ -65,7 +65,6 @@ void MX_SDMMC1_SD_Init(void); void MX_SPI1_Init(void); void MX_SPI2_Init(void); void MX_SPI5_Init(void); -void MX_SPI6_Init(void); void MX_TIM1_Init(void); void MX_TIM2_Init(void); void MX_TIM4_Init(void); @@ -104,10 +103,10 @@ void MX_TIM3_Init(void); #define I2C1_SCL_GPIO_Port GPIOB #define VDD_BRICK_VALID_Pin GPIO_PIN_5 #define VDD_BRICK_VALID_GPIO_Port GPIOB -#define EXTERNAL_SPI6_MOSI_Pin GPIO_PIN_14 -#define EXTERNAL_SPI6_MOSI_GPIO_Port GPIOG -#define EXTERNAL_SPI6_SCK_Pin GPIO_PIN_13 -#define EXTERNAL_SPI6_SCK_GPIO_Port GPIOG +#define PROBE3_Pin GPIO_PIN_14 +#define PROBE3_GPIO_Port GPIOG +#define PROBE1_Pin GPIO_PIN_13 +#define PROBE1_GPIO_Port GPIOG #define LED_BLUE_Pin GPIO_PIN_3 #define LED_BLUE_GPIO_Port GPIOB #define DPS310_CSn_Pin GPIO_PIN_7 @@ -128,8 +127,9 @@ void MX_TIM3_Init(void); #define FMU_UART1_RX_GPIO_Port GPIOB #define FMU_UART1_TX_Pin GPIO_PIN_6 #define FMU_UART1_TX_GPIO_Port GPIOB -#define EXTERNAL_SPI6_MISO_Pin GPIO_PIN_12 -#define EXTERNAL_SPI6_MISO_GPIO_Port GPIOG +#define GPS_PPS_Pin GPIO_PIN_12 +#define GPS_PPS_GPIO_Port GPIOG +#define GPS_PPS_EXTI_IRQn EXTI15_10_IRQn #define TELEM1_USART2_RX_Pin GPIO_PIN_6 #define TELEM1_USART2_RX_GPIO_Port GPIOD #define CAN1_RX_Pin GPIO_PIN_0 @@ -144,8 +144,8 @@ void MX_TIM3_Init(void); #define PWM_08_GPIO_Port GPIOI #define PWM_07_Pin GPIO_PIN_5 #define PWM_07_GPIO_Port GPIOI -#define EXTERNAL_SPI6_CS_Pin GPIO_PIN_9 -#define EXTERNAL_SPI6_CS_GPIO_Port GPIOG +#define PROBE2_Pin GPIO_PIN_9 +#define PROBE2_GPIO_Port GPIOG #define TELEM1_USART2_TX_Pin GPIO_PIN_5 #define TELEM1_USART2_TX_GPIO_Port GPIOD #define CAN1_TX_Pin GPIO_PIN_1 @@ -259,9 +259,6 @@ void MX_TIM3_Init(void); #define CAN2_EN_GPIO_Port GPIOF #define DEBUG_UART7_RX_Pin GPIO_PIN_7 #define DEBUG_UART7_RX_GPIO_Port GPIOE -#define ICM_20948_DRDY_Pin GPIO_PIN_12 -#define ICM_20948_DRDY_GPIO_Port GPIOE -#define ICM_20948_DRDY_EXTI_IRQn EXTI15_10_IRQn #define ICM_20948_CS_Pin GPIO_PIN_15 #define ICM_20948_CS_GPIO_Port GPIOE #define DPS310_FRAM_SCK_Pin GPIO_PIN_10 diff --git a/boards/varmint_h7/pixracer_pro/Core/Inc/stm32h7xx_it.h b/boards/varmint_h7/pixracer_pro/Core/Inc/stm32h7xx_it.h index a2b3821b..2d34be78 100644 --- a/boards/varmint_h7/pixracer_pro/Core/Inc/stm32h7xx_it.h +++ b/boards/varmint_h7/pixracer_pro/Core/Inc/stm32h7xx_it.h @@ -88,12 +88,9 @@ void USART6_IRQHandler(void); void UART7_IRQHandler(void); void UART8_IRQHandler(void); void SPI5_IRQHandler(void); -void SPI6_IRQHandler(void); void OTG_FS_IRQHandler(void); void ADC3_IRQHandler(void); void BDMA_Channel0_IRQHandler(void); -void BDMA_Channel1_IRQHandler(void); -void BDMA_Channel2_IRQHandler(void); /* USER CODE BEGIN EFP */ /* USER CODE END EFP */ diff --git a/boards/varmint_h7/pixracer_pro/Core/Src/main.c b/boards/varmint_h7/pixracer_pro/Core/Src/main.c index a55c9600..03d9988d 100644 --- a/boards/varmint_h7/pixracer_pro/Core/Src/main.c +++ b/boards/varmint_h7/pixracer_pro/Core/Src/main.c @@ -63,15 +63,12 @@ SD_HandleTypeDef hsd1; SPI_HandleTypeDef hspi1; SPI_HandleTypeDef hspi2; SPI_HandleTypeDef hspi5; -SPI_HandleTypeDef hspi6; DMA_HandleTypeDef hdma_spi1_rx; DMA_HandleTypeDef hdma_spi1_tx; DMA_HandleTypeDef hdma_spi2_rx; DMA_HandleTypeDef hdma_spi2_tx; DMA_HandleTypeDef hdma_spi5_rx; DMA_HandleTypeDef hdma_spi5_tx; -DMA_HandleTypeDef hdma_spi6_rx; -DMA_HandleTypeDef hdma_spi6_tx; TIM_HandleTypeDef htim1; TIM_HandleTypeDef htim2; @@ -166,7 +163,6 @@ int main(void) MX_SPI1_Init(); MX_SPI2_Init(); MX_SPI5_Init(); - MX_SPI6_Init(); MX_TIM1_Init(); MX_TIM2_Init(); MX_TIM4_Init(); @@ -277,10 +273,9 @@ void PeriphCommonClock_Config(void) /** Initializes the peripherals clock */ - PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USB|RCC_PERIPHCLK_SPI6 - |RCC_PERIPHCLK_ADC|RCC_PERIPHCLK_SPI2 - |RCC_PERIPHCLK_SPI1|RCC_PERIPHCLK_SPI5 - |RCC_PERIPHCLK_FDCAN; + PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USB|RCC_PERIPHCLK_ADC + |RCC_PERIPHCLK_SPI2|RCC_PERIPHCLK_SPI1 + |RCC_PERIPHCLK_SPI5|RCC_PERIPHCLK_FDCAN; PeriphClkInitStruct.PLL2.PLL2M = 12; PeriphClkInitStruct.PLL2.PLL2N = 240; PeriphClkInitStruct.PLL2.PLL2P = 30; @@ -302,7 +297,6 @@ void PeriphCommonClock_Config(void) PeriphClkInitStruct.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL2; PeriphClkInitStruct.UsbClockSelection = RCC_USBCLKSOURCE_PLL3; PeriphClkInitStruct.AdcClockSelection = RCC_ADCCLKSOURCE_PLL3; - PeriphClkInitStruct.Spi6ClockSelection = RCC_SPI6CLKSOURCE_PLL2; if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) { Error_Handler(); @@ -913,54 +907,6 @@ void MX_SPI5_Init(void) } -/** - * @brief SPI6 Initialization Function - * @param None - * @retval None - */ -void MX_SPI6_Init(void) -{ - - /* USER CODE BEGIN SPI6_Init 0 */ - - /* USER CODE END SPI6_Init 0 */ - - /* USER CODE BEGIN SPI6_Init 1 */ - - /* USER CODE END SPI6_Init 1 */ - /* SPI6 parameter configuration*/ - hspi6.Instance = SPI6; - hspi6.Init.Mode = SPI_MODE_MASTER; - hspi6.Init.Direction = SPI_DIRECTION_2LINES; - hspi6.Init.DataSize = SPI_DATASIZE_8BIT; - hspi6.Init.CLKPolarity = SPI_POLARITY_HIGH; - hspi6.Init.CLKPhase = SPI_PHASE_2EDGE; - hspi6.Init.NSS = SPI_NSS_SOFT; - hspi6.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8; - hspi6.Init.FirstBit = SPI_FIRSTBIT_MSB; - hspi6.Init.TIMode = SPI_TIMODE_DISABLE; - hspi6.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; - hspi6.Init.CRCPolynomial = 0x0; - hspi6.Init.NSSPMode = SPI_NSS_PULSE_ENABLE; - hspi6.Init.NSSPolarity = SPI_NSS_POLARITY_LOW; - hspi6.Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA; - hspi6.Init.TxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN; - hspi6.Init.RxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN; - hspi6.Init.MasterSSIdleness = SPI_MASTER_SS_IDLENESS_00CYCLE; - hspi6.Init.MasterInterDataIdleness = SPI_MASTER_INTERDATA_IDLENESS_00CYCLE; - hspi6.Init.MasterReceiverAutoSusp = SPI_MASTER_RX_AUTOSUSP_DISABLE; - hspi6.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_ENABLE; - hspi6.Init.IOSwap = SPI_IO_SWAP_DISABLE; - if (HAL_SPI_Init(&hspi6) != HAL_OK) - { - Error_Handler(); - } - /* USER CODE BEGIN SPI6_Init 2 */ - - /* USER CODE END SPI6_Init 2 */ - -} - /** * @brief TIM1 Initialization Function * @param None @@ -1784,12 +1730,6 @@ void MX_BDMA_Init(void) /* BDMA_Channel0_IRQn interrupt configuration */ HAL_NVIC_SetPriority(BDMA_Channel0_IRQn, 5, 0); HAL_NVIC_EnableIRQ(BDMA_Channel0_IRQn); - /* BDMA_Channel1_IRQn interrupt configuration */ - HAL_NVIC_SetPriority(BDMA_Channel1_IRQn, 5, 0); - HAL_NVIC_EnableIRQ(BDMA_Channel1_IRQn); - /* BDMA_Channel2_IRQn interrupt configuration */ - HAL_NVIC_SetPriority(BDMA_Channel2_IRQn, 5, 0); - HAL_NVIC_EnableIRQ(BDMA_Channel2_IRQn); } @@ -1878,13 +1818,13 @@ void MX_GPIO_Init(void) HAL_GPIO_WritePin(GPIOE, VDD_3V3_SENSORS_EN_Pin|SPEKTRUM_POWER_EN_Pin|ICM_20948_CS_Pin, GPIO_PIN_SET); /*Configure GPIO pin Output Level */ - HAL_GPIO_WritePin(GPIOB, LED_BLUE_Pin|LED_GREEN_Pin|LED_RED_Pin, GPIO_PIN_RESET); + HAL_GPIO_WritePin(GPIOG, PROBE3_Pin|PROBE1_Pin|PROBE2_Pin, GPIO_PIN_RESET); /*Configure GPIO pin Output Level */ - HAL_GPIO_WritePin(GPIOD, DPS310_CSn_Pin|FRAM_CS_Pin, GPIO_PIN_SET); + HAL_GPIO_WritePin(GPIOB, LED_BLUE_Pin|LED_GREEN_Pin|LED_RED_Pin, GPIO_PIN_RESET); /*Configure GPIO pin Output Level */ - HAL_GPIO_WritePin(GPIOG, EXTERNAL_SPI6_CS_Pin|PWM_VOLT_SEL_Pin, GPIO_PIN_SET); + HAL_GPIO_WritePin(GPIOD, DPS310_CSn_Pin|FRAM_CS_Pin, GPIO_PIN_SET); /*Configure GPIO pin Output Level */ HAL_GPIO_WritePin(VDD_1V8_SENSORS_EN_GPIO_Port, VDD_1V8_SENSORS_EN_Pin, GPIO_PIN_SET); @@ -1892,6 +1832,9 @@ void MX_GPIO_Init(void) /*Configure GPIO pin Output Level */ HAL_GPIO_WritePin(GPIOF, BMI088_INT2_ACCEL_Pin|CAN1_EN_Pin|CAN2_EN_Pin, GPIO_PIN_RESET); + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(PWM_VOLT_SEL_GPIO_Port, PWM_VOLT_SEL_Pin, GPIO_PIN_SET); + /*Configure GPIO pin Output Level */ HAL_GPIO_WritePin(GPIOF, BMI088_ACCEL_CS_Pin|BMI088_GYRO_CS_Pin, GPIO_PIN_SET); @@ -1911,6 +1854,13 @@ void MX_GPIO_Init(void) GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(VDD_BRICK_VALID_GPIO_Port, &GPIO_InitStruct); + /*Configure GPIO pins : PROBE3_Pin PROBE1_Pin PROBE2_Pin PWM_VOLT_SEL_Pin */ + GPIO_InitStruct.Pin = PROBE3_Pin|PROBE1_Pin|PROBE2_Pin|PWM_VOLT_SEL_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + HAL_GPIO_Init(GPIOG, &GPIO_InitStruct); + /*Configure GPIO pins : LED_BLUE_Pin LED_GREEN_Pin LED_RED_Pin */ GPIO_InitStruct.Pin = LED_BLUE_Pin|LED_GREEN_Pin|LED_RED_Pin; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; @@ -1925,12 +1875,11 @@ void MX_GPIO_Init(void) GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(GPIOD, &GPIO_InitStruct); - /*Configure GPIO pin : EXTERNAL_SPI6_CS_Pin */ - GPIO_InitStruct.Pin = EXTERNAL_SPI6_CS_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; - GPIO_InitStruct.Pull = GPIO_PULLUP; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - HAL_GPIO_Init(EXTERNAL_SPI6_CS_GPIO_Port, &GPIO_InitStruct); + /*Configure GPIO pin : GPS_PPS_Pin */ + GPIO_InitStruct.Pin = GPS_PPS_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; + GPIO_InitStruct.Pull = GPIO_PULLDOWN; + HAL_GPIO_Init(GPS_PPS_GPIO_Port, &GPIO_InitStruct); /*Configure GPIO pin : VDD_1V8_SENSORS_EN_Pin */ GPIO_InitStruct.Pin = VDD_1V8_SENSORS_EN_Pin; @@ -1958,13 +1907,6 @@ void MX_GPIO_Init(void) GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOF, &GPIO_InitStruct); - /*Configure GPIO pin : PWM_VOLT_SEL_Pin */ - GPIO_InitStruct.Pin = PWM_VOLT_SEL_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - HAL_GPIO_Init(PWM_VOLT_SEL_GPIO_Port, &GPIO_InitStruct); - /*Configure GPIO pins : BMI088_ACCEL_CS_Pin BMI088_GYRO_CS_Pin */ GPIO_InitStruct.Pin = BMI088_ACCEL_CS_Pin|BMI088_GYRO_CS_Pin; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; @@ -1991,12 +1933,6 @@ void MX_GPIO_Init(void) GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(ICM_20602_CS_GPIO_Port, &GPIO_InitStruct); - /*Configure GPIO pin : ICM_20948_DRDY_Pin */ - GPIO_InitStruct.Pin = ICM_20948_DRDY_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; - GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(ICM_20948_DRDY_GPIO_Port, &GPIO_InitStruct); - /* EXTI interrupt init*/ HAL_NVIC_SetPriority(EXTI1_IRQn, 5, 0); HAL_NVIC_EnableIRQ(EXTI1_IRQn); diff --git a/boards/varmint_h7/pixracer_pro/Core/Src/stm32h7xx_hal_msp.c b/boards/varmint_h7/pixracer_pro/Core/Src/stm32h7xx_hal_msp.c index 97bb03df..11ac3d30 100644 --- a/boards/varmint_h7/pixracer_pro/Core/Src/stm32h7xx_hal_msp.c +++ b/boards/varmint_h7/pixracer_pro/Core/Src/stm32h7xx_hal_msp.c @@ -44,10 +44,6 @@ extern DMA_HandleTypeDef hdma_spi5_rx; extern DMA_HandleTypeDef hdma_spi5_tx; -extern DMA_HandleTypeDef hdma_spi6_rx; - -extern DMA_HandleTypeDef hdma_spi6_tx; - extern DMA_HandleTypeDef hdma_tim1_up; extern DMA_HandleTypeDef hdma_tim4_up; @@ -993,69 +989,6 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi) /* USER CODE END SPI5_MspInit 1 */ } - else if(hspi->Instance==SPI6) - { - /* USER CODE BEGIN SPI6_MspInit 0 */ - - /* USER CODE END SPI6_MspInit 0 */ - /* Peripheral clock enable */ - __HAL_RCC_SPI6_CLK_ENABLE(); - - __HAL_RCC_GPIOG_CLK_ENABLE(); - /**SPI6 GPIO Configuration - PG14 ------> SPI6_MOSI - PG13 ------> SPI6_SCK - PG12 ------> SPI6_MISO - */ - GPIO_InitStruct.Pin = EXTERNAL_SPI6_MOSI_Pin|EXTERNAL_SPI6_SCK_Pin|EXTERNAL_SPI6_MISO_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.Alternate = GPIO_AF5_SPI6; - HAL_GPIO_Init(GPIOG, &GPIO_InitStruct); - - /* SPI6 DMA Init */ - /* SPI6_RX Init */ - hdma_spi6_rx.Instance = BDMA_Channel1; - hdma_spi6_rx.Init.Request = BDMA_REQUEST_SPI6_RX; - hdma_spi6_rx.Init.Direction = DMA_PERIPH_TO_MEMORY; - hdma_spi6_rx.Init.PeriphInc = DMA_PINC_DISABLE; - hdma_spi6_rx.Init.MemInc = DMA_MINC_ENABLE; - hdma_spi6_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; - hdma_spi6_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; - hdma_spi6_rx.Init.Mode = DMA_NORMAL; - hdma_spi6_rx.Init.Priority = DMA_PRIORITY_LOW; - if (HAL_DMA_Init(&hdma_spi6_rx) != HAL_OK) - { - Error_Handler(); - } - - __HAL_LINKDMA(hspi,hdmarx,hdma_spi6_rx); - - /* SPI6_TX Init */ - hdma_spi6_tx.Instance = BDMA_Channel2; - hdma_spi6_tx.Init.Request = BDMA_REQUEST_SPI6_TX; - hdma_spi6_tx.Init.Direction = DMA_MEMORY_TO_PERIPH; - hdma_spi6_tx.Init.PeriphInc = DMA_PINC_DISABLE; - hdma_spi6_tx.Init.MemInc = DMA_MINC_ENABLE; - hdma_spi6_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; - hdma_spi6_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; - hdma_spi6_tx.Init.Mode = DMA_NORMAL; - hdma_spi6_tx.Init.Priority = DMA_PRIORITY_LOW; - if (HAL_DMA_Init(&hdma_spi6_tx) != HAL_OK) - { - Error_Handler(); - } - - __HAL_LINKDMA(hspi,hdmatx,hdma_spi6_tx); - - /* SPI6 interrupt Init */ - HAL_NVIC_SetPriority(SPI6_IRQn, 5, 0); - HAL_NVIC_EnableIRQ(SPI6_IRQn); - /* USER CODE BEGIN SPI6_MspInit 1 */ - - /* USER CODE END SPI6_MspInit 1 */ - } } @@ -1142,31 +1075,6 @@ void HAL_SPI_MspDeInit(SPI_HandleTypeDef* hspi) /* USER CODE END SPI5_MspDeInit 1 */ } - else if(hspi->Instance==SPI6) - { - /* USER CODE BEGIN SPI6_MspDeInit 0 */ - - /* USER CODE END SPI6_MspDeInit 0 */ - /* Peripheral clock disable */ - __HAL_RCC_SPI6_CLK_DISABLE(); - - /**SPI6 GPIO Configuration - PG14 ------> SPI6_MOSI - PG13 ------> SPI6_SCK - PG12 ------> SPI6_MISO - */ - HAL_GPIO_DeInit(GPIOG, EXTERNAL_SPI6_MOSI_Pin|EXTERNAL_SPI6_SCK_Pin|EXTERNAL_SPI6_MISO_Pin); - - /* SPI6 DMA DeInit */ - HAL_DMA_DeInit(hspi->hdmarx); - HAL_DMA_DeInit(hspi->hdmatx); - - /* SPI6 interrupt DeInit */ - HAL_NVIC_DisableIRQ(SPI6_IRQn); - /* USER CODE BEGIN SPI6_MspDeInit 1 */ - - /* USER CODE END SPI6_MspDeInit 1 */ - } } diff --git a/boards/varmint_h7/pixracer_pro/Core/Src/stm32h7xx_it.c b/boards/varmint_h7/pixracer_pro/Core/Src/stm32h7xx_it.c index 93e51dad..35409c10 100644 --- a/boards/varmint_h7/pixracer_pro/Core/Src/stm32h7xx_it.c +++ b/boards/varmint_h7/pixracer_pro/Core/Src/stm32h7xx_it.c @@ -71,12 +71,9 @@ extern DMA_HandleTypeDef hdma_spi2_rx; extern DMA_HandleTypeDef hdma_spi2_tx; extern DMA_HandleTypeDef hdma_spi5_rx; extern DMA_HandleTypeDef hdma_spi5_tx; -extern DMA_HandleTypeDef hdma_spi6_rx; -extern DMA_HandleTypeDef hdma_spi6_tx; extern SPI_HandleTypeDef hspi1; extern SPI_HandleTypeDef hspi2; extern SPI_HandleTypeDef hspi5; -extern SPI_HandleTypeDef hspi6; extern DMA_HandleTypeDef hdma_tim1_up; extern DMA_HandleTypeDef hdma_tim4_up; extern DMA_HandleTypeDef hdma_tim8_up; @@ -479,7 +476,7 @@ void EXTI15_10_IRQHandler(void) /* USER CODE BEGIN EXTI15_10_IRQn 0 */ /* USER CODE END EXTI15_10_IRQn 0 */ - HAL_GPIO_EXTI_IRQHandler(ICM_20948_DRDY_Pin); + HAL_GPIO_EXTI_IRQHandler(GPS_PPS_Pin); /* USER CODE BEGIN EXTI15_10_IRQn 1 */ /* USER CODE END EXTI15_10_IRQn 1 */ @@ -695,20 +692,6 @@ void SPI5_IRQHandler(void) /* USER CODE END SPI5_IRQn 1 */ } -/** - * @brief This function handles SPI6 global interrupt. - */ -void SPI6_IRQHandler(void) -{ - /* USER CODE BEGIN SPI6_IRQn 0 */ - - /* USER CODE END SPI6_IRQn 0 */ - HAL_SPI_IRQHandler(&hspi6); - /* USER CODE BEGIN SPI6_IRQn 1 */ - - /* USER CODE END SPI6_IRQn 1 */ -} - /** * @brief This function handles USB On The Go FS global interrupt. */ @@ -751,34 +734,6 @@ void BDMA_Channel0_IRQHandler(void) /* USER CODE END BDMA_Channel0_IRQn 1 */ } -/** - * @brief This function handles BDMA channel1 global interrupt. - */ -void BDMA_Channel1_IRQHandler(void) -{ - /* USER CODE BEGIN BDMA_Channel1_IRQn 0 */ - - /* USER CODE END BDMA_Channel1_IRQn 0 */ - HAL_DMA_IRQHandler(&hdma_spi6_rx); - /* USER CODE BEGIN BDMA_Channel1_IRQn 1 */ - - /* USER CODE END BDMA_Channel1_IRQn 1 */ -} - -/** - * @brief This function handles BDMA channel2 global interrupt. - */ -void BDMA_Channel2_IRQHandler(void) -{ - /* USER CODE BEGIN BDMA_Channel2_IRQn 0 */ - - /* USER CODE END BDMA_Channel2_IRQn 0 */ - HAL_DMA_IRQHandler(&hdma_spi6_tx); - /* USER CODE BEGIN BDMA_Channel2_IRQn 1 */ - - /* USER CODE END BDMA_Channel2_IRQn 1 */ -} - /* USER CODE BEGIN 1 */ /* USER CODE END 1 */ diff --git a/boards/varmint_h7/pixracer_pro/pixracer_pro Debug.launch b/boards/varmint_h7/pixracer_pro/pixracer_pro Debug.launch new file mode 100644 index 00000000..8c1d54ea --- /dev/null +++ b/boards/varmint_h7/pixracer_pro/pixracer_pro Debug.launch @@ -0,0 +1,99 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/boards/varmint_h7/pixracer_pro/pixracer_pro.ioc b/boards/varmint_h7/pixracer_pro/pixracer_pro.ioc index d18b504d..76a019f5 100644 --- a/boards/varmint_h7/pixracer_pro/pixracer_pro.ioc +++ b/boards/varmint_h7/pixracer_pro/pixracer_pro.ioc @@ -67,43 +67,7 @@ Bdma.ADC3.0.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT Bdma.ADC3.0.SyncRequestNumber=1 Bdma.ADC3.0.SyncSignalID=NONE Bdma.Request0=ADC3 -Bdma.Request1=SPI6_RX -Bdma.Request2=SPI6_TX -Bdma.RequestsNb=3 -Bdma.SPI6_RX.1.Direction=DMA_PERIPH_TO_MEMORY -Bdma.SPI6_RX.1.EventEnable=DISABLE -Bdma.SPI6_RX.1.Instance=BDMA_Channel1 -Bdma.SPI6_RX.1.MemDataAlignment=DMA_MDATAALIGN_BYTE -Bdma.SPI6_RX.1.MemInc=DMA_MINC_ENABLE -Bdma.SPI6_RX.1.Mode=DMA_NORMAL -Bdma.SPI6_RX.1.PeriphDataAlignment=DMA_PDATAALIGN_BYTE -Bdma.SPI6_RX.1.PeriphInc=DMA_PINC_DISABLE -Bdma.SPI6_RX.1.Polarity=HAL_DMAMUX_REQ_GEN_RISING -Bdma.SPI6_RX.1.Priority=DMA_PRIORITY_LOW -Bdma.SPI6_RX.1.RequestNumber=1 -Bdma.SPI6_RX.1.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber -Bdma.SPI6_RX.1.SignalID=NONE -Bdma.SPI6_RX.1.SyncEnable=DISABLE -Bdma.SPI6_RX.1.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT -Bdma.SPI6_RX.1.SyncRequestNumber=1 -Bdma.SPI6_RX.1.SyncSignalID=NONE -Bdma.SPI6_TX.2.Direction=DMA_MEMORY_TO_PERIPH -Bdma.SPI6_TX.2.EventEnable=DISABLE -Bdma.SPI6_TX.2.Instance=BDMA_Channel2 -Bdma.SPI6_TX.2.MemDataAlignment=DMA_MDATAALIGN_BYTE -Bdma.SPI6_TX.2.MemInc=DMA_MINC_ENABLE -Bdma.SPI6_TX.2.Mode=DMA_NORMAL -Bdma.SPI6_TX.2.PeriphDataAlignment=DMA_PDATAALIGN_BYTE -Bdma.SPI6_TX.2.PeriphInc=DMA_PINC_DISABLE -Bdma.SPI6_TX.2.Polarity=HAL_DMAMUX_REQ_GEN_RISING -Bdma.SPI6_TX.2.Priority=DMA_PRIORITY_LOW -Bdma.SPI6_TX.2.RequestNumber=1 -Bdma.SPI6_TX.2.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber -Bdma.SPI6_TX.2.SignalID=NONE -Bdma.SPI6_TX.2.SyncEnable=DISABLE -Bdma.SPI6_TX.2.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT -Bdma.SPI6_TX.2.SyncRequestNumber=1 -Bdma.SPI6_TX.2.SyncSignalID=NONE +Bdma.RequestsNb=1 CAD.formats= CAD.pinconfig= CAD.provider= @@ -476,33 +440,32 @@ Mcu.IP14=SDMMC1 Mcu.IP15=SPI1 Mcu.IP16=SPI2 Mcu.IP17=SPI5 -Mcu.IP18=SPI6 -Mcu.IP19=SYS +Mcu.IP18=SYS +Mcu.IP19=TIM1 Mcu.IP2=BDMA -Mcu.IP20=TIM1 -Mcu.IP21=TIM2 -Mcu.IP22=TIM3 -Mcu.IP23=TIM4 -Mcu.IP24=TIM5 -Mcu.IP25=TIM7 -Mcu.IP26=TIM8 -Mcu.IP27=TIM12 -Mcu.IP28=UART4 -Mcu.IP29=UART7 +Mcu.IP20=TIM2 +Mcu.IP21=TIM3 +Mcu.IP22=TIM4 +Mcu.IP23=TIM5 +Mcu.IP24=TIM7 +Mcu.IP25=TIM8 +Mcu.IP26=TIM12 +Mcu.IP27=UART4 +Mcu.IP28=UART7 +Mcu.IP29=UART8 Mcu.IP3=CORTEX_M7 -Mcu.IP30=UART8 -Mcu.IP31=USART1 -Mcu.IP32=USART2 -Mcu.IP33=USART3 -Mcu.IP34=USART6 -Mcu.IP35=USB_OTG_FS +Mcu.IP30=USART1 +Mcu.IP31=USART2 +Mcu.IP32=USART3 +Mcu.IP33=USART6 +Mcu.IP34=USB_OTG_FS Mcu.IP4=CRC Mcu.IP5=DEBUG Mcu.IP6=DMA Mcu.IP7=FDCAN1 Mcu.IP8=FDCAN2 Mcu.IP9=I2C1 -Mcu.IPNb=36 +Mcu.IPNb=35 Mcu.Name=STM32H743IIKx Mcu.Package=UFBGA176 Mcu.Pin0=PE3 @@ -585,25 +548,24 @@ Mcu.Pin78=PF11 Mcu.Pin79=PF14 Mcu.Pin8=PD7 Mcu.Pin80=PE7 -Mcu.Pin81=PE12 -Mcu.Pin82=PE15 -Mcu.Pin83=PB10 -Mcu.Pin84=PB11 -Mcu.Pin85=PB14 -Mcu.Pin86=PB15 -Mcu.Pin87=VP_ADC3_TempSens_Input -Mcu.Pin88=VP_ADC3_Vref_Input -Mcu.Pin89=VP_ADC3_Vbat_Input +Mcu.Pin81=PE15 +Mcu.Pin82=PB10 +Mcu.Pin83=PB11 +Mcu.Pin84=PB14 +Mcu.Pin85=PB15 +Mcu.Pin86=VP_ADC3_TempSens_Input +Mcu.Pin87=VP_ADC3_Vref_Input +Mcu.Pin88=VP_ADC3_Vbat_Input +Mcu.Pin89=VP_CRC_VS_CRC Mcu.Pin9=PC12 -Mcu.Pin90=VP_CRC_VS_CRC -Mcu.Pin91=VP_RNG_VS_RNG -Mcu.Pin92=VP_RTC_VS_RTC_Activate -Mcu.Pin93=VP_SYS_VS_Systick -Mcu.Pin94=VP_TIM5_VS_ClockSourceINT -Mcu.Pin95=VP_TIM7_VS_ClockSourceINT -Mcu.Pin96=VP_TIM12_VS_ControllerModeClock -Mcu.Pin97=VP_TIM12_VS_ClockSourceITR -Mcu.PinsNb=98 +Mcu.Pin90=VP_RNG_VS_RNG +Mcu.Pin91=VP_RTC_VS_RTC_Activate +Mcu.Pin92=VP_SYS_VS_Systick +Mcu.Pin93=VP_TIM5_VS_ClockSourceINT +Mcu.Pin94=VP_TIM7_VS_ClockSourceINT +Mcu.Pin95=VP_TIM12_VS_ControllerModeClock +Mcu.Pin96=VP_TIM12_VS_ClockSourceITR +Mcu.PinsNb=97 Mcu.ThirdPartyNb=0 Mcu.UserConstants= Mcu.UserName=STM32H743IIKx @@ -612,8 +574,6 @@ MxDb.Version=DB.6.0.100 NVIC.ADC3_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true NVIC.ADC_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true NVIC.BDMA_Channel0_IRQn=true\:5\:0\:true\:false\:true\:false\:true\:true -NVIC.BDMA_Channel1_IRQn=true\:5\:0\:true\:false\:true\:false\:true\:true -NVIC.BDMA_Channel2_IRQn=true\:5\:0\:true\:false\:true\:false\:true\:true NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.DMA1_Stream0_IRQn=true\:5\:0\:true\:false\:true\:false\:true\:true NVIC.DMA1_Stream1_IRQn=true\:5\:0\:true\:false\:true\:false\:true\:true @@ -647,7 +607,6 @@ NVIC.SDMMC1_IRQn=true\:1\:0\:true\:false\:true\:true\:true\:true NVIC.SPI1_IRQn=true\:5\:0\:true\:false\:true\:false\:true\:true NVIC.SPI2_IRQn=true\:5\:0\:true\:false\:true\:false\:true\:true NVIC.SPI5_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true -NVIC.SPI6_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.SysTick_IRQn=true\:0\:0\:true\:false\:true\:false\:true\:false NVIC.TIM7_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true @@ -993,11 +952,6 @@ PE11.GPIO_Label=PWM_03 PE11.Locked=true PE11.PinAttribute=Free PE11.Signal=S_TIM1_CH2 -PE12.GPIOParameters=GPIO_Label,PinAttribute -PE12.GPIO_Label=ICM_20948_DRDY -PE12.Locked=true -PE12.PinAttribute=Free -PE12.Signal=GPXTI12 PE13.GPIOParameters=GPIO_Label,PinAttribute PE13.GPIO_Label=PWM_02 PE13.Locked=true @@ -1105,24 +1059,19 @@ PF9.Locked=true PF9.Mode=Full_Duplex_Master PF9.PinAttribute=Free PF9.Signal=SPI5_MOSI -PG12.GPIOParameters=GPIO_Label,PinAttribute -PG12.GPIO_Label=EXTERNAL_SPI6_MISO +PG12.GPIOParameters=GPIO_PuPd,GPIO_Label +PG12.GPIO_Label=GPS_PPS +PG12.GPIO_PuPd=GPIO_PULLDOWN PG12.Locked=true -PG12.Mode=Full_Duplex_Master -PG12.PinAttribute=Free -PG12.Signal=SPI6_MISO -PG13.GPIOParameters=GPIO_Label,PinAttribute -PG13.GPIO_Label=EXTERNAL_SPI6_SCK +PG12.Signal=GPXTI12 +PG13.GPIOParameters=GPIO_Label +PG13.GPIO_Label=PROBE1 PG13.Locked=true -PG13.Mode=Full_Duplex_Master -PG13.PinAttribute=Free -PG13.Signal=SPI6_SCK -PG14.GPIOParameters=GPIO_Label,PinAttribute -PG14.GPIO_Label=EXTERNAL_SPI6_MOSI +PG13.Signal=GPIO_Output +PG14.GPIOParameters=GPIO_Label +PG14.GPIO_Label=PROBE3 PG14.Locked=true -PG14.Mode=Full_Duplex_Master -PG14.PinAttribute=Free -PG14.Signal=SPI6_MOSI +PG14.Signal=GPIO_Output PG6.GPIOParameters=PinState,GPIO_Label,PinAttribute PG6.GPIO_Label=PWM_VOLT_SEL PG6.Locked=true @@ -1130,11 +1079,11 @@ PG6.PinAttribute=Free PG6.PinState=GPIO_PIN_SET PG6.Signal=GPIO_Output PG9.GPIOParameters=PinState,GPIO_PuPd,GPIO_Label,PinAttribute -PG9.GPIO_Label=EXTERNAL_SPI6_CS -PG9.GPIO_PuPd=GPIO_PULLUP +PG9.GPIO_Label=PROBE2 +PG9.GPIO_PuPd=GPIO_NOPULL PG9.Locked=true PG9.PinAttribute=Free -PG9.PinState=GPIO_PIN_SET +PG9.PinState=GPIO_PIN_RESET PG9.Signal=GPIO_Output PH0-OSC_IN\ (PH0).GPIOParameters=GPIO_Label,PinAttribute PH0-OSC_IN\ (PH0).GPIO_Label=CRYSTAL_24MHz_IN @@ -1168,6 +1117,7 @@ PinOutPanel.CurrentBGAView=Top PinOutPanel.RotationAngle=0 ProjectManager.AskForMigrate=true ProjectManager.BackupPrevious=false +ProjectManager.CompilerLinker=GCC ProjectManager.CompilerOptimize=6 ProjectManager.ComputerToolchain=false ProjectManager.CoupleFile=false @@ -1196,7 +1146,7 @@ ProjectManager.ToolChainLocation= ProjectManager.UAScriptAfterPath= ProjectManager.UAScriptBeforePath= ProjectManager.UnderRoot=true -ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-false,3-MX_DMA_Init-DMA-false-HAL-false,4-MX_BDMA_Init-BDMA-false-HAL-false,5-MX_FDCAN2_Init-FDCAN2-false-HAL-false,6-MX_I2C1_Init-I2C1-false-HAL-false,7-MX_SDMMC1_SD_Init-SDMMC1-false-HAL-false,8-MX_SPI1_Init-SPI1-false-HAL-false,9-MX_SPI2_Init-SPI2-false-HAL-false,10-MX_SPI5_Init-SPI5-false-HAL-false,11-MX_SPI6_Init-SPI6-false-HAL-false,12-MX_TIM1_Init-TIM1-false-HAL-false,13-MX_TIM2_Init-TIM2-false-HAL-false,14-MX_TIM4_Init-TIM4-false-HAL-false,15-MX_TIM8_Init-TIM8-false-HAL-false,16-MX_UART4_Init-UART4-false-HAL-false,17-MX_UART7_Init-UART7-false-HAL-false,18-MX_UART8_Init-UART8-false-HAL-false,19-MX_USART1_UART_Init-USART1-false-HAL-false,20-MX_USART2_UART_Init-USART2-false-HAL-false,21-MX_USART3_UART_Init-USART3-false-HAL-false,22-MX_USART6_UART_Init-USART6-false-HAL-false,23-MX_RNG_Init-RNG-false-HAL-false,24-MX_RTC_Init-RTC-false-HAL-false,25-MX_TIM5_Init-TIM5-false-HAL-false,26-MX_TIM7_Init-TIM7-false-HAL-false,27-MX_FDCAN1_Init-FDCAN1-false-HAL-false,28-MX_ADC3_Init-ADC3-false-HAL-false,29-MX_ADC1_Init-ADC1-false-HAL-false,30-MX_CRC_Init-CRC-false-HAL-false,31-MX_USB_OTG_FS_PCD_Init-USB_OTG_FS-false-HAL-false,32-MX_TIM12_Init-TIM12-false-HAL-false,33-MX_TIM3_Init-TIM3-false-HAL-false,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true +ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-false,3-MX_DMA_Init-DMA-false-HAL-false,4-MX_BDMA_Init-BDMA-false-HAL-false,5-MX_FDCAN2_Init-FDCAN2-false-HAL-false,6-MX_I2C1_Init-I2C1-false-HAL-false,7-MX_SDMMC1_SD_Init-SDMMC1-false-HAL-false,8-MX_SPI1_Init-SPI1-false-HAL-false,9-MX_SPI2_Init-SPI2-false-HAL-false,10-MX_SPI5_Init-SPI5-false-HAL-false,11-MX_TIM1_Init-TIM1-false-HAL-false,12-MX_TIM2_Init-TIM2-false-HAL-false,13-MX_TIM4_Init-TIM4-false-HAL-false,14-MX_TIM8_Init-TIM8-false-HAL-false,15-MX_UART4_Init-UART4-false-HAL-false,16-MX_UART7_Init-UART7-false-HAL-false,17-MX_UART8_Init-UART8-false-HAL-false,18-MX_USART1_UART_Init-USART1-false-HAL-false,19-MX_USART2_UART_Init-USART2-false-HAL-false,20-MX_USART3_UART_Init-USART3-false-HAL-false,21-MX_USART6_UART_Init-USART6-false-HAL-false,22-MX_RNG_Init-RNG-false-HAL-false,23-MX_RTC_Init-RTC-false-HAL-false,24-MX_TIM5_Init-TIM5-false-HAL-false,25-MX_TIM7_Init-TIM7-false-HAL-false,26-MX_FDCAN1_Init-FDCAN1-false-HAL-false,27-MX_ADC3_Init-ADC3-false-HAL-false,28-MX_ADC1_Init-ADC1-false-HAL-false,29-MX_CRC_Init-CRC-false-HAL-false,30-MX_USB_OTG_FS_PCD_Init-USB_OTG_FS-false-HAL-false,31-MX_TIM12_Init-TIM12-false-HAL-false,32-MX_TIM3_Init-TIM3-false-HAL-false,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true RCC.ADCCLockSelection=RCC_ADCCLKSOURCE_PLL3 RCC.ADCFreq_Value=64000000 RCC.AHB12Freq_Value=200000000 @@ -1361,16 +1311,6 @@ SPI5.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,BaudRatePrescaler SPI5.MasterKeepIOState=SPI_MASTER_KEEP_IO_STATE_ENABLE SPI5.Mode=SPI_MODE_MASTER SPI5.VirtualType=VM_MASTER -SPI6.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_8 -SPI6.CLKPhase=SPI_PHASE_2EDGE -SPI6.CLKPolarity=SPI_POLARITY_HIGH -SPI6.CalculateBaudRate=2.0 MBits/s -SPI6.DataSize=SPI_DATASIZE_8BIT -SPI6.Direction=SPI_DIRECTION_2LINES -SPI6.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,BaudRatePrescaler,DataSize,CLKPolarity,CLKPhase,MasterKeepIOState -SPI6.MasterKeepIOState=SPI_MASTER_KEEP_IO_STATE_ENABLE -SPI6.Mode=SPI_MODE_MASTER -SPI6.VirtualType=VM_MASTER TIM1.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE TIM1.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 TIM1.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2 diff --git a/boards/varmint_h7/pixracer_pro/specific/BoardConfig.h b/boards/varmint_h7/pixracer_pro/specific/BoardConfig.h index 1ef420be..befdae16 100644 --- a/boards/varmint_h7/pixracer_pro/specific/BoardConfig.h +++ b/boards/varmint_h7/pixracer_pro/specific/BoardConfig.h @@ -41,7 +41,7 @@ #include "CommonConfig.h" #define SANDBOX false -#define BOARD_STATUS_PRINT false +#define BOARD_STATUS_PRINT (false|SANDBOX) #define USE_TELEM 0 // 1 = use UART, 0 = use VCP for link to companion computer. // UART used for printf's @@ -84,8 +84,6 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s) /**/ // clang-format on -#define AUAV_FIFO_BUFFERS 0 // not used in pixracer pro - // 48-bit us counter. // Prefer to have the 32-bit counter on the low order bytes: #define HTIM_LOW (&htim5) // 32-bit counter @@ -146,21 +144,17 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s) #define BMI088_HZ (EPOCH_HZ) // 400, 1000, 2000 are the only options #define BMI088_RANGE_A (3) // 0,1,2,3 --> 3,6,12,24g for BMI088; 2 4 8 16g for BMI 085 #define BMI088_RANGE_G (2) // 0,1,2,3,4 --> 2000,1000,500,250,125 deg/s -#define BMI088_FIFO_BUFFERS (FIFO_MIN_BUFFERS + BMI088_HZ / EPOCH_HZ) #define BMI088_ROTATION (const double[]){ -1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0} // ADIS IMU #define ADIS165XX_HZ (EPOCH_HZ) -#define ADIS165XX_FIFO_BUFFERS (FIFO_MIN_BUFFERS + ADIS165XX_HZ / EPOCH_HZ) #define ADIS165XX_ROTATION (const double[]){-1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0} // DLHR Pitot is on i2c1 #define DLHRL20G_HZ (100) -#define DLHRL20G_FIFO_BUFFERS (FIFO_MIN_BUFFERS + DLHRL20G_HZ / EPOCH_HZ) // MS4525D Pitot #define MS4525_HZ (100) -#define MS4525_FIFO_BUFFERS (FIFO_MIN_BUFFERS + MS4525_HZ / EPOCH_HZ) #define PITOT_HZ (MS4525_HZ) #define PITOT_I2C (&hi2c1) #define PITOT_I2C_ADDRESS (MS4525_I2C_ADDRESS) @@ -170,11 +164,9 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s) #define AUAV_HZ (100) // Absolute (Baro) #define AUAV_BARO_HZ (AUAV_HZ) // real value is lower -#define AUAV_BARO_FIFO_BUFFERS (FIFO_MIN_BUFFERS + AUAV_BARO_HZ / EPOCH_HZ) // Differential (Pitot) #define AUAV_PITOT_HZ (AUAV_HZ) // real value is lower -#define AUAV_PITOT_FIFO_BUFFERS (FIFO_MIN_BUFFERS + AUAV_PITOT_HZ / EPOCH_HZ) // Digital Potentiometer used in later versions // #define MCP4017_I2C_ADDRESS (0x2F) @@ -190,25 +182,21 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s) // Baro is DPS310 #define DPS310_HZ (50) // up to 50 Hz. -#define DPS310_FIFO_BUFFERS (FIFO_MIN_BUFFERS + DPS310_HZ / EPOCH_HZ) // Mag is IIS2MDC // HZ no faster than 100Hz. 10, 20, 50, 100 are the only options for continuous mode #define IIS2MDC_HZ (100) -#define IIS2MDC_FIFO_BUFFERS (FIFO_MIN_BUFFERS + IIS2MDC_HZ / EPOCH_HZ) #define IIS2MDC_ROTATION (const double[]){1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0} // for mag, z coordinate is already adjusted for right hand rule. // Mag IST8308 (pixracer Pro) // HZ no faster than 100Hz #define IST3808_HZ (100) -#define IST8308_FIFO_BUFFERS (FIFO_MIN_BUFFERS + IST3808_HZ / EPOCH_HZ) #define IST3808_I2C (&hi2c1) #define IST3808_I2C_ADDRESS (0X0C) #define IST3808_ROTATION (const double[]){1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0} // for mag, z coordinate is already adjusted for right hand rule. // SBus is on UART3 for Varmints, UART6 for PixRacer Pro #define SBUS_HZ (112) // 1000/9ms = 111.1Hz, 112 is rounds up -#define SBUS_FIFO_BUFFERS (FIFO_MIN_BUFFERS + SBUS_HZ / EPOCH_HZ) #define SBUS_BAUD (100000) // #define RC_HZ (SBUS_HZ) @@ -220,16 +208,13 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s) // uBlox #define UBX_HZ (10) #define UBX_NUM (3) // number of different types of packets -#define UBX_FIFO_BUFFERS (UBX_NUM * (FIFO_MIN_BUFFERS + UBX_HZ / EPOCH_HZ)) -//#define UBX_BAUD (57600) +#define UBX_BAUD (115200) #define GPS_HZ (UBX_HZ) -#define GPS_BAUD (57600) +#define GPS_BAUD (UBX_BAUD) -#define UBX_PROTOCOL (UBX_M9) -#define GPS_HAS_PPS (false) -#define GPS_PPS_PORT 0 // ignored on pixracer -#define GPS_PPS_PIN 0 // ignored on pixracer +#define GPS_PPS_PORT GPS_PPS_GPIO_Port // wire up to MOSI pin on external SPI connector +#define GPS_PPS_PIN GPS_PPS_Pin // wire up to MOSI pin on external SPI connector #define GPS_UART (&huart4) #define GPS_UART_INSTANCE (UART4) #define GPS_UART_DMA (&hdma_uart4_rx) @@ -238,8 +223,9 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s) // Serial #define SERIAL_HZ (EPOCH_HZ) // Loop time is driven by IMU period. #define SERIAL_QOS_FIFOS (3) -#define SERIAL_TX_FIFO_BUFFERS (PACKET_FIFO_MAX_BUFFERS) #define SERIAL_RX_FIFO_BUFFER_BYTES (4096) +#define SERIAL_TX_FIFO_BUFFERS (PACKET_FIFO_MAX_BUFFERS) + // Telem (USART2) #define TELEM_HZ (SERIAL_HZ) #define TELEM_BAUD (921600) //(57600) @@ -251,7 +237,6 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s) // Onboard ADC's #define ADC_HZ (10) // Maximum is 500 Hz. -#define ADC_FIFO_BUFFERS (FIFO_MIN_BUFFERS) #define ADC_ADC_EXTERNAL (&hadc1) #define ADC_ADC_INSTANCE_EXTERNAL (ADC1) @@ -279,7 +264,7 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s) { \ {ADC_REGULAR_RANK_1, ADC_CHANNEL_11, 1.000, 0.0}, /* ADC_RSSI_V */ \ {ADC_REGULAR_RANK_2, ADC_CHANNEL_14, 12.62, 0.0}, /* ADC_BATTERY_VOLTS */ \ - {ADC_REGULAR_RANK_3, ADC_CHANNEL_15, 60.5, 0.0}, /* ADC_BATTERY_CURRENT */ \ + {ADC_REGULAR_RANK_3, ADC_CHANNEL_15, 60.5, 0.0747}, /* ADC_BATTERY_CURRENT */ \ {ADC_REGULAR_RANK_4, ADC_CHANNEL_18, 2.000, 0.0}, /* ADC_5V0 */ \ {ADC_REGULAR_RANK_1, ADC_CHANNEL_TEMPSENSOR, 1.000, 0.0}, /* ADC_STM_TEMPERATURE */ \ {ADC_REGULAR_RANK_2, ADC_CHANNEL_VBAT, 4.000, 0.0}, /* ADC_STM_VBAT */ \ @@ -308,14 +293,22 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s) // Probes #if 1 // Probe PIN PG9 -#define PROBE1_HI HAL_GPIO_WritePin(GPIOG, GPIO_PIN_9, GPIO_PIN_SET) -#define PROBE1_LO HAL_GPIO_WritePin(GPIOG, GPIO_PIN_9, GPIO_PIN_RESET) -#define PROBE1_TOG HAL_GPIO_TogglePin(GPIOG, GPIO_PIN_9) +#define PROBE1_HI HAL_GPIO_WritePin(PROBE1_GPIO_Port, PROBE1_Pin, GPIO_PIN_SET) +#define PROBE1_LO HAL_GPIO_WritePin(PROBE1_GPIO_Port, PROBE1_Pin, GPIO_PIN_RESET) +#define PROBE1_TOG HAL_GPIO_TogglePin(PROBE1_GPIO_Port, PROBE1_Pin) + +#define PROBE2_HI HAL_GPIO_WritePin(PROBE2_GPIO_Port, PROBE2_Pin, GPIO_PIN_SET) +#define PROBE2_LO HAL_GPIO_WritePin(PROBE2_GPIO_Port, PROBE2_Pin, GPIO_PIN_RESET) +#define PROBE2_TOG HAL_GPIO_TogglePin(PROBE2_GPIO_Port, PROBE2_Pin) + +#define PROBE3_HI HAL_GPIO_WritePin(PROBE3_GPIO_Port, PROBE3_Pin, GPIO_PIN_SET) +#define PROBE3_LO HAL_GPIO_WritePin(PROBE3_GPIO_Port, PROBE3_Pin, GPIO_PIN_RESET) +#define PROBE3_TOG HAL_GPIO_TogglePin(PROBE3_GPIO_Port, PROBE3_Pin) + #else #define PROBE1_HI #define PROBE1_LO #define PROBE1_TOG -#endif #define PROBE2_HI #define PROBE2_LO @@ -328,5 +321,6 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s) #define PROBE4_HI #define PROBE4_LO #define PROBE4_TOG +#endif #endif /* BOARDCONFIG_H_ */ diff --git a/boards/varmint_h7/pixracer_pro/specific/Callbacks.cpp b/boards/varmint_h7/pixracer_pro/specific/Callbacks.cpp index 09a0de4d..53790ada 100644 --- a/boards/varmint_h7/pixracer_pro/specific/Callbacks.cpp +++ b/boards/varmint_h7/pixracer_pro/specific/Callbacks.cpp @@ -88,6 +88,7 @@ void HAL_GPIO_EXTI_Callback(uint16_t exti_pin) HAL_GPIO_WritePin(BMI088_INT2_ACCEL_GPIO_Port, BMI088_INT2_ACCEL_Pin, GPIO_PIN_RESET); if (varmint.imu0_.isMy(exti_pin)) varmint.imu0_.startDma(); + if (varmint.gps_.isMy(exti_pin)) { varmint.gps_.pps(time64.Us()); } } ////////////////////////////////////////////////////////////////////////////////////////// diff --git a/boards/varmint_h7/pixracer_pro/specific/Varmint_Init.cpp b/boards/varmint_h7/pixracer_pro/specific/Varmint_Init.cpp index 32d3ab39..85a566ab 100644 --- a/boards/varmint_h7/pixracer_pro/specific/Varmint_Init.cpp +++ b/boards/varmint_h7/pixracer_pro/specific/Varmint_Init.cpp @@ -90,7 +90,7 @@ void Varmint::init_board(void) MX_SPI1_Init(); MX_SPI2_Init(); MX_SPI5_Init(); - MX_SPI6_Init(); +// MX_SPI6_Init(); // NOt using // MX_TIM1_Init(); // PWM, initialized elsewhere // MX_TIM2_Init(); // PWM (Buzzer) not used as such @@ -181,8 +181,8 @@ void Varmint::init_board(void) // GPS initialization misc_printf("\n\nUbx (gps) Initialization\n"); - init_status = gps_.init(GPS_HZ, GPS_PPS_PORT, GPS_PPS_PIN, GPS_HAS_PPS, GPS_UART, GPS_UART_INSTANCE, GPS_UART_DMA, - GPS_BAUD, UBX_PROTOCOL); + init_status = gps_.init(GPS_HZ, GPS_PPS_PORT, GPS_PPS_PIN, GPS_UART, GPS_UART_INSTANCE, GPS_UART_DMA, + GPS_BAUD); misc_exit_status(init_status); status_list_[status_len_++] = &gps_; @@ -257,6 +257,7 @@ void Varmint::init_board(void) HAL_NVIC_EnableIRQ(BMI088_INT4_GYRO_EXTI_IRQn); // EXTI4_IRQn Gyro DRDY Feedback HAL_NVIC_EnableIRQ(BMI088_INT1_ACCEL_EXTI_IRQn); // EXTI1_IRQn ACCEL DRDY + HAL_NVIC_EnableIRQ(GPS_PPS_EXTI_IRQn); // EXTI15_10_IRQn GPS PPD __HAL_UART_ENABLE_IT(gps_.huart(), UART_IT_IDLE); __HAL_UART_ENABLE_IT(rc_.huart(), UART_IT_IDLE); diff --git a/boards/varmint_h7/pixracer_pro/specific/sandbox.cpp b/boards/varmint_h7/pixracer_pro/specific/sandbox.cpp index d4244392..19d7962e 100644 --- a/boards/varmint_h7/pixracer_pro/specific/sandbox.cpp +++ b/boards/varmint_h7/pixracer_pro/specific/sandbox.cpp @@ -47,7 +47,7 @@ extern Time64 time64; extern bool verbose; -#define ROWSIZE 180 +#define ROWSIZE 256 #define ASCII_ESC 27 void verbose_dashes(void) diff --git a/boards/varmint_h7/varmint_10X/.cproject b/boards/varmint_h7/varmint_10X/.cproject index ff1e5c3e..909cf14d 100644 --- a/boards/varmint_h7/varmint_10X/.cproject +++ b/boards/varmint_h7/varmint_10X/.cproject @@ -112,16 +112,17 @@ + + + + + - - - - - + @@ -593,4 +594,4 @@ - + \ No newline at end of file diff --git a/boards/varmint_h7/varmint_10X/specific/BoardConfig.h b/boards/varmint_h7/varmint_10X/specific/BoardConfig.h index b4db5774..d5a7b306 100644 --- a/boards/varmint_h7/varmint_10X/specific/BoardConfig.h +++ b/boards/varmint_h7/varmint_10X/specific/BoardConfig.h @@ -84,8 +84,6 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s) /**/ // clang-format on -#define AUAV_FIFO_BUFFERS 0 // not used in pixracer pro - // 48-bit us counter. // Prefer to have the 32-bit counter on the low order bytes: #define HTIM_LOW (&htim5) // 32-bit counter @@ -141,12 +139,10 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s) #define BMI088_HZ (EPOCH_HZ) // 400, 1000, 2000 are the only options #define BMI088_RANGE_A (3) // 0,1,2,3 --> 3,6,12,24g for BMI088; 2 4 8 16g for BMI 085 #define BMI088_RANGE_G (2) // 0,1,2,3,4 --> 2000,1000,500,250,125 deg/s -#define BMI088_FIFO_BUFFERS (FIFO_MIN_BUFFERS + BMI088_HZ / EPOCH_HZ) #define BMI088_ROTATION (const double[]){-1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0} // ADIS IMU #define ADIS165XX_HZ (EPOCH_HZ) -#define ADIS165XX_FIFO_BUFFERS (FIFO_MIN_BUFFERS + ADIS165XX_HZ / EPOCH_HZ) #define ADIS165XX_SPI (&hspi4) #define ADIS165XX_HTIM (&htim12) // ADIS 16500 ExtClk @@ -158,7 +154,6 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s) // DLHR Pitot is on i2c1 #define DLHRL20G_HZ (100) -#define DLHRL20G_FIFO_BUFFERS (FIFO_MIN_BUFFERS + DLHRL20G_HZ / EPOCH_HZ) #define PITOT_DRDY_PORT (PITOT_DRDY_GPIO_Port) #define PITOT_DRDY_PIN (PITOT_DRDY_Pin) @@ -169,18 +164,15 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s) // MS4525D Pitot #define MS4525_HZ (100) -#define MS4525_FIFO_BUFFERS (FIFO_MIN_BUFFERS + MS4525_HZ / EPOCH_HZ) // AUAV is both baro (absolute) and differtial (Pitot) #define AUAV_SPI (&hspi4) #define AUAV_HZ (100) // Absolute (Baro) #define AUAV_BARO_HZ (AUAV_HZ) // real value is lower -#define AUAV_BARO_FIFO_BUFFERS (FIFO_MIN_BUFFERS + AUAV_BARO_HZ / EPOCH_HZ) // Differential (Pitot) #define AUAV_PITOT_HZ (AUAV_HZ) // real value is lower -#define AUAV_PITOT_FIFO_BUFFERS (FIFO_MIN_BUFFERS + AUAV_PITOT_HZ / EPOCH_HZ) // Digital Potentiometer used in later versions // #define MCP4017_I2C_ADDRESS (0x2F) @@ -197,25 +189,21 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s) // Baro is DPS310 #define DPS310_HZ (50) // up to 50 Hz. -#define DPS310_FIFO_BUFFERS (FIFO_MIN_BUFFERS + DPS310_HZ / EPOCH_HZ) // Mag is IIS2MDC // HZ no faster than 100Hz. 10, 20, 50, 100 are the only options for continuous mode #define IIS2MDC_HZ (100) -#define IIS2MDC_FIFO_BUFFERS (FIFO_MIN_BUFFERS + IIS2MDC_HZ / EPOCH_HZ) #define IIS2MDC_ROTATION (const double[]){-1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, -1.0} // Mag IST8308 (pixracer Pro) // HZ no faster than 100Hz #define IST3808_HZ (100) -#define IST8308_FIFO_BUFFERS (FIFO_MIN_BUFFERS + IST3808_HZ / EPOCH_HZ) #define IST3808_I2C (&hi2c1) #define IST3808_I2C_ADDRESS (0X0C) #define IST3808_ROTATION (const double[]){-1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, -1.0} // SBus is on UART3 for Varmints, UART6 for PixRacer Pro #define SBUS_HZ (112) // 1000/9ms = 111.1Hz, 112 is rounds up -#define SBUS_FIFO_BUFFERS (FIFO_MIN_BUFFERS + SBUS_HZ / EPOCH_HZ) #define SBUS_BAUD (100000) // #define RC_HZ (SBUS_HZ) @@ -227,14 +215,11 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s) // uBlox #define UBX_HZ (10) #define UBX_NUM (3) // number of different types of packets -#define UBX_FIFO_BUFFERS (UBX_NUM * (FIFO_MIN_BUFFERS + UBX_HZ / EPOCH_HZ)) -//#define UBX_BAUD (57600) +#define UBX_BAUD (115200) #define GPS_HZ (UBX_HZ) -#define GPS_BAUD (57600) +#define GPS_BAUD (UBX_BAUD) -#define UBX_PROTOCOL (UBX_M8) -#define GPS_HAS_PPS (true) #define GPS_PPS_PORT (GPS_1PPS_GPIO_Port) #define GPS_PPS_PIN (GPS_1PPS_Pin) #define GPS_UART (&huart1) @@ -258,7 +243,6 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s) // Onboard ADC's #define ADC_HZ (10) // Maximum is 500 Hz. -#define ADC_FIFO_BUFFERS (FIFO_MIN_BUFFERS) #define ADC_ADC_EXTERNAL (&hadc1) #define ADC_ADC_INSTANCE_EXTERNAL (ADC1) diff --git a/boards/varmint_h7/varmint_10X/specific/Varmint_Init.cpp b/boards/varmint_h7/varmint_10X/specific/Varmint_Init.cpp index ed5ff671..1e98a963 100644 --- a/boards/varmint_h7/varmint_10X/specific/Varmint_Init.cpp +++ b/boards/varmint_h7/varmint_10X/specific/Varmint_Init.cpp @@ -185,8 +185,7 @@ void Varmint::init_board(void) // GPS initialization misc_printf("\n\nUbx (gps) Initialization\n"); - init_status = gps_.init(GPS_HZ, GPS_PPS_PORT, GPS_PPS_PIN, GPS_HAS_PPS, GPS_UART, GPS_UART_INSTANCE, GPS_UART_DMA, - GPS_BAUD, UBX_PROTOCOL); + init_status = gps_.init(GPS_HZ, GPS_PPS_PORT, GPS_PPS_PIN, GPS_UART, GPS_UART_INSTANCE, GPS_UART_DMA, GPS_BAUD); misc_exit_status(init_status); status_list_[status_len_++] = &gps_; diff --git a/boards/varmint_h7/varmint_10X/specific/sandbox.cpp b/boards/varmint_h7/varmint_10X/specific/sandbox.cpp index 00753877..614b4249 100644 --- a/boards/varmint_h7/varmint_10X/specific/sandbox.cpp +++ b/boards/varmint_h7/varmint_10X/specific/sandbox.cpp @@ -47,7 +47,7 @@ extern Time64 time64; extern bool verbose; -#define ROWSIZE 180 +#define ROWSIZE 256 #define ASCII_ESC 27 void verbose_dashes(void) diff --git a/build_varmint.sh b/build_varmint.sh deleted file mode 100644 index 4164a3a4..00000000 --- a/build_varmint.sh +++ /dev/null @@ -1,18 +0,0 @@ -# Building the projects: -# mkdir varmint_build && cd varmint_build && cmake .. -DBOARD_TO_BUILD=varmint && make -j -# mkdir pixracer_pro_build && cd pixracer_pro_build && cmake .. -DBOARD_TO_BUILD=pixracer_pro && make -j -# -# alternate for make -j above is cmake --build . -j (note only one .) -# make -j OR cmake --build . -j -# to clean: -# make clean -j OR make --build . --target clean -j -# -# unit test: -# build: -# mkdir test_build && cd test_build && cmake .. -DBOARD_TO_BUILD=test -DCMAKE_BUILD_TYPE=Release && make -j -# test: -# ./test/unit_tests - - -sudo rm -r varmint_build -mkdir varmint_build && cd varmint_build && cmake .. -DBOARD_TO_BUILD=varmint && make -j \ No newline at end of file diff --git a/comms/mavlink/generating_v1.0_instructions.txt b/comms/mavlink/generating_v1.0_instructions.txt index 9a99309b..7b8e5f45 100644 --- a/comms/mavlink/generating_v1.0_instructions.txt +++ b/comms/mavlink/generating_v1.0_instructions.txt @@ -1,5 +1,6 @@ # get the mavlink/mavlink repository -git clone https://github.com/rosflight/mavlink.git +# git clone https://github.com/rosflight/mavlink.git +git clone https://github.com/mavlink/mavlink.git # Go to your rosflight_firmware/comms/mavlink folder # Put the rosflight.xml file there if not already there diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index 7009e561..683ace5d 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -143,25 +143,25 @@ void Mavlink::send_imu(uint8_t system_id, uint64_t timestamp_us, const turbomath send_message(msg, 0); } -void Mavlink::send_gnss(uint8_t system_id, const GnssStruct & data) +void Mavlink::send_gnss(uint8_t system_id, GnssStruct * data) { mavlink_message_t msg; mavlink_msg_rosflight_gnss_pack( system_id, compid_, &msg, - data.unix_seconds, // Unix time in seconds - data.nano, // nanoseconds - data.fix_type, - data.num_sat, - (double)data.lat * 1e-7, // Convert 100's of nanodegs into deg (DDS format) - (double)data.lon * 1e-7, // Convert 100's of nanodegs into deg (DDS format) - (float)data.height_msl * 1e-3, // mm to m - (float)data.vel_n * 1e-3, // mm/s to m/s - (float)data.vel_e * 1e-3, // mm/s to m/s - (float)data.vel_d * 1e-3, // mm/s to m/s - (float)data.h_acc * 1e-3, // mm/s to m/s - (float)data.v_acc * 1e-3, // mm/s to m/s - (float)data.speed_accy * 1e-3, // mm/s to m/s - (float)data.header.timestamp); + data->unix_seconds, // Unix time in seconds + data->unix_nanos, // Unix nanoseconds + data->fix_type, + data->num_sat, + data->lat, + data->lon, + data->height_msl, + data->vel_n, + data->vel_e, + data->vel_d, + data->h_acc, + data->v_acc, + data->speed_accy, + data->header.timestamp); send_message(msg); } diff --git a/comms/mavlink/mavlink.h b/comms/mavlink/mavlink.h index 3da704dd..3c38e539 100644 --- a/comms/mavlink/mavlink.h +++ b/comms/mavlink/mavlink.h @@ -69,10 +69,6 @@ class Mavlink : public CommLinkInterface const turbomath::Vector & gyro, float temperature) override; void send_log_message(uint8_t system_id, LogSeverity severity, const char * text) override; void send_mag(uint8_t system_id, const turbomath::Vector & mag) override; -// void send_named_value_int(uint8_t system_id, uint32_t timestamp_ms, const char * const name, -// int32_t value) override; -// void send_named_value_float(uint8_t system_id, uint32_t timestamp_ms, const char * const name, -// float value) override; void send_output_raw(uint8_t system_id, uint32_t timestamp_ms, const float raw_outputs[14]) override; void send_param_value_int(uint8_t system_id, uint16_t index, const char * const name, @@ -88,7 +84,7 @@ class Mavlink : public CommLinkInterface int16_t loop_time_us) override; void send_timesync(uint8_t system_id, int64_t tc1, int64_t ts1) override; void send_version(uint8_t system_id, const char * const version) override; - void send_gnss(uint8_t system_id, const GnssStruct & data) override; + void send_gnss(uint8_t system_id, GnssStruct * data) override; void send_error_data(uint8_t system_id, const StateManager::BackupData & error_data) override; void send_battery_status(uint8_t system_id, float voltage, float current) override; diff --git a/comms/mavlink/rosflight.xml b/comms/mavlink/rosflight.xml index 3fe2b439..aab826bc 100644 --- a/comms/mavlink/rosflight.xml +++ b/comms/mavlink/rosflight.xml @@ -118,14 +118,6 @@ python3 -m pymavlink.tools.mavgen --lang=C --wire-protocol=1.0 --output=generate - - - - - - - - Generic micro air vehicle. diff --git a/comms/mavlink/v1.0/checksum.h b/comms/mavlink/v1.0/checksum.h index 0f30b659..fcd5e1d5 100644 --- a/comms/mavlink/v1.0/checksum.h +++ b/comms/mavlink/v1.0/checksum.h @@ -23,7 +23,7 @@ extern "C" { #ifndef HAVE_CRC_ACCUMULATE /** - * @brief Accumulate the X.25 CRC by adding one char at a time. + * @brief Accumulate the MCRF4XX CRC16 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 +44,9 @@ static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) /** - * @brief Initiliaze the buffer for the X.25 CRC + * @brief Initialize 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 +55,7 @@ static inline void crc_init(uint16_t* crcAccum) /** - * @brief Calculates the X.25 checksum on a byte buffer + * @brief Calculates the MCRF4XX CRC16 checksum on a byte buffer * * @param pBuffer buffer containing the byte array to hash * @param length length of the byte array @@ -73,7 +73,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 CRC 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). diff --git a/comms/mavlink/v1.0/mavlink_conversions.h b/comms/mavlink/v1.0/mavlink_conversions.h index 63bcfa39..d45e6bab 100644 --- a/comms/mavlink/v1.0/mavlink_conversions.h +++ b/comms/mavlink/v1.0/mavlink_conversions.h @@ -1,6 +1,8 @@ #ifndef _MAVLINK_CONVERSIONS_H_ #define _MAVLINK_CONVERSIONS_H_ +#ifndef MAVLINK_NO_CONVERSION_HELPERS + /* enable math defines on Windows */ #ifdef _MSC_VER #ifndef _USE_MATH_DEFINES @@ -208,4 +210,7 @@ MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, flo dcm[2][2] = cosPhi * cosThe; } -#endif +#endif // MAVLINK_NO_CONVERSION_HELPERS + +#endif // _MAVLINK_CONVERSIONS_H_ + diff --git a/comms/mavlink/v1.0/mavlink_helpers.h b/comms/mavlink/v1.0/mavlink_helpers.h index 0fa87fcc..73a71330 100644 --- a/comms/mavlink/v1.0/mavlink_helpers.h +++ b/comms/mavlink/v1.0/mavlink_helpers.h @@ -52,6 +52,46 @@ MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan) status->parse_state = MAVLINK_PARSE_STATE_IDLE; } +/** + * @brief Finalize a MAVLink message with channel assignment + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. This function + * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack + * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. + * + * @param msg Message to finalize + * @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_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) +#else +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 length) +#endif +{ + // 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; + msg->sysid = system_id; + msg->compid = component_id; + msg->seq = status->current_tx_seq; + status->current_tx_seq = status->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); + + return length + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + /** * @brief Finalize a MAVLink message with channel assignment * @@ -66,12 +106,14 @@ MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan) */ #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 length, uint8_t crc_extra) + 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 { + // 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; @@ -97,9 +139,9 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui */ #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 length, uint8_t crc_extra) + uint8_t min_length, uint8_t length, uint8_t crc_extra) { - return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, 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, @@ -117,7 +159,7 @@ MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, */ #if MAVLINK_CRC_EXTRA MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, - uint8_t length, uint8_t crc_extra) + 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 @@ -204,38 +246,17 @@ MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) } /** - * This is a varient of mavlink_frame_char() but with caller supplied + * This is a variant of mavlink_frame_char() but with caller supplied * parsing buffers. It is useful when you want to create a MAVLink * parser in a library that doesn't use any global variables * * @param rxmsg parsing message buffer - * @param status parsing starus buffer + * @param status parsing status buffer * @param c The char to parse * - * @param returnMsg NULL if no message could be decoded, the message data else - * @param returnStats if a message was decoded, this is filled with the channel's stats + * @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 - * - * A typical use scenario of this function call is: - * - * @code - * #include - * - * mavlink_message_t msg; - * int chan = 0; - * - * - * while(serial.bytesAvailable > 0) - * { - * uint8_t byte = serial.getNextByte(); - * if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE) - * { - * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); - * } - * } - * - * - * @endcode */ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, mavlink_status_t* status, @@ -267,8 +288,6 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, #endif #endif - int bufferIndex = 0; - status->msg_received = MAVLINK_FRAMING_INCOMPLETE; switch (status->parse_state) @@ -383,8 +402,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, break; } - bufferIndex++; - // If a message has been sucessfully decoded, check index + // If a message has been successfully decoded, check index if (status->msg_received == MAVLINK_FRAMING_OK) { //while(status->current_seq != rxmsg->seq) @@ -428,17 +446,17 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, * 2 (MAVLINK_FRAMING_INCOMPLETE, MAVLINK_FRAMING_OK or MAVLINK_FRAMING_BAD_CRC) * * Messages are parsed into an internal buffer (one for each channel). When a complete - * message is received it is copies into *returnMsg and the channel's status is - * copied into *returnStats. + * message is received it is copies into *r_message and the channel's status is + * copied into *r_mavlink_status. * - * @param chan ID of the current channel. This allows to parse different channels with this function. - * a channel is not a physical message channel like a serial port, but a logic partition of - * the communication streams in this case. COMM_NB is the limit for the number of channels + * @param chan ID of the channel to be parsed. + * A channel is not a physical message channel like a serial port, but a logical partition of + * the communication streams. COMM_NB is the limit for the number of channels * 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 returnMsg NULL if no message could be decoded, the message data else - * @param returnStats if a message was decoded, this is filled with the channel's stats + * @param r_message NULL if no message could be decoded, the message data else + * @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 * * A typical use scenario of this function call is: @@ -446,6 +464,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, * @code * #include * + * mavlink_status_t status; * mavlink_message_t msg; * int chan = 0; * @@ -453,7 +472,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, * while(serial.bytesAvailable > 0) * { * uint8_t byte = serial.getNextByte(); - * if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE) + * if (mavlink_frame_char(chan, byte, &msg, &status) != MAVLINK_FRAMING_INCOMPLETE) * { * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); * } @@ -478,17 +497,17 @@ MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_messa * it could be successfully decoded. This function will return 0 or 1. * * Messages are parsed into an internal buffer (one for each channel). When a complete - * message is received it is copies into *returnMsg and the channel's status is - * copied into *returnStats. + * message is received it is copies into *r_message and the channel's status is + * copied into *r_mavlink_status. * - * @param chan ID of the current channel. This allows to parse different channels with this function. - * a channel is not a physical message channel like a serial port, but a logic partition of - * the communication streams in this case. COMM_NB is the limit for the number of channels + * @param chan ID of the channel to be parsed. + * A channel is not a physical message channel like a serial port, but a logical partition of + * the communication streams. COMM_NB is the limit for the number of channels * 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 returnMsg NULL if no message could be decoded, the message data else - * @param returnStats if a message was decoded, this is filled with the channel's stats + * @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 * * A typical use scenario of this function call is: @@ -496,6 +515,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_messa * @code * #include * + * mavlink_status_t status; * mavlink_message_t msg; * int chan = 0; * @@ -503,7 +523,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_messa * while(serial.bytesAvailable > 0) * { * uint8_t byte = serial.getNextByte(); - * if (mavlink_parse_char(chan, byte, &msg)) + * if (mavlink_parse_char(chan, byte, &msg, &status)) * { * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); * } diff --git a/comms/mavlink/v1.0/mavlink_types.h b/comms/mavlink/v1.0/mavlink_types.h index 0a98ccc0..5b577a33 100644 --- a/comms/mavlink/v1.0/mavlink_types.h +++ b/comms/mavlink/v1.0/mavlink_types.h @@ -170,12 +170,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 diff --git a/comms/mavlink/v1.0/protocol.h b/comms/mavlink/v1.0/protocol.h index bf207084..f48d947f 100644 --- a/comms/mavlink/v1.0/protocol.h +++ b/comms/mavlink/v1.0/protocol.h @@ -45,19 +45,23 @@ #endif MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan); #if MAVLINK_CRC_EXTRA + 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); 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, uint8_t crc_extra); + uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra); MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length, uint8_t crc_extra); + uint8_t min_length, uint8_t length, uint8_t crc_extra); #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, - uint8_t length, uint8_t crc_extra); + uint8_t min_length, uint8_t length, uint8_t crc_extra); #endif #else + 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); 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); + uint8_t chan, uint8_t length); MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length); + uint8_t length); #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length); #endif @@ -239,9 +243,9 @@ _MAV_PUT_ARRAY(int64_t, i64) _MAV_PUT_ARRAY(float, f) _MAV_PUT_ARRAY(double, d) -#define _MAV_RETURN_char(msg, wire_offset) (const char)_MAV_PAYLOAD(msg)[wire_offset] -#define _MAV_RETURN_int8_t(msg, wire_offset) (const int8_t)_MAV_PAYLOAD(msg)[wire_offset] -#define _MAV_RETURN_uint8_t(msg, wire_offset) (const uint8_t)_MAV_PAYLOAD(msg)[wire_offset] +#define _MAV_RETURN_char(msg, wire_offset) (char)_MAV_PAYLOAD(msg)[wire_offset] +#define _MAV_RETURN_int8_t(msg, wire_offset) (int8_t)_MAV_PAYLOAD(msg)[wire_offset] +#define _MAV_RETURN_uint8_t(msg, wire_offset) (uint8_t)_MAV_PAYLOAD(msg)[wire_offset] #if MAVLINK_NEED_BYTE_SWAP #define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \ diff --git a/comms/mavlink/v1.0/rosflight/mavlink.h b/comms/mavlink/v1.0/rosflight/mavlink.h index 226d5d67..a5f93602 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink.h +++ b/comms/mavlink/v1.0/rosflight/mavlink.h @@ -1,10 +1,13 @@ /** @file - * @brief MAVLink comm protocol built from rosflight.xml - * @see http://mavlink.org + * @brief MAVLink comm protocol built from rosflight.xml + * @see http://mavlink.org */ +#pragma once #ifndef MAVLINK_H #define MAVLINK_H +#define MAVLINK_PRIMARY_XML_HASH 9006179629977798615 + #ifndef MAVLINK_STX #define MAVLINK_STX 254 #endif @@ -21,6 +24,10 @@ #define MAVLINK_CRC_EXTRA 1 #endif +#ifndef MAVLINK_COMMAND_24BIT +#define MAVLINK_COMMAND_24BIT 0 +#endif + #include "version.h" #include "rosflight.h" diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_attitude_quaternion.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_attitude_quaternion.h index df362b43..ebfafe7e 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_attitude_quaternion.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_attitude_quaternion.h @@ -1,31 +1,36 @@ +#pragma once // MESSAGE ATTITUDE_QUATERNION PACKING #define MAVLINK_MSG_ID_ATTITUDE_QUATERNION 31 -typedef struct __mavlink_attitude_quaternion_t -{ - uint32_t time_boot_ms; /*< Timestamp (milliseconds since system boot)*/ - float q1; /*< Quaternion component 1, w (1 in null-rotation)*/ - float q2; /*< Quaternion component 2, x (0 in null-rotation)*/ - float q3; /*< Quaternion component 3, y (0 in null-rotation)*/ - float q4; /*< Quaternion component 4, z (0 in null-rotation)*/ - float rollspeed; /*< Roll angular speed (rad/s)*/ - float pitchspeed; /*< Pitch angular speed (rad/s)*/ - float yawspeed; /*< Yaw angular speed (rad/s)*/ + +typedef struct __mavlink_attitude_quaternion_t { + uint32_t time_boot_ms; /*< Timestamp (milliseconds since system boot)*/ + float q1; /*< Quaternion component 1, w (1 in null-rotation)*/ + float q2; /*< Quaternion component 2, x (0 in null-rotation)*/ + float q3; /*< Quaternion component 3, y (0 in null-rotation)*/ + float q4; /*< Quaternion component 4, z (0 in null-rotation)*/ + float rollspeed; /*< Roll angular speed (rad/s)*/ + float pitchspeed; /*< Pitch angular speed (rad/s)*/ + float yawspeed; /*< Yaw angular speed (rad/s)*/ } mavlink_attitude_quaternion_t; #define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN 32 +#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN 32 #define MAVLINK_MSG_ID_31_LEN 32 +#define MAVLINK_MSG_ID_31_MIN_LEN 32 #define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC 246 #define MAVLINK_MSG_ID_31_CRC 246 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION { \ - "ATTITUDE_QUATERNION", \ - 8, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_quaternion_t, time_boot_ms) }, \ + 31, \ + "ATTITUDE_QUATERNION", \ + 8, \ + { { "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) }, \ { "q3", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_attitude_quaternion_t, q3) }, \ @@ -35,7 +40,21 @@ typedef struct __mavlink_attitude_quaternion_t { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_attitude_quaternion_t, yawspeed) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION { \ + "ATTITUDE_QUATERNION", \ + 8, \ + { { "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) }, \ + { "q3", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_attitude_quaternion_t, q3) }, \ + { "q4", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_attitude_quaternion_t, q4) }, \ + { "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) }, \ + } \ +} +#endif /** * @brief Pack a attitude_quaternion message @@ -43,50 +62,100 @@ typedef struct __mavlink_attitude_quaternion_t * @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 Timestamp (milliseconds since system boot) - * @param q1 Quaternion component 1, w (1 in null-rotation) - * @param q2 Quaternion component 2, x (0 in null-rotation) - * @param q3 Quaternion component 3, y (0 in null-rotation) - * @param q4 Quaternion component 4, z (0 in null-rotation) - * @param rollspeed Roll angular speed (rad/s) - * @param pitchspeed Pitch angular speed (rad/s) - * @param yawspeed Yaw angular speed (rad/s) + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param q1 Quaternion component 1, w (1 in null-rotation) + * @param q2 Quaternion component 2, x (0 in null-rotation) + * @param q3 Quaternion component 3, y (0 in null-rotation) + * @param q4 Quaternion component 4, z (0 in null-rotation) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) * @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) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, q1); + _mav_put_float(buf, 8, q2); + _mav_put_float(buf, 12, q3); + _mav_put_float(buf, 16, q4); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#else + mavlink_attitude_quaternion_t packet; + packet.time_boot_ms = time_boot_ms; + packet.q1 = q1; + packet.q2 = q2; + packet.q3 = q3; + packet.q4 = q4; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); +} + +/** + * @brief Pack a attitude_quaternion message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param q1 Quaternion component 1, w (1 in null-rotation) + * @param q2 Quaternion component 2, x (0 in null-rotation) + * @param q3 Quaternion component 3, y (0 in null-rotation) + * @param q4 Quaternion component 4, z (0 in null-rotation) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_quaternion_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, q1); - _mav_put_float(buf, 8, q2); - _mav_put_float(buf, 12, q3); - _mav_put_float(buf, 16, q4); - _mav_put_float(buf, 20, rollspeed); - _mav_put_float(buf, 24, pitchspeed); - _mav_put_float(buf, 28, yawspeed); + char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, q1); + _mav_put_float(buf, 8, q2); + _mav_put_float(buf, 12, q3); + _mav_put_float(buf, 16, q4); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); #else - mavlink_attitude_quaternion_t packet; - packet.time_boot_ms = time_boot_ms; - packet.q1 = q1; - packet.q2 = q2; - packet.q3 = q3; - packet.q4 = q4; - packet.rollspeed = rollspeed; - packet.pitchspeed = pitchspeed; - packet.yawspeed = yawspeed; + mavlink_attitude_quaternion_t packet; + packet.time_boot_ms = time_boot_ms; + packet.q1 = q1; + packet.q2 = q2; + packet.q3 = q3; + packet.q4 = q4; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION; + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); #endif } @@ -96,52 +165,48 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, u * @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 Timestamp (milliseconds since system boot) - * @param q1 Quaternion component 1, w (1 in null-rotation) - * @param q2 Quaternion component 2, x (0 in null-rotation) - * @param q3 Quaternion component 3, y (0 in null-rotation) - * @param q4 Quaternion component 4, z (0 in null-rotation) - * @param rollspeed Roll angular speed (rad/s) - * @param pitchspeed Pitch angular speed (rad/s) - * @param yawspeed Yaw angular speed (rad/s) + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param q1 Quaternion component 1, w (1 in null-rotation) + * @param q2 Quaternion component 2, x (0 in null-rotation) + * @param q3 Quaternion component 3, y (0 in null-rotation) + * @param q4 Quaternion component 4, z (0 in null-rotation) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) * @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) + mavlink_message_t* msg, + uint32_t time_boot_ms,float q1,float q2,float q3,float q4,float rollspeed,float pitchspeed,float yawspeed) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, q1); - _mav_put_float(buf, 8, q2); - _mav_put_float(buf, 12, q3); - _mav_put_float(buf, 16, q4); - _mav_put_float(buf, 20, rollspeed); - _mav_put_float(buf, 24, pitchspeed); - _mav_put_float(buf, 28, yawspeed); + char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, q1); + _mav_put_float(buf, 8, q2); + _mav_put_float(buf, 12, q3); + _mav_put_float(buf, 16, q4); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); #else - mavlink_attitude_quaternion_t packet; - packet.time_boot_ms = time_boot_ms; - packet.q1 = q1; - packet.q2 = q2; - packet.q3 = q3; - packet.q4 = q4; - packet.rollspeed = rollspeed; - packet.pitchspeed = pitchspeed; - packet.yawspeed = yawspeed; + mavlink_attitude_quaternion_t packet; + packet.time_boot_ms = time_boot_ms; + packet.q1 = q1; + packet.q2 = q2; + packet.q3 = q3; + packet.q4 = q4; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); } /** @@ -154,7 +219,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); } /** @@ -168,64 +233,84 @@ 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); +} + +/** + * @brief Encode a attitude_quaternion struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param attitude_quaternion C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_quaternion_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_attitude_quaternion_t* attitude_quaternion) +{ + return mavlink_msg_attitude_quaternion_pack_status(system_id, component_id, _status, 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); } /** * @brief Send a attitude_quaternion message * @param chan MAVLink channel to send the message * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param q1 Quaternion component 1, w (1 in null-rotation) - * @param q2 Quaternion component 2, x (0 in null-rotation) - * @param q3 Quaternion component 3, y (0 in null-rotation) - * @param q4 Quaternion component 4, z (0 in null-rotation) - * @param rollspeed Roll angular speed (rad/s) - * @param pitchspeed Pitch angular speed (rad/s) - * @param yawspeed Yaw angular speed (rad/s) + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param q1 Quaternion component 1, w (1 in null-rotation) + * @param q2 Quaternion component 2, x (0 in null-rotation) + * @param q3 Quaternion component 3, y (0 in null-rotation) + * @param q4 Quaternion component 4, z (0 in null-rotation) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) */ #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) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_float(buf, 4, q1); - _mav_put_float(buf, 8, q2); - _mav_put_float(buf, 12, q3); - _mav_put_float(buf, 16, q4); - _mav_put_float(buf, 20, rollspeed); - _mav_put_float(buf, 24, pitchspeed); - _mav_put_float(buf, 28, yawspeed); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); + char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, q1); + _mav_put_float(buf, 8, q2); + _mav_put_float(buf, 12, q3); + _mav_put_float(buf, 16, q4); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + + _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 - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); + mavlink_attitude_quaternion_t packet; + packet.time_boot_ms = time_boot_ms; + packet.q1 = q1; + packet.q2 = q2; + packet.q3 = q3; + packet.q4 = q4; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + + _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 -#else - mavlink_attitude_quaternion_t packet; - packet.time_boot_ms = time_boot_ms; - packet.q1 = q1; - packet.q2 = q2; - packet.q3 = q3; - packet.q4 = q4; - packet.rollspeed = rollspeed; - packet.pitchspeed = pitchspeed; - packet.yawspeed = yawspeed; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); +/** + * @brief Send a attitude_quaternion message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +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); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); -#endif + _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 } #if MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -234,37 +319,29 @@ static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, 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) { #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, q1); - _mav_put_float(buf, 8, q2); - _mav_put_float(buf, 12, q3); - _mav_put_float(buf, 16, q4); - _mav_put_float(buf, 20, rollspeed); - _mav_put_float(buf, 24, pitchspeed); - _mav_put_float(buf, 28, yawspeed); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); -#endif -#else - mavlink_attitude_quaternion_t *packet = (mavlink_attitude_quaternion_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->q1 = q1; - packet->q2 = q2; - packet->q3 = q3; - packet->q4 = q4; - packet->rollspeed = rollspeed; - packet->pitchspeed = pitchspeed; - packet->yawspeed = yawspeed; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, q1); + _mav_put_float(buf, 8, q2); + _mav_put_float(buf, 12, q3); + _mav_put_float(buf, 16, q4); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + + _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 - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); -#endif + mavlink_attitude_quaternion_t *packet = (mavlink_attitude_quaternion_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->q1 = q1; + packet->q2 = q2; + packet->q3 = q3; + packet->q4 = q4; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + + _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 } #endif @@ -277,81 +354,81 @@ static inline void mavlink_msg_attitude_quaternion_send_buf(mavlink_message_t *m /** * @brief Get field time_boot_ms from attitude_quaternion message * - * @return Timestamp (milliseconds since system boot) + * @return Timestamp (milliseconds since system boot) */ static inline uint32_t mavlink_msg_attitude_quaternion_get_time_boot_ms(const mavlink_message_t* msg) { - return _MAV_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** * @brief Get field q1 from attitude_quaternion message * - * @return Quaternion component 1, w (1 in null-rotation) + * @return Quaternion component 1, w (1 in null-rotation) */ static inline float mavlink_msg_attitude_quaternion_get_q1(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** * @brief Get field q2 from attitude_quaternion message * - * @return Quaternion component 2, x (0 in null-rotation) + * @return Quaternion component 2, x (0 in null-rotation) */ static inline float mavlink_msg_attitude_quaternion_get_q2(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** * @brief Get field q3 from attitude_quaternion message * - * @return Quaternion component 3, y (0 in null-rotation) + * @return Quaternion component 3, y (0 in null-rotation) */ static inline float mavlink_msg_attitude_quaternion_get_q3(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** * @brief Get field q4 from attitude_quaternion message * - * @return Quaternion component 4, z (0 in null-rotation) + * @return Quaternion component 4, z (0 in null-rotation) */ static inline float mavlink_msg_attitude_quaternion_get_q4(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** * @brief Get field rollspeed from attitude_quaternion message * - * @return Roll angular speed (rad/s) + * @return Roll angular speed (rad/s) */ static inline float mavlink_msg_attitude_quaternion_get_rollspeed(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** * @brief Get field pitchspeed from attitude_quaternion message * - * @return Pitch angular speed (rad/s) + * @return Pitch angular speed (rad/s) */ static inline float mavlink_msg_attitude_quaternion_get_pitchspeed(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 24); + return _MAV_RETURN_float(msg, 24); } /** * @brief Get field yawspeed from attitude_quaternion message * - * @return Yaw angular speed (rad/s) + * @return Yaw angular speed (rad/s) */ static inline float mavlink_msg_attitude_quaternion_get_yawspeed(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 28); + return _MAV_RETURN_float(msg, 28); } /** @@ -362,16 +439,18 @@ static inline float mavlink_msg_attitude_quaternion_get_yawspeed(const mavlink_m */ static inline void mavlink_msg_attitude_quaternion_decode(const mavlink_message_t* msg, mavlink_attitude_quaternion_t* attitude_quaternion) { -#if MAVLINK_NEED_BYTE_SWAP - attitude_quaternion->time_boot_ms = mavlink_msg_attitude_quaternion_get_time_boot_ms(msg); - attitude_quaternion->q1 = mavlink_msg_attitude_quaternion_get_q1(msg); - attitude_quaternion->q2 = mavlink_msg_attitude_quaternion_get_q2(msg); - attitude_quaternion->q3 = mavlink_msg_attitude_quaternion_get_q3(msg); - attitude_quaternion->q4 = mavlink_msg_attitude_quaternion_get_q4(msg); - 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); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + attitude_quaternion->time_boot_ms = mavlink_msg_attitude_quaternion_get_time_boot_ms(msg); + attitude_quaternion->q1 = mavlink_msg_attitude_quaternion_get_q1(msg); + attitude_quaternion->q2 = mavlink_msg_attitude_quaternion_get_q2(msg); + attitude_quaternion->q3 = mavlink_msg_attitude_quaternion_get_q3(msg); + attitude_quaternion->q4 = mavlink_msg_attitude_quaternion_get_q4(msg); + 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); #else - memcpy(attitude_quaternion, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); + 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); + memcpy(attitude_quaternion, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_diff_pressure.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_diff_pressure.h index f7277c03..f8ef874e 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_diff_pressure.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_diff_pressure.h @@ -1,31 +1,45 @@ +#pragma once // MESSAGE DIFF_PRESSURE PACKING #define MAVLINK_MSG_ID_DIFF_PRESSURE 184 -typedef struct __mavlink_diff_pressure_t -{ - float velocity; /*< Measured Velocity*/ - float diff_pressure; /*< Measured Differential Pressure (Pa)*/ - float temperature; /*< Measured Temperature (K)*/ + +typedef struct __mavlink_diff_pressure_t { + float velocity; /*< Measured Velocity*/ + float diff_pressure; /*< Measured Differential Pressure (Pa)*/ + float temperature; /*< Measured Temperature (K)*/ } mavlink_diff_pressure_t; #define MAVLINK_MSG_ID_DIFF_PRESSURE_LEN 12 +#define MAVLINK_MSG_ID_DIFF_PRESSURE_MIN_LEN 12 #define MAVLINK_MSG_ID_184_LEN 12 +#define MAVLINK_MSG_ID_184_MIN_LEN 12 #define MAVLINK_MSG_ID_DIFF_PRESSURE_CRC 169 #define MAVLINK_MSG_ID_184_CRC 169 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_DIFF_PRESSURE { \ - "DIFF_PRESSURE", \ - 3, \ - { { "velocity", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_diff_pressure_t, velocity) }, \ + 184, \ + "DIFF_PRESSURE", \ + 3, \ + { { "velocity", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_diff_pressure_t, velocity) }, \ { "diff_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_diff_pressure_t, diff_pressure) }, \ { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_diff_pressure_t, temperature) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_DIFF_PRESSURE { \ + "DIFF_PRESSURE", \ + 3, \ + { { "velocity", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_diff_pressure_t, velocity) }, \ + { "diff_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_diff_pressure_t, diff_pressure) }, \ + { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_diff_pressure_t, temperature) }, \ + } \ +} +#endif /** * @brief Pack a diff_pressure message @@ -33,35 +47,70 @@ typedef struct __mavlink_diff_pressure_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param velocity Measured Velocity - * @param diff_pressure Measured Differential Pressure (Pa) - * @param temperature Measured Temperature (K) + * @param velocity Measured Velocity + * @param diff_pressure Measured Differential Pressure (Pa) + * @param temperature Measured Temperature (K) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_diff_pressure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float velocity, float diff_pressure, float temperature) + float velocity, float diff_pressure, float temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DIFF_PRESSURE_LEN]; - _mav_put_float(buf, 0, velocity); - _mav_put_float(buf, 4, diff_pressure); - _mav_put_float(buf, 8, temperature); + char buf[MAVLINK_MSG_ID_DIFF_PRESSURE_LEN]; + _mav_put_float(buf, 0, velocity); + _mav_put_float(buf, 4, diff_pressure); + _mav_put_float(buf, 8, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN); #else - mavlink_diff_pressure_t packet; - packet.velocity = velocity; - packet.diff_pressure = diff_pressure; - packet.temperature = temperature; + mavlink_diff_pressure_t packet; + packet.velocity = velocity; + packet.diff_pressure = diff_pressure; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_DIFF_PRESSURE; + msg->msgid = MAVLINK_MSG_ID_DIFF_PRESSURE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIFF_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_CRC); +} + +/** + * @brief Pack a diff_pressure message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param velocity Measured Velocity + * @param diff_pressure Measured Differential Pressure (Pa) + * @param temperature Measured Temperature (K) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_diff_pressure_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + float velocity, float diff_pressure, float temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DIFF_PRESSURE_LEN]; + _mav_put_float(buf, 0, velocity); + _mav_put_float(buf, 4, diff_pressure); + _mav_put_float(buf, 8, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN); +#else + mavlink_diff_pressure_t packet; + packet.velocity = velocity; + packet.diff_pressure = diff_pressure; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DIFF_PRESSURE; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_DIFF_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_DIFF_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN); #endif } @@ -71,37 +120,33 @@ static inline uint16_t mavlink_msg_diff_pressure_pack(uint8_t system_id, uint8_t * @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 velocity Measured Velocity - * @param diff_pressure Measured Differential Pressure (Pa) - * @param temperature Measured Temperature (K) + * @param velocity Measured Velocity + * @param diff_pressure Measured Differential Pressure (Pa) + * @param temperature Measured Temperature (K) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_diff_pressure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float velocity,float diff_pressure,float temperature) + mavlink_message_t* msg, + float velocity,float diff_pressure,float temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DIFF_PRESSURE_LEN]; - _mav_put_float(buf, 0, velocity); - _mav_put_float(buf, 4, diff_pressure); - _mav_put_float(buf, 8, temperature); + char buf[MAVLINK_MSG_ID_DIFF_PRESSURE_LEN]; + _mav_put_float(buf, 0, velocity); + _mav_put_float(buf, 4, diff_pressure); + _mav_put_float(buf, 8, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN); #else - mavlink_diff_pressure_t packet; - packet.velocity = velocity; - packet.diff_pressure = diff_pressure; - packet.temperature = temperature; + mavlink_diff_pressure_t packet; + packet.velocity = velocity; + packet.diff_pressure = diff_pressure; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_DIFF_PRESSURE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_DIFF_PRESSURE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIFF_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_CRC); } /** @@ -114,7 +159,7 @@ static inline uint16_t mavlink_msg_diff_pressure_pack_chan(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_diff_pressure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_diff_pressure_t* diff_pressure) { - return mavlink_msg_diff_pressure_pack(system_id, component_id, msg, diff_pressure->velocity, diff_pressure->diff_pressure, diff_pressure->temperature); + return mavlink_msg_diff_pressure_pack(system_id, component_id, msg, diff_pressure->velocity, diff_pressure->diff_pressure, diff_pressure->temperature); } /** @@ -128,49 +173,69 @@ static inline uint16_t mavlink_msg_diff_pressure_encode(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_diff_pressure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_diff_pressure_t* diff_pressure) { - return mavlink_msg_diff_pressure_pack_chan(system_id, component_id, chan, msg, diff_pressure->velocity, diff_pressure->diff_pressure, diff_pressure->temperature); + return mavlink_msg_diff_pressure_pack_chan(system_id, component_id, chan, msg, diff_pressure->velocity, diff_pressure->diff_pressure, diff_pressure->temperature); +} + +/** + * @brief Encode a diff_pressure struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param diff_pressure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_diff_pressure_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_diff_pressure_t* diff_pressure) +{ + return mavlink_msg_diff_pressure_pack_status(system_id, component_id, _status, msg, diff_pressure->velocity, diff_pressure->diff_pressure, diff_pressure->temperature); } /** * @brief Send a diff_pressure message * @param chan MAVLink channel to send the message * - * @param velocity Measured Velocity - * @param diff_pressure Measured Differential Pressure (Pa) - * @param temperature Measured Temperature (K) + * @param velocity Measured Velocity + * @param diff_pressure Measured Differential Pressure (Pa) + * @param temperature Measured Temperature (K) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_diff_pressure_send(mavlink_channel_t chan, float velocity, float diff_pressure, float temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_DIFF_PRESSURE_LEN]; - _mav_put_float(buf, 0, velocity); - _mav_put_float(buf, 4, diff_pressure); - _mav_put_float(buf, 8, temperature); + char buf[MAVLINK_MSG_ID_DIFF_PRESSURE_LEN]; + _mav_put_float(buf, 0, velocity); + _mav_put_float(buf, 4, diff_pressure); + _mav_put_float(buf, 8, temperature); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIFF_PRESSURE, buf, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_CRC); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIFF_PRESSURE, buf, MAVLINK_MSG_ID_DIFF_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIFF_PRESSURE, buf, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN); + mavlink_diff_pressure_t packet; + packet.velocity = velocity; + packet.diff_pressure = diff_pressure; + packet.temperature = temperature; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIFF_PRESSURE, (const char *)&packet, MAVLINK_MSG_ID_DIFF_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_CRC); #endif -#else - mavlink_diff_pressure_t packet; - packet.velocity = velocity; - packet.diff_pressure = diff_pressure; - packet.temperature = temperature; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIFF_PRESSURE, (const char *)&packet, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_CRC); +/** + * @brief Send a diff_pressure message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_diff_pressure_send_struct(mavlink_channel_t chan, const mavlink_diff_pressure_t* diff_pressure) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_diff_pressure_send(chan, diff_pressure->velocity, diff_pressure->diff_pressure, diff_pressure->temperature); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIFF_PRESSURE, (const char *)&packet, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIFF_PRESSURE, (const char *)diff_pressure, MAVLINK_MSG_ID_DIFF_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_CRC); #endif } #if MAVLINK_MSG_ID_DIFF_PRESSURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -179,27 +244,19 @@ static inline void mavlink_msg_diff_pressure_send(mavlink_channel_t chan, float static inline void mavlink_msg_diff_pressure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float velocity, float diff_pressure, float temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, velocity); - _mav_put_float(buf, 4, diff_pressure); - _mav_put_float(buf, 8, temperature); + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, velocity); + _mav_put_float(buf, 4, diff_pressure); + _mav_put_float(buf, 8, temperature); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIFF_PRESSURE, buf, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIFF_PRESSURE, buf, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIFF_PRESSURE, buf, MAVLINK_MSG_ID_DIFF_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_CRC); #else - mavlink_diff_pressure_t *packet = (mavlink_diff_pressure_t *)msgbuf; - packet->velocity = velocity; - packet->diff_pressure = diff_pressure; - packet->temperature = temperature; + mavlink_diff_pressure_t *packet = (mavlink_diff_pressure_t *)msgbuf; + packet->velocity = velocity; + packet->diff_pressure = diff_pressure; + packet->temperature = temperature; -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIFF_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIFF_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIFF_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_DIFF_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN, MAVLINK_MSG_ID_DIFF_PRESSURE_CRC); #endif } #endif @@ -212,31 +269,31 @@ static inline void mavlink_msg_diff_pressure_send_buf(mavlink_message_t *msgbuf, /** * @brief Get field velocity from diff_pressure message * - * @return Measured Velocity + * @return Measured Velocity */ static inline float mavlink_msg_diff_pressure_get_velocity(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** * @brief Get field diff_pressure from diff_pressure message * - * @return Measured Differential Pressure (Pa) + * @return Measured Differential Pressure (Pa) */ static inline float mavlink_msg_diff_pressure_get_diff_pressure(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** * @brief Get field temperature from diff_pressure message * - * @return Measured Temperature (K) + * @return Measured Temperature (K) */ static inline float mavlink_msg_diff_pressure_get_temperature(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -247,11 +304,13 @@ static inline float mavlink_msg_diff_pressure_get_temperature(const mavlink_mess */ static inline void mavlink_msg_diff_pressure_decode(const mavlink_message_t* msg, mavlink_diff_pressure_t* diff_pressure) { -#if MAVLINK_NEED_BYTE_SWAP - diff_pressure->velocity = mavlink_msg_diff_pressure_get_velocity(msg); - diff_pressure->diff_pressure = mavlink_msg_diff_pressure_get_diff_pressure(msg); - diff_pressure->temperature = mavlink_msg_diff_pressure_get_temperature(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + diff_pressure->velocity = mavlink_msg_diff_pressure_get_velocity(msg); + diff_pressure->diff_pressure = mavlink_msg_diff_pressure_get_diff_pressure(msg); + diff_pressure->temperature = mavlink_msg_diff_pressure_get_temperature(msg); #else - memcpy(diff_pressure, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DIFF_PRESSURE_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_DIFF_PRESSURE_LEN? msg->len : MAVLINK_MSG_ID_DIFF_PRESSURE_LEN; + memset(diff_pressure, 0, MAVLINK_MSG_ID_DIFF_PRESSURE_LEN); + memcpy(diff_pressure, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_external_attitude.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_external_attitude.h index 435ae40d..5e171e43 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_external_attitude.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_external_attitude.h @@ -1,33 +1,48 @@ +#pragma once // MESSAGE EXTERNAL_ATTITUDE PACKING #define MAVLINK_MSG_ID_EXTERNAL_ATTITUDE 195 -typedef struct __mavlink_external_attitude_t -{ - float qw; /*< Quaternion scalar value*/ - float qx; /*< Quaternion x value*/ - float qy; /*< Quaternion y value*/ - float qz; /*< Quaternion z value*/ + +typedef struct __mavlink_external_attitude_t { + float qw; /*< Quaternion scalar value*/ + float qx; /*< Quaternion x value*/ + float qy; /*< Quaternion y value*/ + float qz; /*< Quaternion z value*/ } mavlink_external_attitude_t; #define MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN 16 +#define MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_MIN_LEN 16 #define MAVLINK_MSG_ID_195_LEN 16 +#define MAVLINK_MSG_ID_195_MIN_LEN 16 #define MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_CRC 65 #define MAVLINK_MSG_ID_195_CRC 65 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_EXTERNAL_ATTITUDE { \ - "EXTERNAL_ATTITUDE", \ - 4, \ - { { "qw", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_external_attitude_t, qw) }, \ + 195, \ + "EXTERNAL_ATTITUDE", \ + 4, \ + { { "qw", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_external_attitude_t, qw) }, \ { "qx", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_external_attitude_t, qx) }, \ { "qy", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_external_attitude_t, qy) }, \ { "qz", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_external_attitude_t, qz) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_EXTERNAL_ATTITUDE { \ + "EXTERNAL_ATTITUDE", \ + 4, \ + { { "qw", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_external_attitude_t, qw) }, \ + { "qx", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_external_attitude_t, qx) }, \ + { "qy", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_external_attitude_t, qy) }, \ + { "qz", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_external_attitude_t, qz) }, \ + } \ +} +#endif /** * @brief Pack a external_attitude message @@ -35,38 +50,76 @@ typedef struct __mavlink_external_attitude_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param qw Quaternion scalar value - * @param qx Quaternion x value - * @param qy Quaternion y value - * @param qz Quaternion z value + * @param qw Quaternion scalar value + * @param qx Quaternion x value + * @param qy Quaternion y value + * @param qz Quaternion z value * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_external_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float qw, float qx, float qy, float qz) + float qw, float qx, float qy, float qz) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN]; - _mav_put_float(buf, 0, qw); - _mav_put_float(buf, 4, qx); - _mav_put_float(buf, 8, qy); - _mav_put_float(buf, 12, qz); + char buf[MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN]; + _mav_put_float(buf, 0, qw); + _mav_put_float(buf, 4, qx); + _mav_put_float(buf, 8, qy); + _mav_put_float(buf, 12, qz); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN); #else - mavlink_external_attitude_t packet; - packet.qw = qw; - packet.qx = qx; - packet.qy = qy; - packet.qz = qz; + mavlink_external_attitude_t packet; + packet.qw = qw; + packet.qx = qx; + packet.qy = qy; + packet.qz = qz; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_EXTERNAL_ATTITUDE; + msg->msgid = MAVLINK_MSG_ID_EXTERNAL_ATTITUDE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_CRC); +} + +/** + * @brief Pack a external_attitude message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param qw Quaternion scalar value + * @param qx Quaternion x value + * @param qy Quaternion y value + * @param qz Quaternion z value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_external_attitude_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + float qw, float qx, float qy, float qz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN]; + _mav_put_float(buf, 0, qw); + _mav_put_float(buf, 4, qx); + _mav_put_float(buf, 8, qy); + _mav_put_float(buf, 12, qz); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN); +#else + mavlink_external_attitude_t packet; + packet.qw = qw; + packet.qx = qx; + packet.qy = qy; + packet.qz = qz; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_EXTERNAL_ATTITUDE; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN); #endif } @@ -76,40 +129,36 @@ static inline uint16_t mavlink_msg_external_attitude_pack(uint8_t system_id, uin * @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 qw Quaternion scalar value - * @param qx Quaternion x value - * @param qy Quaternion y value - * @param qz Quaternion z value + * @param qw Quaternion scalar value + * @param qx Quaternion x value + * @param qy Quaternion y value + * @param qz Quaternion z value * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_external_attitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float qw,float qx,float qy,float qz) + mavlink_message_t* msg, + float qw,float qx,float qy,float qz) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN]; - _mav_put_float(buf, 0, qw); - _mav_put_float(buf, 4, qx); - _mav_put_float(buf, 8, qy); - _mav_put_float(buf, 12, qz); + char buf[MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN]; + _mav_put_float(buf, 0, qw); + _mav_put_float(buf, 4, qx); + _mav_put_float(buf, 8, qy); + _mav_put_float(buf, 12, qz); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN); #else - mavlink_external_attitude_t packet; - packet.qw = qw; - packet.qx = qx; - packet.qy = qy; - packet.qz = qz; + mavlink_external_attitude_t packet; + packet.qw = qw; + packet.qx = qx; + packet.qy = qy; + packet.qz = qz; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_EXTERNAL_ATTITUDE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_EXTERNAL_ATTITUDE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_CRC); } /** @@ -122,7 +171,7 @@ static inline uint16_t mavlink_msg_external_attitude_pack_chan(uint8_t system_id */ static inline uint16_t mavlink_msg_external_attitude_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_external_attitude_t* external_attitude) { - return mavlink_msg_external_attitude_pack(system_id, component_id, msg, external_attitude->qw, external_attitude->qx, external_attitude->qy, external_attitude->qz); + return mavlink_msg_external_attitude_pack(system_id, component_id, msg, external_attitude->qw, external_attitude->qx, external_attitude->qy, external_attitude->qz); } /** @@ -136,52 +185,72 @@ static inline uint16_t mavlink_msg_external_attitude_encode(uint8_t system_id, u */ static inline uint16_t mavlink_msg_external_attitude_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_external_attitude_t* external_attitude) { - return mavlink_msg_external_attitude_pack_chan(system_id, component_id, chan, msg, external_attitude->qw, external_attitude->qx, external_attitude->qy, external_attitude->qz); + return mavlink_msg_external_attitude_pack_chan(system_id, component_id, chan, msg, external_attitude->qw, external_attitude->qx, external_attitude->qy, external_attitude->qz); +} + +/** + * @brief Encode a external_attitude struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param external_attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_external_attitude_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_external_attitude_t* external_attitude) +{ + return mavlink_msg_external_attitude_pack_status(system_id, component_id, _status, msg, external_attitude->qw, external_attitude->qx, external_attitude->qy, external_attitude->qz); } /** * @brief Send a external_attitude message * @param chan MAVLink channel to send the message * - * @param qw Quaternion scalar value - * @param qx Quaternion x value - * @param qy Quaternion y value - * @param qz Quaternion z value + * @param qw Quaternion scalar value + * @param qx Quaternion x value + * @param qy Quaternion y value + * @param qz Quaternion z value */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_external_attitude_send(mavlink_channel_t chan, float qw, float qx, float qy, float qz) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN]; - _mav_put_float(buf, 0, qw); - _mav_put_float(buf, 4, qx); - _mav_put_float(buf, 8, qy); - _mav_put_float(buf, 12, qz); + char buf[MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN]; + _mav_put_float(buf, 0, qw); + _mav_put_float(buf, 4, qx); + _mav_put_float(buf, 8, qy); + _mav_put_float(buf, 12, qz); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE, buf, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_CRC); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE, buf, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE, buf, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN); + mavlink_external_attitude_t packet; + packet.qw = qw; + packet.qx = qx; + packet.qy = qy; + packet.qz = qz; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_CRC); #endif -#else - mavlink_external_attitude_t packet; - packet.qw = qw; - packet.qx = qx; - packet.qy = qy; - packet.qz = qz; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_CRC); +/** + * @brief Send a external_attitude message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_external_attitude_send_struct(mavlink_channel_t chan, const mavlink_external_attitude_t* external_attitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_external_attitude_send(chan, external_attitude->qw, external_attitude->qx, external_attitude->qy, external_attitude->qz); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE, (const char *)external_attitude, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_CRC); #endif } #if MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -190,29 +259,21 @@ static inline void mavlink_msg_external_attitude_send(mavlink_channel_t chan, fl static inline void mavlink_msg_external_attitude_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float qw, float qx, float qy, float qz) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, qw); - _mav_put_float(buf, 4, qx); - _mav_put_float(buf, 8, qy); - _mav_put_float(buf, 12, qz); + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, qw); + _mav_put_float(buf, 4, qx); + _mav_put_float(buf, 8, qy); + _mav_put_float(buf, 12, qz); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE, buf, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE, buf, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE, buf, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_CRC); #else - mavlink_external_attitude_t *packet = (mavlink_external_attitude_t *)msgbuf; - packet->qw = qw; - packet->qx = qx; - packet->qy = qy; - packet->qz = qz; + mavlink_external_attitude_t *packet = (mavlink_external_attitude_t *)msgbuf; + packet->qw = qw; + packet->qx = qx; + packet->qy = qy; + packet->qz = qz; -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_CRC); #endif } #endif @@ -225,41 +286,41 @@ static inline void mavlink_msg_external_attitude_send_buf(mavlink_message_t *msg /** * @brief Get field qw from external_attitude message * - * @return Quaternion scalar value + * @return Quaternion scalar value */ static inline float mavlink_msg_external_attitude_get_qw(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** * @brief Get field qx from external_attitude message * - * @return Quaternion x value + * @return Quaternion x value */ static inline float mavlink_msg_external_attitude_get_qx(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** * @brief Get field qy from external_attitude message * - * @return Quaternion y value + * @return Quaternion y value */ static inline float mavlink_msg_external_attitude_get_qy(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** * @brief Get field qz from external_attitude message * - * @return Quaternion z value + * @return Quaternion z value */ static inline float mavlink_msg_external_attitude_get_qz(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -270,12 +331,14 @@ static inline float mavlink_msg_external_attitude_get_qz(const mavlink_message_t */ static inline void mavlink_msg_external_attitude_decode(const mavlink_message_t* msg, mavlink_external_attitude_t* external_attitude) { -#if MAVLINK_NEED_BYTE_SWAP - external_attitude->qw = mavlink_msg_external_attitude_get_qw(msg); - external_attitude->qx = mavlink_msg_external_attitude_get_qx(msg); - external_attitude->qy = mavlink_msg_external_attitude_get_qy(msg); - external_attitude->qz = mavlink_msg_external_attitude_get_qz(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + external_attitude->qw = mavlink_msg_external_attitude_get_qw(msg); + external_attitude->qx = mavlink_msg_external_attitude_get_qx(msg); + external_attitude->qy = mavlink_msg_external_attitude_get_qy(msg); + external_attitude->qz = mavlink_msg_external_attitude_get_qz(msg); #else - memcpy(external_attitude, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN? msg->len : MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN; + memset(external_attitude, 0, MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_LEN); + memcpy(external_attitude, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_heartbeat.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_heartbeat.h index 1efa218e..82ec13cc 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_heartbeat.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_heartbeat.h @@ -1,37 +1,54 @@ +#pragma once // MESSAGE HEARTBEAT PACKING #define MAVLINK_MSG_ID_HEARTBEAT 0 -typedef struct __mavlink_heartbeat_t -{ - uint32_t custom_mode; /*< A bitfield for use for autopilot-specific flags.*/ - uint8_t type; /*< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)*/ - uint8_t autopilot; /*< Autopilot type / class. defined in MAV_AUTOPILOT ENUM*/ - uint8_t base_mode; /*< System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h*/ - uint8_t system_status; /*< System status flag, see MAV_STATE ENUM*/ - uint8_t mavlink_version; /*< MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version*/ + +typedef struct __mavlink_heartbeat_t { + uint32_t custom_mode; /*< A bitfield for use for autopilot-specific flags.*/ + uint8_t type; /*< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)*/ + uint8_t autopilot; /*< Autopilot type / class. defined in MAV_AUTOPILOT ENUM*/ + uint8_t base_mode; /*< System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h*/ + uint8_t system_status; /*< System status flag, see MAV_STATE ENUM*/ + uint8_t mavlink_version; /*< MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version*/ } mavlink_heartbeat_t; #define MAVLINK_MSG_ID_HEARTBEAT_LEN 9 +#define MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN 9 #define MAVLINK_MSG_ID_0_LEN 9 +#define MAVLINK_MSG_ID_0_MIN_LEN 9 #define MAVLINK_MSG_ID_HEARTBEAT_CRC 50 #define MAVLINK_MSG_ID_0_CRC 50 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_HEARTBEAT { \ - "HEARTBEAT", \ - 6, \ - { { "custom_mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_heartbeat_t, custom_mode) }, \ - { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_heartbeat_t, type) }, \ + 0, \ + "HEARTBEAT", \ + 6, \ + { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_heartbeat_t, type) }, \ { "autopilot", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_heartbeat_t, autopilot) }, \ { "base_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_heartbeat_t, base_mode) }, \ + { "custom_mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_heartbeat_t, custom_mode) }, \ { "system_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_heartbeat_t, system_status) }, \ { "mavlink_version", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_heartbeat_t, mavlink_version) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_HEARTBEAT { \ + "HEARTBEAT", \ + 6, \ + { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_heartbeat_t, type) }, \ + { "autopilot", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_heartbeat_t, autopilot) }, \ + { "base_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_heartbeat_t, base_mode) }, \ + { "custom_mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_heartbeat_t, custom_mode) }, \ + { "system_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_heartbeat_t, system_status) }, \ + { "mavlink_version", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_heartbeat_t, mavlink_version) }, \ + } \ +} +#endif /** * @brief Pack a heartbeat message @@ -39,43 +56,86 @@ typedef struct __mavlink_heartbeat_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM - * @param base_mode System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h - * @param custom_mode A bitfield for use for autopilot-specific flags. - * @param system_status System status flag, see MAV_STATE ENUM + * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM + * @param base_mode System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h + * @param custom_mode A bitfield for use for autopilot-specific flags. + * @param system_status System status flag, see MAV_STATE ENUM * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) + uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN]; - _mav_put_uint32_t(buf, 0, custom_mode); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 5, autopilot); - _mav_put_uint8_t(buf, 6, base_mode); - _mav_put_uint8_t(buf, 7, system_status); - _mav_put_uint8_t(buf, 8, 2); + char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 5, autopilot); + _mav_put_uint8_t(buf, 6, base_mode); + _mav_put_uint8_t(buf, 7, system_status); + _mav_put_uint8_t(buf, 8, 2); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); #else - mavlink_heartbeat_t packet; - packet.custom_mode = custom_mode; - packet.type = type; - packet.autopilot = autopilot; - packet.base_mode = base_mode; - packet.system_status = system_status; - packet.mavlink_version = 2; + mavlink_heartbeat_t packet; + packet.custom_mode = custom_mode; + packet.type = type; + packet.autopilot = autopilot; + packet.base_mode = base_mode; + packet.system_status = system_status; + packet.mavlink_version = 2; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); +} + +/** + * @brief Pack a heartbeat message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM + * @param base_mode System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h + * @param custom_mode A bitfield for use for autopilot-specific flags. + * @param system_status System status flag, see MAV_STATE ENUM + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_heartbeat_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 5, autopilot); + _mav_put_uint8_t(buf, 6, base_mode); + _mav_put_uint8_t(buf, 7, system_status); + _mav_put_uint8_t(buf, 8, 2); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#else + mavlink_heartbeat_t packet; + packet.custom_mode = custom_mode; + packet.type = type; + packet.autopilot = autopilot; + packet.base_mode = base_mode; + packet.system_status = system_status; + packet.mavlink_version = 2; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN); #endif } @@ -85,45 +145,41 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com * @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 type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM - * @param base_mode System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h - * @param custom_mode A bitfield for use for autopilot-specific flags. - * @param system_status System status flag, see MAV_STATE ENUM + * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM + * @param base_mode System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h + * @param custom_mode A bitfield for use for autopilot-specific flags. + * @param system_status System status flag, see MAV_STATE ENUM * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,uint8_t autopilot,uint8_t base_mode,uint32_t custom_mode,uint8_t system_status) + mavlink_message_t* msg, + uint8_t type,uint8_t autopilot,uint8_t base_mode,uint32_t custom_mode,uint8_t system_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN]; - _mav_put_uint32_t(buf, 0, custom_mode); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 5, autopilot); - _mav_put_uint8_t(buf, 6, base_mode); - _mav_put_uint8_t(buf, 7, system_status); - _mav_put_uint8_t(buf, 8, 2); + char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 5, autopilot); + _mav_put_uint8_t(buf, 6, base_mode); + _mav_put_uint8_t(buf, 7, system_status); + _mav_put_uint8_t(buf, 8, 2); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); #else - mavlink_heartbeat_t packet; - packet.custom_mode = custom_mode; - packet.type = type; - packet.autopilot = autopilot; - packet.base_mode = base_mode; - packet.system_status = system_status; - packet.mavlink_version = 2; + mavlink_heartbeat_t packet; + packet.custom_mode = custom_mode; + packet.type = type; + packet.autopilot = autopilot; + packet.base_mode = base_mode; + packet.system_status = system_status; + packet.mavlink_version = 2; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); } /** @@ -136,7 +192,7 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat) { - return mavlink_msg_heartbeat_pack(system_id, component_id, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status); + return mavlink_msg_heartbeat_pack(system_id, component_id, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status); } /** @@ -150,57 +206,77 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c */ static inline uint16_t mavlink_msg_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat) { - return mavlink_msg_heartbeat_pack_chan(system_id, component_id, chan, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status); + return mavlink_msg_heartbeat_pack_chan(system_id, component_id, chan, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status); +} + +/** + * @brief Encode a heartbeat struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param heartbeat C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_heartbeat_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat) +{ + return mavlink_msg_heartbeat_pack_status(system_id, component_id, _status, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status); } /** * @brief Send a heartbeat message * @param chan MAVLink channel to send the message * - * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM - * @param base_mode System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h - * @param custom_mode A bitfield for use for autopilot-specific flags. - * @param system_status System status flag, see MAV_STATE ENUM + * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM + * @param base_mode System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h + * @param custom_mode A bitfield for use for autopilot-specific flags. + * @param system_status System status flag, see MAV_STATE ENUM */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN]; - _mav_put_uint32_t(buf, 0, custom_mode); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 5, autopilot); - _mav_put_uint8_t(buf, 6, base_mode); - _mav_put_uint8_t(buf, 7, system_status); - _mav_put_uint8_t(buf, 8, 2); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); + char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 5, autopilot); + _mav_put_uint8_t(buf, 6, base_mode); + _mav_put_uint8_t(buf, 7, system_status); + _mav_put_uint8_t(buf, 8, 2); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); + mavlink_heartbeat_t packet; + packet.custom_mode = custom_mode; + packet.type = type; + packet.autopilot = autopilot; + packet.base_mode = base_mode; + packet.system_status = system_status; + packet.mavlink_version = 2; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); #endif -#else - mavlink_heartbeat_t packet; - packet.custom_mode = custom_mode; - packet.type = type; - packet.autopilot = autopilot; - packet.base_mode = base_mode; - packet.system_status = system_status; - packet.mavlink_version = 2; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); +/** + * @brief Send a heartbeat message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_heartbeat_send_struct(mavlink_channel_t chan, const mavlink_heartbeat_t* heartbeat) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_heartbeat_send(chan, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)heartbeat, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); #endif } #if MAVLINK_MSG_ID_HEARTBEAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -209,33 +285,25 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty static inline void mavlink_msg_heartbeat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, custom_mode); - _mav_put_uint8_t(buf, 4, type); - _mav_put_uint8_t(buf, 5, autopilot); - _mav_put_uint8_t(buf, 6, base_mode); - _mav_put_uint8_t(buf, 7, system_status); - _mav_put_uint8_t(buf, 8, 2); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#endif -#else - mavlink_heartbeat_t *packet = (mavlink_heartbeat_t *)msgbuf; - packet->custom_mode = custom_mode; - packet->type = type; - packet->autopilot = autopilot; - packet->base_mode = base_mode; - packet->system_status = system_status; - packet->mavlink_version = 2; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 5, autopilot); + _mav_put_uint8_t(buf, 6, base_mode); + _mav_put_uint8_t(buf, 7, system_status); + _mav_put_uint8_t(buf, 8, 2); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); -#endif + mavlink_heartbeat_t *packet = (mavlink_heartbeat_t *)msgbuf; + packet->custom_mode = custom_mode; + packet->type = type; + packet->autopilot = autopilot; + packet->base_mode = base_mode; + packet->system_status = system_status; + packet->mavlink_version = 2; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); #endif } #endif @@ -248,61 +316,61 @@ static inline void mavlink_msg_heartbeat_send_buf(mavlink_message_t *msgbuf, mav /** * @brief Get field type from heartbeat message * - * @return Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + * @return Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) */ static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 4); + return _MAV_RETURN_uint8_t(msg, 4); } /** * @brief Get field autopilot from heartbeat message * - * @return Autopilot type / class. defined in MAV_AUTOPILOT ENUM + * @return Autopilot type / class. defined in MAV_AUTOPILOT ENUM */ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 5); + return _MAV_RETURN_uint8_t(msg, 5); } /** * @brief Get field base_mode from heartbeat message * - * @return System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h + * @return System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h */ static inline uint8_t mavlink_msg_heartbeat_get_base_mode(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 6); + return _MAV_RETURN_uint8_t(msg, 6); } /** * @brief Get field custom_mode from heartbeat message * - * @return A bitfield for use for autopilot-specific flags. + * @return A bitfield for use for autopilot-specific flags. */ static inline uint32_t mavlink_msg_heartbeat_get_custom_mode(const mavlink_message_t* msg) { - return _MAV_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** * @brief Get field system_status from heartbeat message * - * @return System status flag, see MAV_STATE ENUM + * @return System status flag, see MAV_STATE ENUM */ static inline uint8_t mavlink_msg_heartbeat_get_system_status(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 7); + return _MAV_RETURN_uint8_t(msg, 7); } /** * @brief Get field mavlink_version from heartbeat message * - * @return MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version + * @return MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version */ static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 8); + return _MAV_RETURN_uint8_t(msg, 8); } /** @@ -313,14 +381,16 @@ static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_me */ static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, mavlink_heartbeat_t* heartbeat) { -#if MAVLINK_NEED_BYTE_SWAP - heartbeat->custom_mode = mavlink_msg_heartbeat_get_custom_mode(msg); - heartbeat->type = mavlink_msg_heartbeat_get_type(msg); - heartbeat->autopilot = mavlink_msg_heartbeat_get_autopilot(msg); - heartbeat->base_mode = mavlink_msg_heartbeat_get_base_mode(msg); - heartbeat->system_status = mavlink_msg_heartbeat_get_system_status(msg); - heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + heartbeat->custom_mode = mavlink_msg_heartbeat_get_custom_mode(msg); + heartbeat->type = mavlink_msg_heartbeat_get_type(msg); + heartbeat->autopilot = mavlink_msg_heartbeat_get_autopilot(msg); + heartbeat->base_mode = mavlink_msg_heartbeat_get_base_mode(msg); + heartbeat->system_status = mavlink_msg_heartbeat_get_system_status(msg); + heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg); #else - memcpy(heartbeat, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HEARTBEAT_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_HEARTBEAT_LEN? msg->len : MAVLINK_MSG_ID_HEARTBEAT_LEN; + memset(heartbeat, 0, MAVLINK_MSG_ID_HEARTBEAT_LEN); + memcpy(heartbeat, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_offboard_control.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_offboard_control.h index 1076307e..38bc9325 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_offboard_control.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_offboard_control.h @@ -1,41 +1,60 @@ +#pragma once // MESSAGE OFFBOARD_CONTROL PACKING #define MAVLINK_MSG_ID_OFFBOARD_CONTROL 180 -typedef struct __mavlink_offboard_control_t -{ - float Qx; /*< Qx control channel interpreted according to mode*/ - float Qy; /*< Qy control channel, interpreted according to mode*/ - float Qz; /*< Qz control channel, interpreted according to mode*/ - float Fx; /*< Fx control channel interpreted according to mode*/ - float Fy; /*< Fy control channel, interpreted according to mode*/ - float Fz; /*< Fz control channel, interpreted according to mode*/ - uint8_t mode; /*< Offboard control mode, see OFFBOARD_CONTROL_MODE*/ - uint8_t ignore; /*< Bitfield specifying which fields should be ignored, see OFFBOARD_CONTROL_IGNORE*/ + +typedef struct __mavlink_offboard_control_t { + float Qx; /*< Qx control channel interpreted according to mode*/ + float Qy; /*< Qy control channel, interpreted according to mode*/ + float Qz; /*< Qz control channel, interpreted according to mode*/ + float Fx; /*< Fx control channel interpreted according to mode*/ + float Fy; /*< Fy control channel, interpreted according to mode*/ + float Fz; /*< Fz control channel, interpreted according to mode*/ + uint8_t mode; /*< Offboard control mode, see OFFBOARD_CONTROL_MODE*/ + uint8_t ignore; /*< Bitfield specifying which fields should be ignored, see OFFBOARD_CONTROL_IGNORE*/ } mavlink_offboard_control_t; #define MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN 26 +#define MAVLINK_MSG_ID_OFFBOARD_CONTROL_MIN_LEN 26 #define MAVLINK_MSG_ID_180_LEN 26 +#define MAVLINK_MSG_ID_180_MIN_LEN 26 #define MAVLINK_MSG_ID_OFFBOARD_CONTROL_CRC 190 #define MAVLINK_MSG_ID_180_CRC 190 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_OFFBOARD_CONTROL { \ - "OFFBOARD_CONTROL", \ - 8, \ - { { "Qx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_offboard_control_t, Qx) }, \ + 180, \ + "OFFBOARD_CONTROL", \ + 8, \ + { { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_offboard_control_t, mode) }, \ + { "ignore", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_offboard_control_t, ignore) }, \ + { "Qx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_offboard_control_t, Qx) }, \ { "Qy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_offboard_control_t, Qy) }, \ { "Qz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_offboard_control_t, Qz) }, \ { "Fx", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_offboard_control_t, Fx) }, \ { "Fy", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_offboard_control_t, Fy) }, \ { "Fz", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_offboard_control_t, Fz) }, \ - { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_offboard_control_t, mode) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OFFBOARD_CONTROL { \ + "OFFBOARD_CONTROL", \ + 8, \ + { { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_offboard_control_t, mode) }, \ { "ignore", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_offboard_control_t, ignore) }, \ + { "Qx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_offboard_control_t, Qx) }, \ + { "Qy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_offboard_control_t, Qy) }, \ + { "Qz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_offboard_control_t, Qz) }, \ + { "Fx", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_offboard_control_t, Fx) }, \ + { "Fy", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_offboard_control_t, Fy) }, \ + { "Fz", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_offboard_control_t, Fz) }, \ } \ } - +#endif /** * @brief Pack a offboard_control message @@ -43,50 +62,100 @@ typedef struct __mavlink_offboard_control_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param mode Offboard control mode, see OFFBOARD_CONTROL_MODE - * @param ignore Bitfield specifying which fields should be ignored, see OFFBOARD_CONTROL_IGNORE - * @param Qx Qx control channel interpreted according to mode - * @param Qy Qy control channel, interpreted according to mode - * @param Qz Qz control channel, interpreted according to mode - * @param Fx Fx control channel interpreted according to mode - * @param Fy Fy control channel, interpreted according to mode - * @param Fz Fz control channel, interpreted according to mode + * @param mode Offboard control mode, see OFFBOARD_CONTROL_MODE + * @param ignore Bitfield specifying which fields should be ignored, see OFFBOARD_CONTROL_IGNORE + * @param Qx Qx control channel interpreted according to mode + * @param Qy Qy control channel, interpreted according to mode + * @param Qz Qz control channel, interpreted according to mode + * @param Fx Fx control channel interpreted according to mode + * @param Fy Fy control channel, interpreted according to mode + * @param Fz Fz control channel, interpreted according to mode * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_offboard_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t mode, uint8_t ignore, float Qx, float Qy, float Qz, float Fx, float Fy, float Fz) + uint8_t mode, uint8_t ignore, float Qx, float Qy, float Qz, float Fx, float Fy, float Fz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN]; + _mav_put_float(buf, 0, Qx); + _mav_put_float(buf, 4, Qy); + _mav_put_float(buf, 8, Qz); + _mav_put_float(buf, 12, Fx); + _mav_put_float(buf, 16, Fy); + _mav_put_float(buf, 20, Fz); + _mav_put_uint8_t(buf, 24, mode); + _mav_put_uint8_t(buf, 25, ignore); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN); +#else + mavlink_offboard_control_t packet; + packet.Qx = Qx; + packet.Qy = Qy; + packet.Qz = Qz; + packet.Fx = Fx; + packet.Fy = Fy; + packet.Fz = Fz; + packet.mode = mode; + packet.ignore = ignore; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OFFBOARD_CONTROL; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OFFBOARD_CONTROL_MIN_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_CRC); +} + +/** + * @brief Pack a offboard_control message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param mode Offboard control mode, see OFFBOARD_CONTROL_MODE + * @param ignore Bitfield specifying which fields should be ignored, see OFFBOARD_CONTROL_IGNORE + * @param Qx Qx control channel interpreted according to mode + * @param Qy Qy control channel, interpreted according to mode + * @param Qz Qz control channel, interpreted according to mode + * @param Fx Fx control channel interpreted according to mode + * @param Fy Fy control channel, interpreted according to mode + * @param Fz Fz control channel, interpreted according to mode + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_offboard_control_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t mode, uint8_t ignore, float Qx, float Qy, float Qz, float Fx, float Fy, float Fz) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN]; - _mav_put_float(buf, 0, Qx); - _mav_put_float(buf, 4, Qy); - _mav_put_float(buf, 8, Qz); - _mav_put_float(buf, 12, Fx); - _mav_put_float(buf, 16, Fy); - _mav_put_float(buf, 20, Fz); - _mav_put_uint8_t(buf, 24, mode); - _mav_put_uint8_t(buf, 25, ignore); + char buf[MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN]; + _mav_put_float(buf, 0, Qx); + _mav_put_float(buf, 4, Qy); + _mav_put_float(buf, 8, Qz); + _mav_put_float(buf, 12, Fx); + _mav_put_float(buf, 16, Fy); + _mav_put_float(buf, 20, Fz); + _mav_put_uint8_t(buf, 24, mode); + _mav_put_uint8_t(buf, 25, ignore); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN); #else - mavlink_offboard_control_t packet; - packet.Qx = Qx; - packet.Qy = Qy; - packet.Qz = Qz; - packet.Fx = Fx; - packet.Fy = Fy; - packet.Fz = Fz; - packet.mode = mode; - packet.ignore = ignore; + mavlink_offboard_control_t packet; + packet.Qx = Qx; + packet.Qy = Qy; + packet.Qz = Qz; + packet.Fx = Fx; + packet.Fy = Fy; + packet.Fz = Fz; + packet.mode = mode; + packet.ignore = ignore; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_OFFBOARD_CONTROL; + msg->msgid = MAVLINK_MSG_ID_OFFBOARD_CONTROL; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OFFBOARD_CONTROL_MIN_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OFFBOARD_CONTROL_MIN_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN); #endif } @@ -96,52 +165,48 @@ static inline uint16_t mavlink_msg_offboard_control_pack(uint8_t system_id, uint * @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 mode Offboard control mode, see OFFBOARD_CONTROL_MODE - * @param ignore Bitfield specifying which fields should be ignored, see OFFBOARD_CONTROL_IGNORE - * @param Qx Qx control channel interpreted according to mode - * @param Qy Qy control channel, interpreted according to mode - * @param Qz Qz control channel, interpreted according to mode - * @param Fx Fx control channel interpreted according to mode - * @param Fy Fy control channel, interpreted according to mode - * @param Fz Fz control channel, interpreted according to mode + * @param mode Offboard control mode, see OFFBOARD_CONTROL_MODE + * @param ignore Bitfield specifying which fields should be ignored, see OFFBOARD_CONTROL_IGNORE + * @param Qx Qx control channel interpreted according to mode + * @param Qy Qy control channel, interpreted according to mode + * @param Qz Qz control channel, interpreted according to mode + * @param Fx Fx control channel interpreted according to mode + * @param Fy Fy control channel, interpreted according to mode + * @param Fz Fz control channel, interpreted according to mode * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_offboard_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t mode,uint8_t ignore,float Qx,float Qy,float Qz,float Fx,float Fy,float Fz) + mavlink_message_t* msg, + uint8_t mode,uint8_t ignore,float Qx,float Qy,float Qz,float Fx,float Fy,float Fz) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN]; - _mav_put_float(buf, 0, Qx); - _mav_put_float(buf, 4, Qy); - _mav_put_float(buf, 8, Qz); - _mav_put_float(buf, 12, Fx); - _mav_put_float(buf, 16, Fy); - _mav_put_float(buf, 20, Fz); - _mav_put_uint8_t(buf, 24, mode); - _mav_put_uint8_t(buf, 25, ignore); + char buf[MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN]; + _mav_put_float(buf, 0, Qx); + _mav_put_float(buf, 4, Qy); + _mav_put_float(buf, 8, Qz); + _mav_put_float(buf, 12, Fx); + _mav_put_float(buf, 16, Fy); + _mav_put_float(buf, 20, Fz); + _mav_put_uint8_t(buf, 24, mode); + _mav_put_uint8_t(buf, 25, ignore); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN); #else - mavlink_offboard_control_t packet; - packet.Qx = Qx; - packet.Qy = Qy; - packet.Qz = Qz; - packet.Fx = Fx; - packet.Fy = Fy; - packet.Fz = Fz; - packet.mode = mode; - packet.ignore = ignore; + mavlink_offboard_control_t packet; + packet.Qx = Qx; + packet.Qy = Qy; + packet.Qz = Qz; + packet.Fx = Fx; + packet.Fy = Fy; + packet.Fz = Fz; + packet.mode = mode; + packet.ignore = ignore; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_OFFBOARD_CONTROL; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_OFFBOARD_CONTROL; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OFFBOARD_CONTROL_MIN_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_CRC); } /** @@ -154,7 +219,7 @@ static inline uint16_t mavlink_msg_offboard_control_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_offboard_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_offboard_control_t* offboard_control) { - return mavlink_msg_offboard_control_pack(system_id, component_id, msg, offboard_control->mode, offboard_control->ignore, offboard_control->Qx, offboard_control->Qy, offboard_control->Qz, offboard_control->Fx, offboard_control->Fy, offboard_control->Fz); + return mavlink_msg_offboard_control_pack(system_id, component_id, msg, offboard_control->mode, offboard_control->ignore, offboard_control->Qx, offboard_control->Qy, offboard_control->Qz, offboard_control->Fx, offboard_control->Fy, offboard_control->Fz); } /** @@ -168,64 +233,84 @@ static inline uint16_t mavlink_msg_offboard_control_encode(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_offboard_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_offboard_control_t* offboard_control) { - return mavlink_msg_offboard_control_pack_chan(system_id, component_id, chan, msg, offboard_control->mode, offboard_control->ignore, offboard_control->Qx, offboard_control->Qy, offboard_control->Qz, offboard_control->Fx, offboard_control->Fy, offboard_control->Fz); + return mavlink_msg_offboard_control_pack_chan(system_id, component_id, chan, msg, offboard_control->mode, offboard_control->ignore, offboard_control->Qx, offboard_control->Qy, offboard_control->Qz, offboard_control->Fx, offboard_control->Fy, offboard_control->Fz); +} + +/** + * @brief Encode a offboard_control struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param offboard_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_offboard_control_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_offboard_control_t* offboard_control) +{ + return mavlink_msg_offboard_control_pack_status(system_id, component_id, _status, msg, offboard_control->mode, offboard_control->ignore, offboard_control->Qx, offboard_control->Qy, offboard_control->Qz, offboard_control->Fx, offboard_control->Fy, offboard_control->Fz); } /** * @brief Send a offboard_control message * @param chan MAVLink channel to send the message * - * @param mode Offboard control mode, see OFFBOARD_CONTROL_MODE - * @param ignore Bitfield specifying which fields should be ignored, see OFFBOARD_CONTROL_IGNORE - * @param Qx Qx control channel interpreted according to mode - * @param Qy Qy control channel, interpreted according to mode - * @param Qz Qz control channel, interpreted according to mode - * @param Fx Fx control channel interpreted according to mode - * @param Fy Fy control channel, interpreted according to mode - * @param Fz Fz control channel, interpreted according to mode + * @param mode Offboard control mode, see OFFBOARD_CONTROL_MODE + * @param ignore Bitfield specifying which fields should be ignored, see OFFBOARD_CONTROL_IGNORE + * @param Qx Qx control channel interpreted according to mode + * @param Qy Qy control channel, interpreted according to mode + * @param Qz Qz control channel, interpreted according to mode + * @param Fx Fx control channel interpreted according to mode + * @param Fy Fy control channel, interpreted according to mode + * @param Fz Fz control channel, interpreted according to mode */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_offboard_control_send(mavlink_channel_t chan, uint8_t mode, uint8_t ignore, float Qx, float Qy, float Qz, float Fx, float Fy, float Fz) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN]; - _mav_put_float(buf, 0, Qx); - _mav_put_float(buf, 4, Qy); - _mav_put_float(buf, 8, Qz); - _mav_put_float(buf, 12, Fx); - _mav_put_float(buf, 16, Fy); - _mav_put_float(buf, 20, Fz); - _mav_put_uint8_t(buf, 24, mode); - _mav_put_uint8_t(buf, 25, ignore); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OFFBOARD_CONTROL, buf, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_CRC); + char buf[MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN]; + _mav_put_float(buf, 0, Qx); + _mav_put_float(buf, 4, Qy); + _mav_put_float(buf, 8, Qz); + _mav_put_float(buf, 12, Fx); + _mav_put_float(buf, 16, Fy); + _mav_put_float(buf, 20, Fz); + _mav_put_uint8_t(buf, 24, mode); + _mav_put_uint8_t(buf, 25, ignore); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OFFBOARD_CONTROL, buf, MAVLINK_MSG_ID_OFFBOARD_CONTROL_MIN_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OFFBOARD_CONTROL, buf, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN); + mavlink_offboard_control_t packet; + packet.Qx = Qx; + packet.Qy = Qy; + packet.Qz = Qz; + packet.Fx = Fx; + packet.Fy = Fy; + packet.Fz = Fz; + packet.mode = mode; + packet.ignore = ignore; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OFFBOARD_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_OFFBOARD_CONTROL_MIN_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_CRC); #endif -#else - mavlink_offboard_control_t packet; - packet.Qx = Qx; - packet.Qy = Qy; - packet.Qz = Qz; - packet.Fx = Fx; - packet.Fy = Fy; - packet.Fz = Fz; - packet.mode = mode; - packet.ignore = ignore; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OFFBOARD_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_CRC); +/** + * @brief Send a offboard_control message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_offboard_control_send_struct(mavlink_channel_t chan, const mavlink_offboard_control_t* offboard_control) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_offboard_control_send(chan, offboard_control->mode, offboard_control->ignore, offboard_control->Qx, offboard_control->Qy, offboard_control->Qz, offboard_control->Fx, offboard_control->Fy, offboard_control->Fz); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OFFBOARD_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OFFBOARD_CONTROL, (const char *)offboard_control, MAVLINK_MSG_ID_OFFBOARD_CONTROL_MIN_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_CRC); #endif } #if MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -234,37 +319,29 @@ static inline void mavlink_msg_offboard_control_send(mavlink_channel_t chan, uin static inline void mavlink_msg_offboard_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t mode, uint8_t ignore, float Qx, float Qy, float Qz, float Fx, float Fy, float Fz) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, Qx); - _mav_put_float(buf, 4, Qy); - _mav_put_float(buf, 8, Qz); - _mav_put_float(buf, 12, Fx); - _mav_put_float(buf, 16, Fy); - _mav_put_float(buf, 20, Fz); - _mav_put_uint8_t(buf, 24, mode); - _mav_put_uint8_t(buf, 25, ignore); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OFFBOARD_CONTROL, buf, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OFFBOARD_CONTROL, buf, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN); -#endif -#else - mavlink_offboard_control_t *packet = (mavlink_offboard_control_t *)msgbuf; - packet->Qx = Qx; - packet->Qy = Qy; - packet->Qz = Qz; - packet->Fx = Fx; - packet->Fy = Fy; - packet->Fz = Fz; - packet->mode = mode; - packet->ignore = ignore; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OFFBOARD_CONTROL, (const char *)packet, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_CRC); + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, Qx); + _mav_put_float(buf, 4, Qy); + _mav_put_float(buf, 8, Qz); + _mav_put_float(buf, 12, Fx); + _mav_put_float(buf, 16, Fy); + _mav_put_float(buf, 20, Fz); + _mav_put_uint8_t(buf, 24, mode); + _mav_put_uint8_t(buf, 25, ignore); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OFFBOARD_CONTROL, buf, MAVLINK_MSG_ID_OFFBOARD_CONTROL_MIN_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OFFBOARD_CONTROL, (const char *)packet, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN); -#endif + mavlink_offboard_control_t *packet = (mavlink_offboard_control_t *)msgbuf; + packet->Qx = Qx; + packet->Qy = Qy; + packet->Qz = Qz; + packet->Fx = Fx; + packet->Fy = Fy; + packet->Fz = Fz; + packet->mode = mode; + packet->ignore = ignore; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OFFBOARD_CONTROL, (const char *)packet, MAVLINK_MSG_ID_OFFBOARD_CONTROL_MIN_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN, MAVLINK_MSG_ID_OFFBOARD_CONTROL_CRC); #endif } #endif @@ -277,81 +354,81 @@ static inline void mavlink_msg_offboard_control_send_buf(mavlink_message_t *msgb /** * @brief Get field mode from offboard_control message * - * @return Offboard control mode, see OFFBOARD_CONTROL_MODE + * @return Offboard control mode, see OFFBOARD_CONTROL_MODE */ static inline uint8_t mavlink_msg_offboard_control_get_mode(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 24); + return _MAV_RETURN_uint8_t(msg, 24); } /** * @brief Get field ignore from offboard_control message * - * @return Bitfield specifying which fields should be ignored, see OFFBOARD_CONTROL_IGNORE + * @return Bitfield specifying which fields should be ignored, see OFFBOARD_CONTROL_IGNORE */ static inline uint8_t mavlink_msg_offboard_control_get_ignore(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 25); + return _MAV_RETURN_uint8_t(msg, 25); } /** * @brief Get field Qx from offboard_control message * - * @return Qx control channel interpreted according to mode + * @return Qx control channel interpreted according to mode */ static inline float mavlink_msg_offboard_control_get_Qx(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** * @brief Get field Qy from offboard_control message * - * @return Qy control channel, interpreted according to mode + * @return Qy control channel, interpreted according to mode */ static inline float mavlink_msg_offboard_control_get_Qy(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** * @brief Get field Qz from offboard_control message * - * @return Qz control channel, interpreted according to mode + * @return Qz control channel, interpreted according to mode */ static inline float mavlink_msg_offboard_control_get_Qz(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** * @brief Get field Fx from offboard_control message * - * @return Fx control channel interpreted according to mode + * @return Fx control channel interpreted according to mode */ static inline float mavlink_msg_offboard_control_get_Fx(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** * @brief Get field Fy from offboard_control message * - * @return Fy control channel, interpreted according to mode + * @return Fy control channel, interpreted according to mode */ static inline float mavlink_msg_offboard_control_get_Fy(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** * @brief Get field Fz from offboard_control message * - * @return Fz control channel, interpreted according to mode + * @return Fz control channel, interpreted according to mode */ static inline float mavlink_msg_offboard_control_get_Fz(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -362,16 +439,18 @@ static inline float mavlink_msg_offboard_control_get_Fz(const mavlink_message_t* */ static inline void mavlink_msg_offboard_control_decode(const mavlink_message_t* msg, mavlink_offboard_control_t* offboard_control) { -#if MAVLINK_NEED_BYTE_SWAP - offboard_control->Qx = mavlink_msg_offboard_control_get_Qx(msg); - offboard_control->Qy = mavlink_msg_offboard_control_get_Qy(msg); - offboard_control->Qz = mavlink_msg_offboard_control_get_Qz(msg); - offboard_control->Fx = mavlink_msg_offboard_control_get_Fx(msg); - offboard_control->Fy = mavlink_msg_offboard_control_get_Fy(msg); - offboard_control->Fz = mavlink_msg_offboard_control_get_Fz(msg); - offboard_control->mode = mavlink_msg_offboard_control_get_mode(msg); - offboard_control->ignore = mavlink_msg_offboard_control_get_ignore(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + offboard_control->Qx = mavlink_msg_offboard_control_get_Qx(msg); + offboard_control->Qy = mavlink_msg_offboard_control_get_Qy(msg); + offboard_control->Qz = mavlink_msg_offboard_control_get_Qz(msg); + offboard_control->Fx = mavlink_msg_offboard_control_get_Fx(msg); + offboard_control->Fy = mavlink_msg_offboard_control_get_Fy(msg); + offboard_control->Fz = mavlink_msg_offboard_control_get_Fz(msg); + offboard_control->mode = mavlink_msg_offboard_control_get_mode(msg); + offboard_control->ignore = mavlink_msg_offboard_control_get_ignore(msg); #else - memcpy(offboard_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN? msg->len : MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN; + memset(offboard_control, 0, MAVLINK_MSG_ID_OFFBOARD_CONTROL_LEN); + memcpy(offboard_control, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_param_request_list.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_param_request_list.h index f78f049b..d569e9fb 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_param_request_list.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_param_request_list.h @@ -1,29 +1,42 @@ +#pragma once // MESSAGE PARAM_REQUEST_LIST PACKING #define MAVLINK_MSG_ID_PARAM_REQUEST_LIST 21 -typedef struct __mavlink_param_request_list_t -{ - uint8_t target_system; /*< System ID*/ - uint8_t target_component; /*< Component ID*/ + +typedef struct __mavlink_param_request_list_t { + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ } mavlink_param_request_list_t; #define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN 2 +#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_MIN_LEN 2 #define MAVLINK_MSG_ID_21_LEN 2 +#define MAVLINK_MSG_ID_21_MIN_LEN 2 #define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC 159 #define MAVLINK_MSG_ID_21_CRC 159 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST { \ - "PARAM_REQUEST_LIST", \ - 2, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_request_list_t, target_system) }, \ + 21, \ + "PARAM_REQUEST_LIST", \ + 2, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_request_list_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_request_list_t, target_component) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST { \ + "PARAM_REQUEST_LIST", \ + 2, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_request_list_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_request_list_t, target_component) }, \ + } \ +} +#endif /** * @brief Pack a param_request_list message @@ -31,32 +44,64 @@ typedef struct __mavlink_param_request_list_t * @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 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_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) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_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_REQUEST_LIST_LEN); +#else + mavlink_param_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_REQUEST_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); +} + +/** + * @brief Pack a param_request_list message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @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_request_list_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, 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_REQUEST_LIST_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); + char buf[MAVLINK_MSG_ID_PARAM_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_REQUEST_LIST_LEN); #else - mavlink_param_request_list_t packet; - packet.target_system = target_system; - packet.target_component = target_component; + mavlink_param_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_REQUEST_LIST_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); #endif } @@ -66,34 +111,30 @@ static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, ui * @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 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_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) + 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_REQUEST_LIST_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); + char buf[MAVLINK_MSG_ID_PARAM_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_REQUEST_LIST_LEN); #else - mavlink_param_request_list_t packet; - packet.target_system = target_system; - packet.target_component = target_component; + mavlink_param_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_REQUEST_LIST_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); } /** @@ -106,7 +147,7 @@ static inline uint16_t mavlink_msg_param_request_list_pack_chan(uint8_t system_i */ static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_request_list_t* param_request_list) { - return mavlink_msg_param_request_list_pack(system_id, component_id, msg, param_request_list->target_system, param_request_list->target_component); + return mavlink_msg_param_request_list_pack(system_id, component_id, msg, param_request_list->target_system, param_request_list->target_component); } /** @@ -120,46 +161,66 @@ static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, */ static inline uint16_t mavlink_msg_param_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_request_list_t* param_request_list) { - return mavlink_msg_param_request_list_pack_chan(system_id, component_id, chan, msg, param_request_list->target_system, param_request_list->target_component); + return mavlink_msg_param_request_list_pack_chan(system_id, component_id, chan, msg, param_request_list->target_system, param_request_list->target_component); +} + +/** + * @brief Encode a param_request_list struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param param_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_request_list_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_param_request_list_t* param_request_list) +{ + return mavlink_msg_param_request_list_pack_status(system_id, component_id, _status, msg, param_request_list->target_system, param_request_list->target_component); } /** * @brief Send a param_request_list message * @param chan MAVLink channel to send the message * - * @param target_system System ID - * @param target_component Component ID + * @param target_system System ID + * @param target_component Component ID */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_param_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_REQUEST_LIST_LEN]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); + char buf[MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); + mavlink_param_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); #endif -#else - mavlink_param_request_list_t packet; - packet.target_system = target_system; - packet.target_component = target_component; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); +/** + * @brief Send a param_request_list message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_param_request_list_send_struct(mavlink_channel_t chan, const mavlink_param_request_list_t* param_request_list) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_param_request_list_send(chan, param_request_list->target_system, param_request_list->target_component); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)param_request_list, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); #endif } #if MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -168,25 +229,17 @@ static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, u static inline void mavlink_msg_param_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); + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); #else - mavlink_param_request_list_t *packet = (mavlink_param_request_list_t *)msgbuf; - packet->target_system = target_system; - packet->target_component = target_component; + mavlink_param_request_list_t *packet = (mavlink_param_request_list_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); #endif } #endif @@ -199,21 +252,21 @@ static inline void mavlink_msg_param_request_list_send_buf(mavlink_message_t *ms /** * @brief Get field target_system from param_request_list message * - * @return System ID + * @return System ID */ static inline uint8_t mavlink_msg_param_request_list_get_target_system(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 0); + return _MAV_RETURN_uint8_t(msg, 0); } /** * @brief Get field target_component from param_request_list message * - * @return Component ID + * @return Component ID */ static inline uint8_t mavlink_msg_param_request_list_get_target_component(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 1); + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -224,10 +277,12 @@ static inline uint8_t mavlink_msg_param_request_list_get_target_component(const */ static inline void mavlink_msg_param_request_list_decode(const mavlink_message_t* msg, mavlink_param_request_list_t* param_request_list) { -#if MAVLINK_NEED_BYTE_SWAP - param_request_list->target_system = mavlink_msg_param_request_list_get_target_system(msg); - param_request_list->target_component = mavlink_msg_param_request_list_get_target_component(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + param_request_list->target_system = mavlink_msg_param_request_list_get_target_system(msg); + param_request_list->target_component = mavlink_msg_param_request_list_get_target_component(msg); #else - memcpy(param_request_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN? msg->len : MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN; + memset(param_request_list, 0, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); + memcpy(param_request_list, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_param_request_read.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_param_request_read.h index 497f5984..276924e3 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_param_request_read.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_param_request_read.h @@ -1,33 +1,48 @@ +#pragma once // MESSAGE PARAM_REQUEST_READ PACKING #define MAVLINK_MSG_ID_PARAM_REQUEST_READ 20 -typedef struct __mavlink_param_request_read_t -{ - int16_t param_index; /*< Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored)*/ - uint8_t target_system; /*< System ID*/ - uint8_t target_component; /*< Component ID*/ - char param_id[16]; /*< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string*/ + +typedef struct __mavlink_param_request_read_t { + int16_t param_index; /*< Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored)*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + char param_id[16]; /*< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string*/ } mavlink_param_request_read_t; #define MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN 20 +#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_MIN_LEN 20 #define MAVLINK_MSG_ID_20_LEN 20 +#define MAVLINK_MSG_ID_20_MIN_LEN 20 #define MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC 214 #define MAVLINK_MSG_ID_20_CRC 214 #define MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 16 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ { \ - "PARAM_REQUEST_READ", \ - 4, \ - { { "param_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_param_request_read_t, param_index) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_param_request_read_t, target_system) }, \ + 20, \ + "PARAM_REQUEST_READ", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_param_request_read_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_param_request_read_t, target_component) }, \ { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 4, offsetof(mavlink_param_request_read_t, param_id) }, \ + { "param_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_param_request_read_t, param_index) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ { \ + "PARAM_REQUEST_READ", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_param_request_read_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_param_request_read_t, target_component) }, \ + { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 4, offsetof(mavlink_param_request_read_t, param_id) }, \ + { "param_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_param_request_read_t, param_index) }, \ + } \ +} +#endif /** * @brief Pack a param_request_read message @@ -35,36 +50,72 @@ typedef struct __mavlink_param_request_read_t * @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 Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_param_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) + 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_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_REQUEST_READ_LEN); +#else + mavlink_param_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_REQUEST_READ_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); +} + +/** + * @brief Pack a param_request_read message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_request_read_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, 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_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); + char buf[MAVLINK_MSG_ID_PARAM_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_REQUEST_READ_LEN); #else - mavlink_param_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); + mavlink_param_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_REQUEST_READ_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); #endif } @@ -74,38 +125,34 @@ static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, ui * @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 Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_param_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) + 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_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); + char buf[MAVLINK_MSG_ID_PARAM_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_REQUEST_READ_LEN); #else - mavlink_param_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); + mavlink_param_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_REQUEST_READ_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); } /** @@ -118,7 +165,7 @@ static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_i */ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_request_read_t* param_request_read) { - return mavlink_msg_param_request_read_pack(system_id, component_id, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); + return mavlink_msg_param_request_read_pack(system_id, component_id, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); } /** @@ -132,50 +179,70 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, */ static inline uint16_t mavlink_msg_param_request_read_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_request_read_t* param_request_read) { - return mavlink_msg_param_request_read_pack_chan(system_id, component_id, chan, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); + return mavlink_msg_param_request_read_pack_chan(system_id, component_id, chan, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); +} + +/** + * @brief Encode a param_request_read struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param param_request_read C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_request_read_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_param_request_read_t* param_request_read) +{ + return mavlink_msg_param_request_read_pack_status(system_id, component_id, _status, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); } /** * @brief Send a param_request_read message * @param chan MAVLink channel to send the message * - * @param target_system System ID - * @param target_component Component ID - * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_param_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_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); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); + char buf[MAVLINK_MSG_ID_PARAM_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_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); + mavlink_param_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_REQUEST_READ, (const char *)&packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); #endif +} + +/** + * @brief Send a param_request_read message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_param_request_read_send_struct(mavlink_channel_t chan, const mavlink_param_request_read_t* param_request_read) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_param_request_read_send(chan, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); #else - mavlink_param_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); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)&packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)&packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)param_request_read, MAVLINK_MSG_ID_PARAM_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); #endif } #if MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -184,27 +251,19 @@ static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, u static inline void mavlink_msg_param_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); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); + 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_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); -#endif -#else - mavlink_param_request_read_t *packet = (mavlink_param_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); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); -#endif + mavlink_param_request_read_t *packet = (mavlink_param_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_REQUEST_READ, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); #endif } #endif @@ -217,41 +276,41 @@ static inline void mavlink_msg_param_request_read_send_buf(mavlink_message_t *ms /** * @brief Get field target_system from param_request_read message * - * @return System ID + * @return System ID */ static inline uint8_t mavlink_msg_param_request_read_get_target_system(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** * @brief Get field target_component from param_request_read message * - * @return Component ID + * @return Component ID */ static inline uint8_t mavlink_msg_param_request_read_get_target_component(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 3); + return _MAV_RETURN_uint8_t(msg, 3); } /** * @brief Get field param_id from param_request_read message * - * @return Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @return Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string */ static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink_message_t* msg, char *param_id) { - return _MAV_RETURN_char_array(msg, param_id, 16, 4); + return _MAV_RETURN_char_array(msg, param_id, 16, 4); } /** * @brief Get field param_index from param_request_read message * - * @return Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) + * @return Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) */ static inline int16_t mavlink_msg_param_request_read_get_param_index(const mavlink_message_t* msg) { - return _MAV_RETURN_int16_t(msg, 0); + return _MAV_RETURN_int16_t(msg, 0); } /** @@ -262,12 +321,14 @@ static inline int16_t mavlink_msg_param_request_read_get_param_index(const mavli */ static inline void mavlink_msg_param_request_read_decode(const mavlink_message_t* msg, mavlink_param_request_read_t* param_request_read) { -#if MAVLINK_NEED_BYTE_SWAP - param_request_read->param_index = mavlink_msg_param_request_read_get_param_index(msg); - param_request_read->target_system = mavlink_msg_param_request_read_get_target_system(msg); - param_request_read->target_component = mavlink_msg_param_request_read_get_target_component(msg); - mavlink_msg_param_request_read_get_param_id(msg, param_request_read->param_id); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + param_request_read->param_index = mavlink_msg_param_request_read_get_param_index(msg); + param_request_read->target_system = mavlink_msg_param_request_read_get_target_system(msg); + param_request_read->target_component = mavlink_msg_param_request_read_get_target_component(msg); + mavlink_msg_param_request_read_get_param_id(msg, param_request_read->param_id); #else - memcpy(param_request_read, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN? msg->len : MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN; + memset(param_request_read, 0, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); + memcpy(param_request_read, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_param_set.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_param_set.h index 471e6747..f0560b4f 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_param_set.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_param_set.h @@ -1,35 +1,51 @@ +#pragma once // MESSAGE PARAM_SET PACKING #define MAVLINK_MSG_ID_PARAM_SET 23 -typedef struct __mavlink_param_set_t -{ - float param_value; /*< Onboard parameter value*/ - uint8_t target_system; /*< System ID*/ - uint8_t target_component; /*< Component ID*/ - char param_id[16]; /*< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string*/ - uint8_t param_type; /*< Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types.*/ + +typedef struct __mavlink_param_set_t { + float param_value; /*< Onboard parameter value*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + char param_id[16]; /*< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string*/ + uint8_t param_type; /*< Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types.*/ } mavlink_param_set_t; #define MAVLINK_MSG_ID_PARAM_SET_LEN 23 +#define MAVLINK_MSG_ID_PARAM_SET_MIN_LEN 23 #define MAVLINK_MSG_ID_23_LEN 23 +#define MAVLINK_MSG_ID_23_MIN_LEN 23 #define MAVLINK_MSG_ID_PARAM_SET_CRC 168 #define MAVLINK_MSG_ID_23_CRC 168 #define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 16 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_PARAM_SET { \ - "PARAM_SET", \ - 5, \ - { { "param_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_param_set_t, param_value) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_param_set_t, target_system) }, \ + 23, \ + "PARAM_SET", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_param_set_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_param_set_t, target_component) }, \ { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 6, offsetof(mavlink_param_set_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_param_set_t, param_value) }, \ { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_param_set_t, param_type) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_PARAM_SET { \ + "PARAM_SET", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_param_set_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_param_set_t, target_component) }, \ + { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 6, offsetof(mavlink_param_set_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_param_set_t, param_value) }, \ + { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_param_set_t, param_type) }, \ + } \ +} +#endif /** * @brief Pack a param_set message @@ -37,39 +53,78 @@ typedef struct __mavlink_param_set_t * @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 Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_value Onboard parameter value - * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_param_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, float param_value, uint8_t param_type) + uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_SET_LEN]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 22, param_type); + _mav_put_char_array(buf, 6, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_SET_LEN); +#else + mavlink_param_set_t packet; + packet.param_value = param_value; + 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); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_SET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_SET; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); +} + +/** + * @brief Pack a param_set message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_set_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_SET_LEN]; - _mav_put_float(buf, 0, param_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 22, param_type); - _mav_put_char_array(buf, 6, param_id, 16); + char buf[MAVLINK_MSG_ID_PARAM_SET_LEN]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 22, param_type); + _mav_put_char_array(buf, 6, param_id, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_SET_LEN); #else - mavlink_param_set_t packet; - packet.param_value = param_value; - 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); + mavlink_param_set_t packet; + packet.param_value = param_value; + 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); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_SET_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_PARAM_SET; + msg->msgid = MAVLINK_MSG_ID_PARAM_SET; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_SET_LEN); #endif } @@ -79,41 +134,37 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com * @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 Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_value Onboard parameter value - * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_param_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,float param_value,uint8_t param_type) + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const char *param_id,float param_value,uint8_t param_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_SET_LEN]; - _mav_put_float(buf, 0, param_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 22, param_type); - _mav_put_char_array(buf, 6, param_id, 16); + char buf[MAVLINK_MSG_ID_PARAM_SET_LEN]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 22, param_type); + _mav_put_char_array(buf, 6, param_id, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_SET_LEN); #else - mavlink_param_set_t packet; - packet.param_value = param_value; - 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); + mavlink_param_set_t packet; + packet.param_value = param_value; + 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); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_SET_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_PARAM_SET; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_SET; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); } /** @@ -126,7 +177,7 @@ static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_set_t* param_set) { - return mavlink_msg_param_set_pack(system_id, component_id, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type); + return mavlink_msg_param_set_pack(system_id, component_id, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type); } /** @@ -140,53 +191,73 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c */ static inline uint16_t mavlink_msg_param_set_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_set_t* param_set) { - return mavlink_msg_param_set_pack_chan(system_id, component_id, chan, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type); + return mavlink_msg_param_set_pack_chan(system_id, component_id, chan, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type); +} + +/** + * @brief Encode a param_set struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param param_set C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_set_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_param_set_t* param_set) +{ + return mavlink_msg_param_set_pack_status(system_id, component_id, _status, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type); } /** * @brief Send a param_set message * @param chan MAVLink channel to send the message * - * @param target_system System ID - * @param target_component Component ID - * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_value Onboard parameter value - * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_SET_LEN]; - _mav_put_float(buf, 0, param_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 22, param_type); - _mav_put_char_array(buf, 6, param_id, 16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); + char buf[MAVLINK_MSG_ID_PARAM_SET_LEN]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 22, param_type); + _mav_put_char_array(buf, 6, param_id, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN); + mavlink_param_set_t packet; + packet.param_value = param_value; + 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_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)&packet, MAVLINK_MSG_ID_PARAM_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); #endif +} + +/** + * @brief Send a param_set message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_param_set_send_struct(mavlink_channel_t chan, const mavlink_param_set_t* param_set) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_param_set_send(chan, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type); #else - mavlink_param_set_t packet; - packet.param_value = param_value; - 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); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)&packet, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)&packet, MAVLINK_MSG_ID_PARAM_SET_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)param_set, MAVLINK_MSG_ID_PARAM_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); #endif } #if MAVLINK_MSG_ID_PARAM_SET_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -195,29 +266,21 @@ static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t ta static inline void mavlink_msg_param_set_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, param_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 22, param_type); - _mav_put_char_array(buf, 6, param_id, 16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 22, param_type); + _mav_put_char_array(buf, 6, param_id, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN); -#endif -#else - mavlink_param_set_t *packet = (mavlink_param_set_t *)msgbuf; - packet->param_value = param_value; - 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); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)packet, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)packet, MAVLINK_MSG_ID_PARAM_SET_LEN); -#endif + mavlink_param_set_t *packet = (mavlink_param_set_t *)msgbuf; + packet->param_value = param_value; + 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_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)packet, MAVLINK_MSG_ID_PARAM_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); #endif } #endif @@ -230,51 +293,51 @@ static inline void mavlink_msg_param_set_send_buf(mavlink_message_t *msgbuf, mav /** * @brief Get field target_system from param_set message * - * @return System ID + * @return System ID */ static inline uint8_t mavlink_msg_param_set_get_target_system(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 4); + return _MAV_RETURN_uint8_t(msg, 4); } /** * @brief Get field target_component from param_set message * - * @return Component ID + * @return Component ID */ static inline uint8_t mavlink_msg_param_set_get_target_component(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 5); + return _MAV_RETURN_uint8_t(msg, 5); } /** * @brief Get field param_id from param_set message * - * @return Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @return Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string */ static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_t* msg, char *param_id) { - return _MAV_RETURN_char_array(msg, param_id, 16, 6); + return _MAV_RETURN_char_array(msg, param_id, 16, 6); } /** * @brief Get field param_value from param_set message * - * @return Onboard parameter value + * @return Onboard parameter value */ static inline float mavlink_msg_param_set_get_param_value(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** * @brief Get field param_type from param_set message * - * @return Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + * @return Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. */ static inline uint8_t mavlink_msg_param_set_get_param_type(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 22); + return _MAV_RETURN_uint8_t(msg, 22); } /** @@ -285,13 +348,15 @@ static inline uint8_t mavlink_msg_param_set_get_param_type(const mavlink_message */ static inline void mavlink_msg_param_set_decode(const mavlink_message_t* msg, mavlink_param_set_t* param_set) { -#if MAVLINK_NEED_BYTE_SWAP - param_set->param_value = mavlink_msg_param_set_get_param_value(msg); - param_set->target_system = mavlink_msg_param_set_get_target_system(msg); - param_set->target_component = mavlink_msg_param_set_get_target_component(msg); - mavlink_msg_param_set_get_param_id(msg, param_set->param_id); - param_set->param_type = mavlink_msg_param_set_get_param_type(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + param_set->param_value = mavlink_msg_param_set_get_param_value(msg); + param_set->target_system = mavlink_msg_param_set_get_target_system(msg); + param_set->target_component = mavlink_msg_param_set_get_target_component(msg); + mavlink_msg_param_set_get_param_id(msg, param_set->param_id); + param_set->param_type = mavlink_msg_param_set_get_param_type(msg); #else - memcpy(param_set, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PARAM_SET_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_PARAM_SET_LEN? msg->len : MAVLINK_MSG_ID_PARAM_SET_LEN; + memset(param_set, 0, MAVLINK_MSG_ID_PARAM_SET_LEN); + memcpy(param_set, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_param_value.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_param_value.h index 856fa5ab..51bc8075 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_param_value.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_param_value.h @@ -1,35 +1,51 @@ +#pragma once // MESSAGE PARAM_VALUE PACKING #define MAVLINK_MSG_ID_PARAM_VALUE 22 -typedef struct __mavlink_param_value_t -{ - float param_value; /*< Onboard parameter value*/ - uint16_t param_count; /*< Total number of onboard parameters*/ - uint16_t param_index; /*< Index of this onboard parameter*/ - char param_id[16]; /*< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string*/ - uint8_t param_type; /*< Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types.*/ + +typedef struct __mavlink_param_value_t { + float param_value; /*< Onboard parameter value*/ + uint16_t param_count; /*< Total number of onboard parameters*/ + uint16_t param_index; /*< Index of this onboard parameter*/ + char param_id[16]; /*< Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string*/ + uint8_t param_type; /*< Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types.*/ } mavlink_param_value_t; #define MAVLINK_MSG_ID_PARAM_VALUE_LEN 25 +#define MAVLINK_MSG_ID_PARAM_VALUE_MIN_LEN 25 #define MAVLINK_MSG_ID_22_LEN 25 +#define MAVLINK_MSG_ID_22_MIN_LEN 25 #define MAVLINK_MSG_ID_PARAM_VALUE_CRC 220 #define MAVLINK_MSG_ID_22_CRC 220 #define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 16 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_PARAM_VALUE { \ - "PARAM_VALUE", \ - 5, \ - { { "param_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_param_value_t, param_value) }, \ + 22, \ + "PARAM_VALUE", \ + 5, \ + { { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 8, offsetof(mavlink_param_value_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_param_value_t, param_value) }, \ + { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_param_value_t, param_type) }, \ { "param_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_param_value_t, param_count) }, \ { "param_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_param_value_t, param_index) }, \ - { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 8, offsetof(mavlink_param_value_t, param_id) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_PARAM_VALUE { \ + "PARAM_VALUE", \ + 5, \ + { { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 8, offsetof(mavlink_param_value_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_param_value_t, param_value) }, \ { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_param_value_t, param_type) }, \ + { "param_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_param_value_t, param_count) }, \ + { "param_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_param_value_t, param_index) }, \ } \ } - +#endif /** * @brief Pack a param_value message @@ -37,39 +53,78 @@ typedef struct __mavlink_param_value_t * @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 Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_value Onboard parameter value - * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. - * @param param_count Total number of onboard parameters - * @param param_index Index of this onboard parameter + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + * @param param_count Total number of onboard parameters + * @param param_index Index of this onboard parameter * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const char *param_id, float param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) + const char *param_id, float 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_VALUE_LEN]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint16_t(buf, 4, param_count); + _mav_put_uint16_t(buf, 6, param_index); + _mav_put_uint8_t(buf, 24, param_type); + _mav_put_char_array(buf, 8, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#else + mavlink_param_value_t packet; + packet.param_value = param_value; + 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); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); +} + +/** + * @brief Pack a param_value message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + * @param param_count Total number of onboard parameters + * @param param_index Index of this onboard parameter + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_value_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + const char *param_id, float 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_VALUE_LEN]; - _mav_put_float(buf, 0, param_value); - _mav_put_uint16_t(buf, 4, param_count); - _mav_put_uint16_t(buf, 6, param_index); - _mav_put_uint8_t(buf, 24, param_type); - _mav_put_char_array(buf, 8, param_id, 16); + char buf[MAVLINK_MSG_ID_PARAM_VALUE_LEN]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint16_t(buf, 4, param_count); + _mav_put_uint16_t(buf, 6, param_index); + _mav_put_uint8_t(buf, 24, param_type); + _mav_put_char_array(buf, 8, param_id, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN); #else - mavlink_param_value_t packet; - packet.param_value = param_value; - 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); + mavlink_param_value_t packet; + packet.param_value = param_value; + 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); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; + msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_VALUE_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_VALUE_LEN); #endif } @@ -79,41 +134,37 @@ static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t c * @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 Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_value Onboard parameter value - * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. - * @param param_count Total number of onboard parameters - * @param param_index Index of this onboard parameter + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + * @param param_count Total number of onboard parameters + * @param param_index Index of this onboard parameter * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const char *param_id,float param_value,uint8_t param_type,uint16_t param_count,uint16_t param_index) + mavlink_message_t* msg, + const char *param_id,float 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_VALUE_LEN]; - _mav_put_float(buf, 0, param_value); - _mav_put_uint16_t(buf, 4, param_count); - _mav_put_uint16_t(buf, 6, param_index); - _mav_put_uint8_t(buf, 24, param_type); - _mav_put_char_array(buf, 8, param_id, 16); + char buf[MAVLINK_MSG_ID_PARAM_VALUE_LEN]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint16_t(buf, 4, param_count); + _mav_put_uint16_t(buf, 6, param_index); + _mav_put_uint8_t(buf, 24, param_type); + _mav_put_char_array(buf, 8, param_id, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN); #else - mavlink_param_value_t packet; - packet.param_value = param_value; - 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); + mavlink_param_value_t packet; + packet.param_value = param_value; + 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); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_VALUE_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); } /** @@ -126,7 +177,7 @@ static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_value_t* param_value) { - return mavlink_msg_param_value_pack(system_id, component_id, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); + return mavlink_msg_param_value_pack(system_id, component_id, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); } /** @@ -140,53 +191,73 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_param_value_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_value_t* param_value) { - return mavlink_msg_param_value_pack_chan(system_id, component_id, chan, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); + return mavlink_msg_param_value_pack_chan(system_id, component_id, chan, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); +} + +/** + * @brief Encode a param_value struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param param_value C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_value_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_param_value_t* param_value) +{ + return mavlink_msg_param_value_pack_status(system_id, component_id, _status, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); } /** * @brief Send a param_value message * @param chan MAVLink channel to send the message * - * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_value Onboard parameter value - * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. - * @param param_count Total number of onboard parameters - * @param param_index Index of this onboard parameter + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + * @param param_count Total number of onboard parameters + * @param param_index Index of this onboard parameter */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char *param_id, float 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_VALUE_LEN]; - _mav_put_float(buf, 0, param_value); - _mav_put_uint16_t(buf, 4, param_count); - _mav_put_uint16_t(buf, 6, param_index); - _mav_put_uint8_t(buf, 24, param_type); - _mav_put_char_array(buf, 8, param_id, 16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); + char buf[MAVLINK_MSG_ID_PARAM_VALUE_LEN]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint16_t(buf, 4, param_count); + _mav_put_uint16_t(buf, 6, param_index); + _mav_put_uint8_t(buf, 24, param_type); + _mav_put_char_array(buf, 8, param_id, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN); + mavlink_param_value_t packet; + packet.param_value = param_value; + 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_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)&packet, MAVLINK_MSG_ID_PARAM_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); #endif +} + +/** + * @brief Send a param_value message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_param_value_send_struct(mavlink_channel_t chan, const mavlink_param_value_t* param_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_param_value_send(chan, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); #else - mavlink_param_value_t packet; - packet.param_value = param_value; - 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); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)&packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)&packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)param_value, MAVLINK_MSG_ID_PARAM_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); #endif } #if MAVLINK_MSG_ID_PARAM_VALUE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -195,29 +266,21 @@ static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const ch static inline void mavlink_msg_param_value_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *param_id, float 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_float(buf, 0, param_value); - _mav_put_uint16_t(buf, 4, param_count); - _mav_put_uint16_t(buf, 6, param_index); - _mav_put_uint8_t(buf, 24, param_type); - _mav_put_char_array(buf, 8, param_id, 16); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param_value); + _mav_put_uint16_t(buf, 4, param_count); + _mav_put_uint16_t(buf, 6, param_index); + _mav_put_uint8_t(buf, 24, param_type); + _mav_put_char_array(buf, 8, param_id, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN); -#endif -#else - mavlink_param_value_t *packet = (mavlink_param_value_t *)msgbuf; - packet->param_value = param_value; - 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); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); -#endif + mavlink_param_value_t *packet = (mavlink_param_value_t *)msgbuf; + packet->param_value = param_value; + 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_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)packet, MAVLINK_MSG_ID_PARAM_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); #endif } #endif @@ -230,51 +293,51 @@ static inline void mavlink_msg_param_value_send_buf(mavlink_message_t *msgbuf, m /** * @brief Get field param_id from param_value message * - * @return Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @return Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string */ static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_message_t* msg, char *param_id) { - return _MAV_RETURN_char_array(msg, param_id, 16, 8); + return _MAV_RETURN_char_array(msg, param_id, 16, 8); } /** * @brief Get field param_value from param_value message * - * @return Onboard parameter value + * @return Onboard parameter value */ static inline float mavlink_msg_param_value_get_param_value(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** * @brief Get field param_type from param_value message * - * @return Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. + * @return Onboard parameter type: see the MAV_PARAM_TYPE enum for supported data types. */ static inline uint8_t mavlink_msg_param_value_get_param_type(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 24); + return _MAV_RETURN_uint8_t(msg, 24); } /** * @brief Get field param_count from param_value message * - * @return Total number of onboard parameters + * @return Total number of onboard parameters */ static inline uint16_t mavlink_msg_param_value_get_param_count(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 4); + return _MAV_RETURN_uint16_t(msg, 4); } /** * @brief Get field param_index from param_value message * - * @return Index of this onboard parameter + * @return Index of this onboard parameter */ static inline uint16_t mavlink_msg_param_value_get_param_index(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 6); + return _MAV_RETURN_uint16_t(msg, 6); } /** @@ -285,13 +348,15 @@ static inline uint16_t mavlink_msg_param_value_get_param_index(const mavlink_mes */ static inline void mavlink_msg_param_value_decode(const mavlink_message_t* msg, mavlink_param_value_t* param_value) { -#if MAVLINK_NEED_BYTE_SWAP - param_value->param_value = mavlink_msg_param_value_get_param_value(msg); - param_value->param_count = mavlink_msg_param_value_get_param_count(msg); - param_value->param_index = mavlink_msg_param_value_get_param_index(msg); - mavlink_msg_param_value_get_param_id(msg, param_value->param_id); - param_value->param_type = mavlink_msg_param_value_get_param_type(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + param_value->param_value = mavlink_msg_param_value_get_param_value(msg); + param_value->param_count = mavlink_msg_param_value_get_param_count(msg); + param_value->param_index = mavlink_msg_param_value_get_param_index(msg); + mavlink_msg_param_value_get_param_id(msg, param_value->param_id); + param_value->param_type = mavlink_msg_param_value_get_param_type(msg); #else - memcpy(param_value, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_PARAM_VALUE_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_PARAM_VALUE_LEN? msg->len : MAVLINK_MSG_ID_PARAM_VALUE_LEN; + memset(param_value, 0, MAVLINK_MSG_ID_PARAM_VALUE_LEN); + memcpy(param_value, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_rc_channels.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_rc_channels.h index b4ece266..5fd76f87 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_rc_channels.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_rc_channels.h @@ -1,44 +1,50 @@ +#pragma once // MESSAGE RC_CHANNELS PACKING #define MAVLINK_MSG_ID_RC_CHANNELS 65 -typedef struct __mavlink_rc_channels_t -{ - uint32_t time_boot_ms; /*< Timestamp (milliseconds since system boot)*/ - uint16_t chan1_raw; /*< RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint16_t chan2_raw; /*< RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint16_t chan3_raw; /*< RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint16_t chan4_raw; /*< RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint16_t chan5_raw; /*< RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint16_t chan6_raw; /*< RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint16_t chan7_raw; /*< RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint16_t chan8_raw; /*< RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint16_t chan9_raw; /*< RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint16_t chan10_raw; /*< RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint16_t chan11_raw; /*< RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint16_t chan12_raw; /*< RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint16_t chan13_raw; /*< RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint16_t chan14_raw; /*< RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint16_t chan15_raw; /*< RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint16_t chan16_raw; /*< RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint16_t chan17_raw; /*< RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint16_t chan18_raw; /*< RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ - uint8_t chancount; /*< Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available.*/ - uint8_t rssi; /*< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown.*/ + +typedef struct __mavlink_rc_channels_t { + uint32_t time_boot_ms; /*< Timestamp (milliseconds since system boot)*/ + uint16_t chan1_raw; /*< RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint16_t chan2_raw; /*< RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint16_t chan3_raw; /*< RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint16_t chan4_raw; /*< RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint16_t chan5_raw; /*< RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint16_t chan6_raw; /*< RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint16_t chan7_raw; /*< RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint16_t chan8_raw; /*< RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint16_t chan9_raw; /*< RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint16_t chan10_raw; /*< RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint16_t chan11_raw; /*< RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint16_t chan12_raw; /*< RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint16_t chan13_raw; /*< RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint16_t chan14_raw; /*< RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint16_t chan15_raw; /*< RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint16_t chan16_raw; /*< RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint16_t chan17_raw; /*< RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint16_t chan18_raw; /*< RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused.*/ + uint8_t chancount; /*< Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available.*/ + uint8_t rssi; /*< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown.*/ } mavlink_rc_channels_t; #define MAVLINK_MSG_ID_RC_CHANNELS_LEN 42 +#define MAVLINK_MSG_ID_RC_CHANNELS_MIN_LEN 42 #define MAVLINK_MSG_ID_65_LEN 42 +#define MAVLINK_MSG_ID_65_MIN_LEN 42 #define MAVLINK_MSG_ID_RC_CHANNELS_CRC 118 #define MAVLINK_MSG_ID_65_CRC 118 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_RC_CHANNELS { \ - "RC_CHANNELS", \ - 21, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_rc_channels_t, time_boot_ms) }, \ + 65, \ + "RC_CHANNELS", \ + 21, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_rc_channels_t, time_boot_ms) }, \ + { "chancount", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_rc_channels_t, chancount) }, \ { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_t, chan1_raw) }, \ { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_t, chan2_raw) }, \ { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_t, chan3_raw) }, \ @@ -57,11 +63,37 @@ typedef struct __mavlink_rc_channels_t { "chan16_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 34, offsetof(mavlink_rc_channels_t, chan16_raw) }, \ { "chan17_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_rc_channels_t, chan17_raw) }, \ { "chan18_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 38, offsetof(mavlink_rc_channels_t, chan18_raw) }, \ + { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_rc_channels_t, rssi) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_RC_CHANNELS { \ + "RC_CHANNELS", \ + 21, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_rc_channels_t, time_boot_ms) }, \ { "chancount", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_rc_channels_t, chancount) }, \ + { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_t, chan1_raw) }, \ + { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_t, chan2_raw) }, \ + { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_t, chan3_raw) }, \ + { "chan4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_t, chan4_raw) }, \ + { "chan5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_t, chan5_raw) }, \ + { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_t, chan6_raw) }, \ + { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_rc_channels_t, chan7_raw) }, \ + { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_rc_channels_t, chan8_raw) }, \ + { "chan9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_rc_channels_t, chan9_raw) }, \ + { "chan10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_rc_channels_t, chan10_raw) }, \ + { "chan11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_rc_channels_t, chan11_raw) }, \ + { "chan12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_rc_channels_t, chan12_raw) }, \ + { "chan13_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_rc_channels_t, chan13_raw) }, \ + { "chan14_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_rc_channels_t, chan14_raw) }, \ + { "chan15_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_rc_channels_t, chan15_raw) }, \ + { "chan16_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 34, offsetof(mavlink_rc_channels_t, chan16_raw) }, \ + { "chan17_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_rc_channels_t, chan17_raw) }, \ + { "chan18_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 38, offsetof(mavlink_rc_channels_t, chan18_raw) }, \ { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_rc_channels_t, rssi) }, \ } \ } - +#endif /** * @brief Pack a rc_channels message @@ -69,89 +101,178 @@ typedef struct __mavlink_rc_channels_t * @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 Timestamp (milliseconds since system boot) - * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. - * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, uint8_t chancount, 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, uint8_t rssi) + uint32_t time_boot_ms, uint8_t chancount, 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, uint8_t rssi) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint16_t(buf, 4, chan1_raw); - _mav_put_uint16_t(buf, 6, chan2_raw); - _mav_put_uint16_t(buf, 8, chan3_raw); - _mav_put_uint16_t(buf, 10, chan4_raw); - _mav_put_uint16_t(buf, 12, chan5_raw); - _mav_put_uint16_t(buf, 14, chan6_raw); - _mav_put_uint16_t(buf, 16, chan7_raw); - _mav_put_uint16_t(buf, 18, chan8_raw); - _mav_put_uint16_t(buf, 20, chan9_raw); - _mav_put_uint16_t(buf, 22, chan10_raw); - _mav_put_uint16_t(buf, 24, chan11_raw); - _mav_put_uint16_t(buf, 26, chan12_raw); - _mav_put_uint16_t(buf, 28, chan13_raw); - _mav_put_uint16_t(buf, 30, chan14_raw); - _mav_put_uint16_t(buf, 32, chan15_raw); - _mav_put_uint16_t(buf, 34, chan16_raw); - _mav_put_uint16_t(buf, 36, chan17_raw); - _mav_put_uint16_t(buf, 38, chan18_raw); - _mav_put_uint8_t(buf, 40, chancount); - _mav_put_uint8_t(buf, 41, rssi); + char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); #else - mavlink_rc_channels_t packet; - packet.time_boot_ms = time_boot_ms; - packet.chan1_raw = chan1_raw; - packet.chan2_raw = chan2_raw; - packet.chan3_raw = chan3_raw; - packet.chan4_raw = chan4_raw; - packet.chan5_raw = chan5_raw; - packet.chan6_raw = chan6_raw; - packet.chan7_raw = chan7_raw; - packet.chan8_raw = chan8_raw; - 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; - packet.chancount = chancount; - packet.rssi = rssi; + mavlink_rc_channels_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + 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; + packet.chancount = chancount; + packet.rssi = rssi; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS; + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +} + +/** + * @brief Pack a rc_channels message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t chancount, 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, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#else + mavlink_rc_channels_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + 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; + packet.chancount = chancount; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RC_CHANNELS_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RC_CHANNELS_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_LEN); #endif } @@ -161,91 +282,87 @@ static inline uint16_t mavlink_msg_rc_channels_pack(uint8_t system_id, uint8_t c * @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 Timestamp (milliseconds since system boot) - * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. - * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,uint8_t chancount,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,uint8_t rssi) + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t chancount,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,uint8_t rssi) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint16_t(buf, 4, chan1_raw); - _mav_put_uint16_t(buf, 6, chan2_raw); - _mav_put_uint16_t(buf, 8, chan3_raw); - _mav_put_uint16_t(buf, 10, chan4_raw); - _mav_put_uint16_t(buf, 12, chan5_raw); - _mav_put_uint16_t(buf, 14, chan6_raw); - _mav_put_uint16_t(buf, 16, chan7_raw); - _mav_put_uint16_t(buf, 18, chan8_raw); - _mav_put_uint16_t(buf, 20, chan9_raw); - _mav_put_uint16_t(buf, 22, chan10_raw); - _mav_put_uint16_t(buf, 24, chan11_raw); - _mav_put_uint16_t(buf, 26, chan12_raw); - _mav_put_uint16_t(buf, 28, chan13_raw); - _mav_put_uint16_t(buf, 30, chan14_raw); - _mav_put_uint16_t(buf, 32, chan15_raw); - _mav_put_uint16_t(buf, 34, chan16_raw); - _mav_put_uint16_t(buf, 36, chan17_raw); - _mav_put_uint16_t(buf, 38, chan18_raw); - _mav_put_uint8_t(buf, 40, chancount); - _mav_put_uint8_t(buf, 41, rssi); + char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); #else - mavlink_rc_channels_t packet; - packet.time_boot_ms = time_boot_ms; - packet.chan1_raw = chan1_raw; - packet.chan2_raw = chan2_raw; - packet.chan3_raw = chan3_raw; - packet.chan4_raw = chan4_raw; - packet.chan5_raw = chan5_raw; - packet.chan6_raw = chan6_raw; - packet.chan7_raw = chan7_raw; - packet.chan8_raw = chan8_raw; - 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; - packet.chancount = chancount; - packet.rssi = rssi; + mavlink_rc_channels_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + 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; + packet.chancount = chancount; + packet.rssi = rssi; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); } /** @@ -258,7 +375,7 @@ static inline uint16_t mavlink_msg_rc_channels_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_rc_channels_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_t* rc_channels) { - return mavlink_msg_rc_channels_pack(system_id, component_id, msg, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi); + return mavlink_msg_rc_channels_pack(system_id, component_id, msg, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi); } /** @@ -272,103 +389,123 @@ static inline uint16_t mavlink_msg_rc_channels_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_rc_channels_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_t* rc_channels) { - return mavlink_msg_rc_channels_pack_chan(system_id, component_id, chan, msg, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi); + return mavlink_msg_rc_channels_pack_chan(system_id, component_id, chan, msg, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi); +} + +/** + * @brief Encode a rc_channels struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param rc_channels C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_rc_channels_t* rc_channels) +{ + return mavlink_msg_rc_channels_pack_status(system_id, component_id, _status, msg, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi); } /** * @brief Send a rc_channels message * @param chan MAVLink channel to send the message * - * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. - * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. - * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_rc_channels_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t chancount, 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, uint8_t rssi) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint16_t(buf, 4, chan1_raw); - _mav_put_uint16_t(buf, 6, chan2_raw); - _mav_put_uint16_t(buf, 8, chan3_raw); - _mav_put_uint16_t(buf, 10, chan4_raw); - _mav_put_uint16_t(buf, 12, chan5_raw); - _mav_put_uint16_t(buf, 14, chan6_raw); - _mav_put_uint16_t(buf, 16, chan7_raw); - _mav_put_uint16_t(buf, 18, chan8_raw); - _mav_put_uint16_t(buf, 20, chan9_raw); - _mav_put_uint16_t(buf, 22, chan10_raw); - _mav_put_uint16_t(buf, 24, chan11_raw); - _mav_put_uint16_t(buf, 26, chan12_raw); - _mav_put_uint16_t(buf, 28, chan13_raw); - _mav_put_uint16_t(buf, 30, chan14_raw); - _mav_put_uint16_t(buf, 32, chan15_raw); - _mav_put_uint16_t(buf, 34, chan16_raw); - _mav_put_uint16_t(buf, 36, chan17_raw); - _mav_put_uint16_t(buf, 38, chan18_raw); - _mav_put_uint8_t(buf, 40, chancount); - _mav_put_uint8_t(buf, 41, rssi); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); + char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); + mavlink_rc_channels_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + 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; + packet.chancount = chancount; + packet.rssi = rssi; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); #endif -#else - mavlink_rc_channels_t packet; - packet.time_boot_ms = time_boot_ms; - packet.chan1_raw = chan1_raw; - packet.chan2_raw = chan2_raw; - packet.chan3_raw = chan3_raw; - packet.chan4_raw = chan4_raw; - packet.chan5_raw = chan5_raw; - packet.chan6_raw = chan6_raw; - packet.chan7_raw = chan7_raw; - packet.chan8_raw = chan8_raw; - 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; - packet.chancount = chancount; - packet.rssi = rssi; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +/** + * @brief Send a rc_channels message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_rc_channels_send_struct(mavlink_channel_t chan, const mavlink_rc_channels_t* rc_channels) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_rc_channels_send(chan, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)rc_channels, MAVLINK_MSG_ID_RC_CHANNELS_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); #endif } #if MAVLINK_MSG_ID_RC_CHANNELS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -377,63 +514,55 @@ static inline void mavlink_msg_rc_channels_send(mavlink_channel_t chan, uint32_t static inline void mavlink_msg_rc_channels_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t chancount, 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, uint8_t rssi) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint16_t(buf, 4, chan1_raw); - _mav_put_uint16_t(buf, 6, chan2_raw); - _mav_put_uint16_t(buf, 8, chan3_raw); - _mav_put_uint16_t(buf, 10, chan4_raw); - _mav_put_uint16_t(buf, 12, chan5_raw); - _mav_put_uint16_t(buf, 14, chan6_raw); - _mav_put_uint16_t(buf, 16, chan7_raw); - _mav_put_uint16_t(buf, 18, chan8_raw); - _mav_put_uint16_t(buf, 20, chan9_raw); - _mav_put_uint16_t(buf, 22, chan10_raw); - _mav_put_uint16_t(buf, 24, chan11_raw); - _mav_put_uint16_t(buf, 26, chan12_raw); - _mav_put_uint16_t(buf, 28, chan13_raw); - _mav_put_uint16_t(buf, 30, chan14_raw); - _mav_put_uint16_t(buf, 32, chan15_raw); - _mav_put_uint16_t(buf, 34, chan16_raw); - _mav_put_uint16_t(buf, 36, chan17_raw); - _mav_put_uint16_t(buf, 38, chan18_raw); - _mav_put_uint8_t(buf, 40, chancount); - _mav_put_uint8_t(buf, 41, rssi); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); -#endif + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); #else - mavlink_rc_channels_t *packet = (mavlink_rc_channels_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->chan1_raw = chan1_raw; - packet->chan2_raw = chan2_raw; - packet->chan3_raw = chan3_raw; - packet->chan4_raw = chan4_raw; - packet->chan5_raw = chan5_raw; - packet->chan6_raw = chan6_raw; - packet->chan7_raw = chan7_raw; - packet->chan8_raw = chan8_raw; - 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; - packet->chancount = chancount; - packet->rssi = rssi; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); -#endif + mavlink_rc_channels_t *packet = (mavlink_rc_channels_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->chan1_raw = chan1_raw; + packet->chan2_raw = chan2_raw; + packet->chan3_raw = chan3_raw; + packet->chan4_raw = chan4_raw; + packet->chan5_raw = chan5_raw; + packet->chan6_raw = chan6_raw; + packet->chan7_raw = chan7_raw; + packet->chan8_raw = chan8_raw; + 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; + packet->chancount = chancount; + packet->rssi = rssi; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); #endif } #endif @@ -446,211 +575,211 @@ static inline void mavlink_msg_rc_channels_send_buf(mavlink_message_t *msgbuf, m /** * @brief Get field time_boot_ms from rc_channels message * - * @return Timestamp (milliseconds since system boot) + * @return Timestamp (milliseconds since system boot) */ static inline uint32_t mavlink_msg_rc_channels_get_time_boot_ms(const mavlink_message_t* msg) { - return _MAV_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** * @brief Get field chancount from rc_channels message * - * @return Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + * @return Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. */ static inline uint8_t mavlink_msg_rc_channels_get_chancount(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 40); + return _MAV_RETURN_uint8_t(msg, 40); } /** * @brief Get field chan1_raw from rc_channels message * - * @return RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan1_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 4); + return _MAV_RETURN_uint16_t(msg, 4); } /** * @brief Get field chan2_raw from rc_channels message * - * @return RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan2_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 6); + return _MAV_RETURN_uint16_t(msg, 6); } /** * @brief Get field chan3_raw from rc_channels message * - * @return RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan3_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 8); + return _MAV_RETURN_uint16_t(msg, 8); } /** * @brief Get field chan4_raw from rc_channels message * - * @return RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan4_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 10); + return _MAV_RETURN_uint16_t(msg, 10); } /** * @brief Get field chan5_raw from rc_channels message * - * @return RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan5_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 12); + return _MAV_RETURN_uint16_t(msg, 12); } /** * @brief Get field chan6_raw from rc_channels message * - * @return RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan6_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 14); + return _MAV_RETURN_uint16_t(msg, 14); } /** * @brief Get field chan7_raw from rc_channels message * - * @return RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan7_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 16); + return _MAV_RETURN_uint16_t(msg, 16); } /** * @brief Get field chan8_raw from rc_channels message * - * @return RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan8_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 18); + return _MAV_RETURN_uint16_t(msg, 18); } /** * @brief Get field chan9_raw from rc_channels message * - * @return RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan9_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 20); + return _MAV_RETURN_uint16_t(msg, 20); } /** * @brief Get field chan10_raw from rc_channels message * - * @return RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan10_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 22); + return _MAV_RETURN_uint16_t(msg, 22); } /** * @brief Get field chan11_raw from rc_channels message * - * @return RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan11_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 24); + return _MAV_RETURN_uint16_t(msg, 24); } /** * @brief Get field chan12_raw from rc_channels message * - * @return RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan12_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 26); + return _MAV_RETURN_uint16_t(msg, 26); } /** * @brief Get field chan13_raw from rc_channels message * - * @return RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan13_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 28); + return _MAV_RETURN_uint16_t(msg, 28); } /** * @brief Get field chan14_raw from rc_channels message * - * @return RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan14_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 30); + return _MAV_RETURN_uint16_t(msg, 30); } /** * @brief Get field chan15_raw from rc_channels message * - * @return RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan15_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 32); + return _MAV_RETURN_uint16_t(msg, 32); } /** * @brief Get field chan16_raw from rc_channels message * - * @return RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan16_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 34); + return _MAV_RETURN_uint16_t(msg, 34); } /** * @brief Get field chan17_raw from rc_channels message * - * @return RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan17_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 36); + return _MAV_RETURN_uint16_t(msg, 36); } /** * @brief Get field chan18_raw from rc_channels message * - * @return RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @return RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_get_chan18_raw(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 38); + return _MAV_RETURN_uint16_t(msg, 38); } /** * @brief Get field rssi from rc_channels message * - * @return Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + * @return Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. */ static inline uint8_t mavlink_msg_rc_channels_get_rssi(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 41); + return _MAV_RETURN_uint8_t(msg, 41); } /** @@ -661,29 +790,31 @@ static inline uint8_t mavlink_msg_rc_channels_get_rssi(const mavlink_message_t* */ static inline void mavlink_msg_rc_channels_decode(const mavlink_message_t* msg, mavlink_rc_channels_t* rc_channels) { -#if MAVLINK_NEED_BYTE_SWAP - rc_channels->time_boot_ms = mavlink_msg_rc_channels_get_time_boot_ms(msg); - rc_channels->chan1_raw = mavlink_msg_rc_channels_get_chan1_raw(msg); - rc_channels->chan2_raw = mavlink_msg_rc_channels_get_chan2_raw(msg); - rc_channels->chan3_raw = mavlink_msg_rc_channels_get_chan3_raw(msg); - rc_channels->chan4_raw = mavlink_msg_rc_channels_get_chan4_raw(msg); - rc_channels->chan5_raw = mavlink_msg_rc_channels_get_chan5_raw(msg); - rc_channels->chan6_raw = mavlink_msg_rc_channels_get_chan6_raw(msg); - rc_channels->chan7_raw = mavlink_msg_rc_channels_get_chan7_raw(msg); - rc_channels->chan8_raw = mavlink_msg_rc_channels_get_chan8_raw(msg); - rc_channels->chan9_raw = mavlink_msg_rc_channels_get_chan9_raw(msg); - rc_channels->chan10_raw = mavlink_msg_rc_channels_get_chan10_raw(msg); - rc_channels->chan11_raw = mavlink_msg_rc_channels_get_chan11_raw(msg); - rc_channels->chan12_raw = mavlink_msg_rc_channels_get_chan12_raw(msg); - rc_channels->chan13_raw = mavlink_msg_rc_channels_get_chan13_raw(msg); - rc_channels->chan14_raw = mavlink_msg_rc_channels_get_chan14_raw(msg); - rc_channels->chan15_raw = mavlink_msg_rc_channels_get_chan15_raw(msg); - rc_channels->chan16_raw = mavlink_msg_rc_channels_get_chan16_raw(msg); - rc_channels->chan17_raw = mavlink_msg_rc_channels_get_chan17_raw(msg); - rc_channels->chan18_raw = mavlink_msg_rc_channels_get_chan18_raw(msg); - rc_channels->chancount = mavlink_msg_rc_channels_get_chancount(msg); - rc_channels->rssi = mavlink_msg_rc_channels_get_rssi(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + rc_channels->time_boot_ms = mavlink_msg_rc_channels_get_time_boot_ms(msg); + rc_channels->chan1_raw = mavlink_msg_rc_channels_get_chan1_raw(msg); + rc_channels->chan2_raw = mavlink_msg_rc_channels_get_chan2_raw(msg); + rc_channels->chan3_raw = mavlink_msg_rc_channels_get_chan3_raw(msg); + rc_channels->chan4_raw = mavlink_msg_rc_channels_get_chan4_raw(msg); + rc_channels->chan5_raw = mavlink_msg_rc_channels_get_chan5_raw(msg); + rc_channels->chan6_raw = mavlink_msg_rc_channels_get_chan6_raw(msg); + rc_channels->chan7_raw = mavlink_msg_rc_channels_get_chan7_raw(msg); + rc_channels->chan8_raw = mavlink_msg_rc_channels_get_chan8_raw(msg); + rc_channels->chan9_raw = mavlink_msg_rc_channels_get_chan9_raw(msg); + rc_channels->chan10_raw = mavlink_msg_rc_channels_get_chan10_raw(msg); + rc_channels->chan11_raw = mavlink_msg_rc_channels_get_chan11_raw(msg); + rc_channels->chan12_raw = mavlink_msg_rc_channels_get_chan12_raw(msg); + rc_channels->chan13_raw = mavlink_msg_rc_channels_get_chan13_raw(msg); + rc_channels->chan14_raw = mavlink_msg_rc_channels_get_chan14_raw(msg); + rc_channels->chan15_raw = mavlink_msg_rc_channels_get_chan15_raw(msg); + rc_channels->chan16_raw = mavlink_msg_rc_channels_get_chan16_raw(msg); + rc_channels->chan17_raw = mavlink_msg_rc_channels_get_chan17_raw(msg); + rc_channels->chan18_raw = mavlink_msg_rc_channels_get_chan18_raw(msg); + rc_channels->chancount = mavlink_msg_rc_channels_get_chancount(msg); + rc_channels->rssi = mavlink_msg_rc_channels_get_rssi(msg); #else - memcpy(rc_channels, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RC_CHANNELS_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_RC_CHANNELS_LEN? msg->len : MAVLINK_MSG_ID_RC_CHANNELS_LEN; + memset(rc_channels, 0, MAVLINK_MSG_ID_RC_CHANNELS_LEN); + memcpy(rc_channels, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_aux_cmd.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_aux_cmd.h index 730c2d93..25efd027 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_aux_cmd.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_aux_cmd.h @@ -1,15 +1,18 @@ +#pragma once // MESSAGE ROSFLIGHT_AUX_CMD PACKING #define MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD 193 -typedef struct __mavlink_rosflight_aux_cmd_t -{ - float aux_cmd_array[14]; /*< */ - uint8_t type_array[14]; /*< */ + +typedef struct __mavlink_rosflight_aux_cmd_t { + float aux_cmd_array[14]; /*< */ + uint8_t type_array[14]; /*< */ } mavlink_rosflight_aux_cmd_t; #define MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN 70 +#define MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_MIN_LEN 70 #define MAVLINK_MSG_ID_193_LEN 70 +#define MAVLINK_MSG_ID_193_MIN_LEN 70 #define MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_CRC 1 #define MAVLINK_MSG_ID_193_CRC 1 @@ -17,14 +20,24 @@ typedef struct __mavlink_rosflight_aux_cmd_t #define MAVLINK_MSG_ROSFLIGHT_AUX_CMD_FIELD_AUX_CMD_ARRAY_LEN 14 #define MAVLINK_MSG_ROSFLIGHT_AUX_CMD_FIELD_TYPE_ARRAY_LEN 14 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_ROSFLIGHT_AUX_CMD { \ - "ROSFLIGHT_AUX_CMD", \ - 2, \ - { { "aux_cmd_array", NULL, MAVLINK_TYPE_FLOAT, 14, 0, offsetof(mavlink_rosflight_aux_cmd_t, aux_cmd_array) }, \ - { "type_array", NULL, MAVLINK_TYPE_UINT8_T, 14, 56, offsetof(mavlink_rosflight_aux_cmd_t, type_array) }, \ + 193, \ + "ROSFLIGHT_AUX_CMD", \ + 2, \ + { { "type_array", NULL, MAVLINK_TYPE_UINT8_T, 14, 56, offsetof(mavlink_rosflight_aux_cmd_t, type_array) }, \ + { "aux_cmd_array", NULL, MAVLINK_TYPE_FLOAT, 14, 0, offsetof(mavlink_rosflight_aux_cmd_t, aux_cmd_array) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_ROSFLIGHT_AUX_CMD { \ + "ROSFLIGHT_AUX_CMD", \ + 2, \ + { { "type_array", NULL, MAVLINK_TYPE_UINT8_T, 14, 56, offsetof(mavlink_rosflight_aux_cmd_t, type_array) }, \ + { "aux_cmd_array", NULL, MAVLINK_TYPE_FLOAT, 14, 0, offsetof(mavlink_rosflight_aux_cmd_t, aux_cmd_array) }, \ + } \ +} +#endif /** * @brief Pack a rosflight_aux_cmd message @@ -32,32 +45,64 @@ typedef struct __mavlink_rosflight_aux_cmd_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param type_array - * @param aux_cmd_array + * @param type_array + * @param aux_cmd_array * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_aux_cmd_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const uint8_t *type_array, const float *aux_cmd_array) + const uint8_t *type_array, const float *aux_cmd_array) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN]; + + _mav_put_float_array(buf, 0, aux_cmd_array, 14); + _mav_put_uint8_t_array(buf, 56, type_array, 14); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN); +#else + mavlink_rosflight_aux_cmd_t packet; + + mav_array_memcpy(packet.aux_cmd_array, aux_cmd_array, sizeof(float)*14); + mav_array_memcpy(packet.type_array, type_array, sizeof(uint8_t)*14); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_CRC); +} + +/** + * @brief Pack a rosflight_aux_cmd message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param type_array + * @param aux_cmd_array + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rosflight_aux_cmd_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + const uint8_t *type_array, const float *aux_cmd_array) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN]; + char buf[MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN]; - _mav_put_float_array(buf, 0, aux_cmd_array, 14); - _mav_put_uint8_t_array(buf, 56, type_array, 14); + _mav_put_float_array(buf, 0, aux_cmd_array, 14); + _mav_put_uint8_t_array(buf, 56, type_array, 14); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN); #else - mavlink_rosflight_aux_cmd_t packet; + mavlink_rosflight_aux_cmd_t packet; - mav_array_memcpy(packet.aux_cmd_array, aux_cmd_array, sizeof(float)*14); - mav_array_memcpy(packet.type_array, type_array, sizeof(uint8_t)*14); + mav_array_memcpy(packet.aux_cmd_array, aux_cmd_array, sizeof(float)*14); + mav_array_memcpy(packet.type_array, type_array, sizeof(uint8_t)*14); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD; + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN); #endif } @@ -67,34 +112,30 @@ static inline uint16_t mavlink_msg_rosflight_aux_cmd_pack(uint8_t system_id, uin * @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 type_array - * @param aux_cmd_array + * @param type_array + * @param aux_cmd_array * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_aux_cmd_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const uint8_t *type_array,const float *aux_cmd_array) + mavlink_message_t* msg, + const uint8_t *type_array,const float *aux_cmd_array) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN]; + char buf[MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN]; - _mav_put_float_array(buf, 0, aux_cmd_array, 14); - _mav_put_uint8_t_array(buf, 56, type_array, 14); + _mav_put_float_array(buf, 0, aux_cmd_array, 14); + _mav_put_uint8_t_array(buf, 56, type_array, 14); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN); #else - mavlink_rosflight_aux_cmd_t packet; + mavlink_rosflight_aux_cmd_t packet; - mav_array_memcpy(packet.aux_cmd_array, aux_cmd_array, sizeof(float)*14); - mav_array_memcpy(packet.type_array, type_array, sizeof(uint8_t)*14); + mav_array_memcpy(packet.aux_cmd_array, aux_cmd_array, sizeof(float)*14); + mav_array_memcpy(packet.type_array, type_array, sizeof(uint8_t)*14); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_CRC); } /** @@ -107,7 +148,7 @@ static inline uint16_t mavlink_msg_rosflight_aux_cmd_pack_chan(uint8_t system_id */ static inline uint16_t mavlink_msg_rosflight_aux_cmd_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rosflight_aux_cmd_t* rosflight_aux_cmd) { - return mavlink_msg_rosflight_aux_cmd_pack(system_id, component_id, msg, rosflight_aux_cmd->type_array, rosflight_aux_cmd->aux_cmd_array); + return mavlink_msg_rosflight_aux_cmd_pack(system_id, component_id, msg, rosflight_aux_cmd->type_array, rosflight_aux_cmd->aux_cmd_array); } /** @@ -121,46 +162,66 @@ static inline uint16_t mavlink_msg_rosflight_aux_cmd_encode(uint8_t system_id, u */ static inline uint16_t mavlink_msg_rosflight_aux_cmd_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rosflight_aux_cmd_t* rosflight_aux_cmd) { - return mavlink_msg_rosflight_aux_cmd_pack_chan(system_id, component_id, chan, msg, rosflight_aux_cmd->type_array, rosflight_aux_cmd->aux_cmd_array); + return mavlink_msg_rosflight_aux_cmd_pack_chan(system_id, component_id, chan, msg, rosflight_aux_cmd->type_array, rosflight_aux_cmd->aux_cmd_array); +} + +/** + * @brief Encode a rosflight_aux_cmd struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param rosflight_aux_cmd C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rosflight_aux_cmd_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_rosflight_aux_cmd_t* rosflight_aux_cmd) +{ + return mavlink_msg_rosflight_aux_cmd_pack_status(system_id, component_id, _status, msg, rosflight_aux_cmd->type_array, rosflight_aux_cmd->aux_cmd_array); } /** * @brief Send a rosflight_aux_cmd message * @param chan MAVLink channel to send the message * - * @param type_array - * @param aux_cmd_array + * @param type_array + * @param aux_cmd_array */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_rosflight_aux_cmd_send(mavlink_channel_t chan, const uint8_t *type_array, const float *aux_cmd_array) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN]; + char buf[MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN]; - _mav_put_float_array(buf, 0, aux_cmd_array, 14); - _mav_put_uint8_t_array(buf, 56, type_array, 14); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD, buf, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_CRC); + _mav_put_float_array(buf, 0, aux_cmd_array, 14); + _mav_put_uint8_t_array(buf, 56, type_array, 14); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD, buf, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD, buf, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN); + mavlink_rosflight_aux_cmd_t packet; + + mav_array_memcpy(packet.aux_cmd_array, aux_cmd_array, sizeof(float)*14); + mav_array_memcpy(packet.type_array, type_array, sizeof(uint8_t)*14); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_CRC); #endif -#else - mavlink_rosflight_aux_cmd_t packet; +} - mav_array_memcpy(packet.aux_cmd_array, aux_cmd_array, sizeof(float)*14); - mav_array_memcpy(packet.type_array, type_array, sizeof(uint8_t)*14); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_CRC); +/** + * @brief Send a rosflight_aux_cmd message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_rosflight_aux_cmd_send_struct(mavlink_channel_t chan, const mavlink_rosflight_aux_cmd_t* rosflight_aux_cmd) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_rosflight_aux_cmd_send(chan, rosflight_aux_cmd->type_array, rosflight_aux_cmd->aux_cmd_array); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD, (const char *)rosflight_aux_cmd, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_CRC); #endif } #if MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -169,25 +230,17 @@ static inline void mavlink_msg_rosflight_aux_cmd_send(mavlink_channel_t chan, co static inline void mavlink_msg_rosflight_aux_cmd_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const uint8_t *type_array, const float *aux_cmd_array) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; + char *buf = (char *)msgbuf; - _mav_put_float_array(buf, 0, aux_cmd_array, 14); - _mav_put_uint8_t_array(buf, 56, type_array, 14); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD, buf, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD, buf, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN); -#endif + _mav_put_float_array(buf, 0, aux_cmd_array, 14); + _mav_put_uint8_t_array(buf, 56, type_array, 14); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD, buf, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_CRC); #else - mavlink_rosflight_aux_cmd_t *packet = (mavlink_rosflight_aux_cmd_t *)msgbuf; + mavlink_rosflight_aux_cmd_t *packet = (mavlink_rosflight_aux_cmd_t *)msgbuf; - mav_array_memcpy(packet->aux_cmd_array, aux_cmd_array, sizeof(float)*14); - mav_array_memcpy(packet->type_array, type_array, sizeof(uint8_t)*14); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN); -#endif + mav_array_memcpy(packet->aux_cmd_array, aux_cmd_array, sizeof(float)*14); + mav_array_memcpy(packet->type_array, type_array, sizeof(uint8_t)*14); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_CRC); #endif } #endif @@ -200,21 +253,21 @@ static inline void mavlink_msg_rosflight_aux_cmd_send_buf(mavlink_message_t *msg /** * @brief Get field type_array from rosflight_aux_cmd message * - * @return + * @return */ static inline uint16_t mavlink_msg_rosflight_aux_cmd_get_type_array(const mavlink_message_t* msg, uint8_t *type_array) { - return _MAV_RETURN_uint8_t_array(msg, type_array, 14, 56); + return _MAV_RETURN_uint8_t_array(msg, type_array, 14, 56); } /** * @brief Get field aux_cmd_array from rosflight_aux_cmd message * - * @return + * @return */ static inline uint16_t mavlink_msg_rosflight_aux_cmd_get_aux_cmd_array(const mavlink_message_t* msg, float *aux_cmd_array) { - return _MAV_RETURN_float_array(msg, aux_cmd_array, 14, 0); + return _MAV_RETURN_float_array(msg, aux_cmd_array, 14, 0); } /** @@ -225,10 +278,12 @@ static inline uint16_t mavlink_msg_rosflight_aux_cmd_get_aux_cmd_array(const mav */ static inline void mavlink_msg_rosflight_aux_cmd_decode(const mavlink_message_t* msg, mavlink_rosflight_aux_cmd_t* rosflight_aux_cmd) { -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_rosflight_aux_cmd_get_aux_cmd_array(msg, rosflight_aux_cmd->aux_cmd_array); - mavlink_msg_rosflight_aux_cmd_get_type_array(msg, rosflight_aux_cmd->type_array); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_rosflight_aux_cmd_get_aux_cmd_array(msg, rosflight_aux_cmd->aux_cmd_array); + mavlink_msg_rosflight_aux_cmd_get_type_array(msg, rosflight_aux_cmd->type_array); #else - memcpy(rosflight_aux_cmd, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN? msg->len : MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN; + memset(rosflight_aux_cmd, 0, MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_LEN); + memcpy(rosflight_aux_cmd, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_battery_status.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_battery_status.h index 7bd87a64..12b44144 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_battery_status.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_battery_status.h @@ -1,29 +1,42 @@ +#pragma once // MESSAGE ROSFLIGHT_BATTERY_STATUS PACKING #define MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS 199 -typedef struct __mavlink_rosflight_battery_status_t -{ - float battery_voltage; /*< */ - float battery_current; /*< */ + +typedef struct __mavlink_rosflight_battery_status_t { + float battery_voltage; /*< */ + float battery_current; /*< */ } mavlink_rosflight_battery_status_t; #define MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN 8 +#define MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_MIN_LEN 8 #define MAVLINK_MSG_ID_199_LEN 8 +#define MAVLINK_MSG_ID_199_MIN_LEN 8 #define MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_CRC 48 #define MAVLINK_MSG_ID_199_CRC 48 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_ROSFLIGHT_BATTERY_STATUS { \ - "ROSFLIGHT_BATTERY_STATUS", \ - 2, \ - { { "battery_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_rosflight_battery_status_t, battery_voltage) }, \ + 199, \ + "ROSFLIGHT_BATTERY_STATUS", \ + 2, \ + { { "battery_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_rosflight_battery_status_t, battery_voltage) }, \ { "battery_current", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_rosflight_battery_status_t, battery_current) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_ROSFLIGHT_BATTERY_STATUS { \ + "ROSFLIGHT_BATTERY_STATUS", \ + 2, \ + { { "battery_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_rosflight_battery_status_t, battery_voltage) }, \ + { "battery_current", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_rosflight_battery_status_t, battery_current) }, \ + } \ +} +#endif /** * @brief Pack a rosflight_battery_status message @@ -31,32 +44,64 @@ typedef struct __mavlink_rosflight_battery_status_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param battery_voltage - * @param battery_current + * @param battery_voltage + * @param battery_current * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_battery_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float battery_voltage, float battery_current) + float battery_voltage, float battery_current) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN]; + _mav_put_float(buf, 0, battery_voltage); + _mav_put_float(buf, 4, battery_current); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN); +#else + mavlink_rosflight_battery_status_t packet; + packet.battery_voltage = battery_voltage; + packet.battery_current = battery_current; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_CRC); +} + +/** + * @brief Pack a rosflight_battery_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param battery_voltage + * @param battery_current + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rosflight_battery_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + float battery_voltage, float battery_current) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN]; - _mav_put_float(buf, 0, battery_voltage); - _mav_put_float(buf, 4, battery_current); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN]; + _mav_put_float(buf, 0, battery_voltage); + _mav_put_float(buf, 4, battery_current); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN); #else - mavlink_rosflight_battery_status_t packet; - packet.battery_voltage = battery_voltage; - packet.battery_current = battery_current; + mavlink_rosflight_battery_status_t packet; + packet.battery_voltage = battery_voltage; + packet.battery_current = battery_current; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS; + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN); #endif } @@ -66,34 +111,30 @@ static inline uint16_t mavlink_msg_rosflight_battery_status_pack(uint8_t 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 battery_voltage - * @param battery_current + * @param battery_voltage + * @param battery_current * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_battery_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float battery_voltage,float battery_current) + mavlink_message_t* msg, + float battery_voltage,float battery_current) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN]; - _mav_put_float(buf, 0, battery_voltage); - _mav_put_float(buf, 4, battery_current); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN]; + _mav_put_float(buf, 0, battery_voltage); + _mav_put_float(buf, 4, battery_current); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN); #else - mavlink_rosflight_battery_status_t packet; - packet.battery_voltage = battery_voltage; - packet.battery_current = battery_current; + mavlink_rosflight_battery_status_t packet; + packet.battery_voltage = battery_voltage; + packet.battery_current = battery_current; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_CRC); } /** @@ -106,7 +147,7 @@ static inline uint16_t mavlink_msg_rosflight_battery_status_pack_chan(uint8_t sy */ static inline uint16_t mavlink_msg_rosflight_battery_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rosflight_battery_status_t* rosflight_battery_status) { - return mavlink_msg_rosflight_battery_status_pack(system_id, component_id, msg, rosflight_battery_status->battery_voltage, rosflight_battery_status->battery_current); + return mavlink_msg_rosflight_battery_status_pack(system_id, component_id, msg, rosflight_battery_status->battery_voltage, rosflight_battery_status->battery_current); } /** @@ -120,46 +161,66 @@ static inline uint16_t mavlink_msg_rosflight_battery_status_encode(uint8_t syste */ static inline uint16_t mavlink_msg_rosflight_battery_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rosflight_battery_status_t* rosflight_battery_status) { - return mavlink_msg_rosflight_battery_status_pack_chan(system_id, component_id, chan, msg, rosflight_battery_status->battery_voltage, rosflight_battery_status->battery_current); + return mavlink_msg_rosflight_battery_status_pack_chan(system_id, component_id, chan, msg, rosflight_battery_status->battery_voltage, rosflight_battery_status->battery_current); +} + +/** + * @brief Encode a rosflight_battery_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param rosflight_battery_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rosflight_battery_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_rosflight_battery_status_t* rosflight_battery_status) +{ + return mavlink_msg_rosflight_battery_status_pack_status(system_id, component_id, _status, msg, rosflight_battery_status->battery_voltage, rosflight_battery_status->battery_current); } /** * @brief Send a rosflight_battery_status message * @param chan MAVLink channel to send the message * - * @param battery_voltage - * @param battery_current + * @param battery_voltage + * @param battery_current */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_rosflight_battery_status_send(mavlink_channel_t chan, float battery_voltage, float battery_current) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN]; - _mav_put_float(buf, 0, battery_voltage); - _mav_put_float(buf, 4, battery_current); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN]; + _mav_put_float(buf, 0, battery_voltage); + _mav_put_float(buf, 4, battery_current); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS, buf, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_CRC); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS, buf, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS, buf, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN); + mavlink_rosflight_battery_status_t packet; + packet.battery_voltage = battery_voltage; + packet.battery_current = battery_current; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_CRC); #endif -#else - mavlink_rosflight_battery_status_t packet; - packet.battery_voltage = battery_voltage; - packet.battery_current = battery_current; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_CRC); +/** + * @brief Send a rosflight_battery_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_rosflight_battery_status_send_struct(mavlink_channel_t chan, const mavlink_rosflight_battery_status_t* rosflight_battery_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_rosflight_battery_status_send(chan, rosflight_battery_status->battery_voltage, rosflight_battery_status->battery_current); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS, (const char *)rosflight_battery_status, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_CRC); #endif } #if MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -168,25 +229,17 @@ static inline void mavlink_msg_rosflight_battery_status_send(mavlink_channel_t c static inline void mavlink_msg_rosflight_battery_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float battery_voltage, float battery_current) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, battery_voltage); - _mav_put_float(buf, 4, battery_current); + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, battery_voltage); + _mav_put_float(buf, 4, battery_current); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS, buf, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS, buf, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS, buf, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_CRC); #else - mavlink_rosflight_battery_status_t *packet = (mavlink_rosflight_battery_status_t *)msgbuf; - packet->battery_voltage = battery_voltage; - packet->battery_current = battery_current; + mavlink_rosflight_battery_status_t *packet = (mavlink_rosflight_battery_status_t *)msgbuf; + packet->battery_voltage = battery_voltage; + packet->battery_current = battery_current; -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_CRC); #endif } #endif @@ -199,21 +252,21 @@ static inline void mavlink_msg_rosflight_battery_status_send_buf(mavlink_message /** * @brief Get field battery_voltage from rosflight_battery_status message * - * @return + * @return */ static inline float mavlink_msg_rosflight_battery_status_get_battery_voltage(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** * @brief Get field battery_current from rosflight_battery_status message * - * @return + * @return */ static inline float mavlink_msg_rosflight_battery_status_get_battery_current(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -224,10 +277,12 @@ static inline float mavlink_msg_rosflight_battery_status_get_battery_current(con */ static inline void mavlink_msg_rosflight_battery_status_decode(const mavlink_message_t* msg, mavlink_rosflight_battery_status_t* rosflight_battery_status) { -#if MAVLINK_NEED_BYTE_SWAP - rosflight_battery_status->battery_voltage = mavlink_msg_rosflight_battery_status_get_battery_voltage(msg); - rosflight_battery_status->battery_current = mavlink_msg_rosflight_battery_status_get_battery_current(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + rosflight_battery_status->battery_voltage = mavlink_msg_rosflight_battery_status_get_battery_voltage(msg); + rosflight_battery_status->battery_current = mavlink_msg_rosflight_battery_status_get_battery_current(msg); #else - memcpy(rosflight_battery_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN? msg->len : MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN; + memset(rosflight_battery_status, 0, MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_LEN); + memcpy(rosflight_battery_status, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_cmd.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_cmd.h index 281fd657..d72e1655 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_cmd.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_cmd.h @@ -1,27 +1,39 @@ +#pragma once // MESSAGE ROSFLIGHT_CMD PACKING #define MAVLINK_MSG_ID_ROSFLIGHT_CMD 188 -typedef struct __mavlink_rosflight_cmd_t -{ - uint8_t command; /*< */ + +typedef struct __mavlink_rosflight_cmd_t { + uint8_t command; /*< */ } mavlink_rosflight_cmd_t; #define MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN 1 +#define MAVLINK_MSG_ID_ROSFLIGHT_CMD_MIN_LEN 1 #define MAVLINK_MSG_ID_188_LEN 1 +#define MAVLINK_MSG_ID_188_MIN_LEN 1 #define MAVLINK_MSG_ID_ROSFLIGHT_CMD_CRC 249 #define MAVLINK_MSG_ID_188_CRC 249 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_ROSFLIGHT_CMD { \ - "ROSFLIGHT_CMD", \ - 1, \ - { { "command", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_rosflight_cmd_t, command) }, \ + 188, \ + "ROSFLIGHT_CMD", \ + 1, \ + { { "command", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_rosflight_cmd_t, command) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_ROSFLIGHT_CMD { \ + "ROSFLIGHT_CMD", \ + 1, \ + { { "command", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_rosflight_cmd_t, command) }, \ + } \ +} +#endif /** * @brief Pack a rosflight_cmd message @@ -29,29 +41,58 @@ typedef struct __mavlink_rosflight_cmd_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param command + * @param command * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_cmd_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t command) + uint8_t command) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN]; + _mav_put_uint8_t(buf, 0, command); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN); +#else + mavlink_rosflight_cmd_t packet; + packet.command = command; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_CMD; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_CRC); +} + +/** + * @brief Pack a rosflight_cmd message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param command + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rosflight_cmd_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t command) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN]; - _mav_put_uint8_t(buf, 0, command); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN]; + _mav_put_uint8_t(buf, 0, command); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN); #else - mavlink_rosflight_cmd_t packet; - packet.command = command; + mavlink_rosflight_cmd_t packet; + packet.command = command; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_CMD; + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_CMD; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN); #endif } @@ -61,31 +102,27 @@ static inline uint16_t mavlink_msg_rosflight_cmd_pack(uint8_t system_id, uint8_t * @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 command + * @param command * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_cmd_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t command) + mavlink_message_t* msg, + uint8_t command) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN]; - _mav_put_uint8_t(buf, 0, command); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN]; + _mav_put_uint8_t(buf, 0, command); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN); #else - mavlink_rosflight_cmd_t packet; - packet.command = command; + mavlink_rosflight_cmd_t packet; + packet.command = command; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_CMD; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_CMD; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_CRC); } /** @@ -98,7 +135,7 @@ static inline uint16_t mavlink_msg_rosflight_cmd_pack_chan(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_rosflight_cmd_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rosflight_cmd_t* rosflight_cmd) { - return mavlink_msg_rosflight_cmd_pack(system_id, component_id, msg, rosflight_cmd->command); + return mavlink_msg_rosflight_cmd_pack(system_id, component_id, msg, rosflight_cmd->command); } /** @@ -112,43 +149,63 @@ static inline uint16_t mavlink_msg_rosflight_cmd_encode(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_rosflight_cmd_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rosflight_cmd_t* rosflight_cmd) { - return mavlink_msg_rosflight_cmd_pack_chan(system_id, component_id, chan, msg, rosflight_cmd->command); + return mavlink_msg_rosflight_cmd_pack_chan(system_id, component_id, chan, msg, rosflight_cmd->command); +} + +/** + * @brief Encode a rosflight_cmd struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param rosflight_cmd C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rosflight_cmd_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_rosflight_cmd_t* rosflight_cmd) +{ + return mavlink_msg_rosflight_cmd_pack_status(system_id, component_id, _status, msg, rosflight_cmd->command); } /** * @brief Send a rosflight_cmd message * @param chan MAVLink channel to send the message * - * @param command + * @param command */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_rosflight_cmd_send(mavlink_channel_t chan, uint8_t command) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN]; - _mav_put_uint8_t(buf, 0, command); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN]; + _mav_put_uint8_t(buf, 0, command); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD, buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_CRC); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD, buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD, buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN); + mavlink_rosflight_cmd_t packet; + packet.command = command; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_CRC); #endif -#else - mavlink_rosflight_cmd_t packet; - packet.command = command; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_CRC); +/** + * @brief Send a rosflight_cmd message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_rosflight_cmd_send_struct(mavlink_channel_t chan, const mavlink_rosflight_cmd_t* rosflight_cmd) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_rosflight_cmd_send(chan, rosflight_cmd->command); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD, (const char *)rosflight_cmd, MAVLINK_MSG_ID_ROSFLIGHT_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_CRC); #endif } #if MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -157,23 +214,15 @@ static inline void mavlink_msg_rosflight_cmd_send(mavlink_channel_t chan, uint8_ static inline void mavlink_msg_rosflight_cmd_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t command) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, command); + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, command); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD, buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD, buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD, buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_CRC); #else - mavlink_rosflight_cmd_t *packet = (mavlink_rosflight_cmd_t *)msgbuf; - packet->command = command; + mavlink_rosflight_cmd_t *packet = (mavlink_rosflight_cmd_t *)msgbuf; + packet->command = command; -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_CRC); #endif } #endif @@ -186,11 +235,11 @@ static inline void mavlink_msg_rosflight_cmd_send_buf(mavlink_message_t *msgbuf, /** * @brief Get field command from rosflight_cmd message * - * @return + * @return */ static inline uint8_t mavlink_msg_rosflight_cmd_get_command(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 0); + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -201,9 +250,11 @@ static inline uint8_t mavlink_msg_rosflight_cmd_get_command(const mavlink_messag */ static inline void mavlink_msg_rosflight_cmd_decode(const mavlink_message_t* msg, mavlink_rosflight_cmd_t* rosflight_cmd) { -#if MAVLINK_NEED_BYTE_SWAP - rosflight_cmd->command = mavlink_msg_rosflight_cmd_get_command(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + rosflight_cmd->command = mavlink_msg_rosflight_cmd_get_command(msg); #else - memcpy(rosflight_cmd, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN? msg->len : MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN; + memset(rosflight_cmd, 0, MAVLINK_MSG_ID_ROSFLIGHT_CMD_LEN); + memcpy(rosflight_cmd, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_cmd_ack.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_cmd_ack.h index 4e8d931f..22b035cc 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_cmd_ack.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_cmd_ack.h @@ -1,29 +1,42 @@ +#pragma once // MESSAGE ROSFLIGHT_CMD_ACK PACKING #define MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK 189 -typedef struct __mavlink_rosflight_cmd_ack_t -{ - uint8_t command; /*< */ - uint8_t success; /*< */ + +typedef struct __mavlink_rosflight_cmd_ack_t { + uint8_t command; /*< */ + uint8_t success; /*< */ } mavlink_rosflight_cmd_ack_t; #define MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN 2 +#define MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_MIN_LEN 2 #define MAVLINK_MSG_ID_189_LEN 2 +#define MAVLINK_MSG_ID_189_MIN_LEN 2 #define MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_CRC 113 #define MAVLINK_MSG_ID_189_CRC 113 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_ROSFLIGHT_CMD_ACK { \ - "ROSFLIGHT_CMD_ACK", \ - 2, \ - { { "command", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_rosflight_cmd_ack_t, command) }, \ + 189, \ + "ROSFLIGHT_CMD_ACK", \ + 2, \ + { { "command", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_rosflight_cmd_ack_t, command) }, \ { "success", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_rosflight_cmd_ack_t, success) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_ROSFLIGHT_CMD_ACK { \ + "ROSFLIGHT_CMD_ACK", \ + 2, \ + { { "command", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_rosflight_cmd_ack_t, command) }, \ + { "success", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_rosflight_cmd_ack_t, success) }, \ + } \ +} +#endif /** * @brief Pack a rosflight_cmd_ack message @@ -31,32 +44,64 @@ typedef struct __mavlink_rosflight_cmd_ack_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param command - * @param success + * @param command + * @param success * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_cmd_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t command, uint8_t success) + uint8_t command, uint8_t success) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN]; + _mav_put_uint8_t(buf, 0, command); + _mav_put_uint8_t(buf, 1, success); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN); +#else + mavlink_rosflight_cmd_ack_t packet; + packet.command = command; + packet.success = success; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_CRC); +} + +/** + * @brief Pack a rosflight_cmd_ack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param command + * @param success + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rosflight_cmd_ack_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t command, uint8_t success) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN]; - _mav_put_uint8_t(buf, 0, command); - _mav_put_uint8_t(buf, 1, success); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN]; + _mav_put_uint8_t(buf, 0, command); + _mav_put_uint8_t(buf, 1, success); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN); #else - mavlink_rosflight_cmd_ack_t packet; - packet.command = command; - packet.success = success; + mavlink_rosflight_cmd_ack_t packet; + packet.command = command; + packet.success = success; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK; + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN); #endif } @@ -66,34 +111,30 @@ static inline uint16_t mavlink_msg_rosflight_cmd_ack_pack(uint8_t system_id, uin * @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 command - * @param success + * @param command + * @param success * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_cmd_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t command,uint8_t success) + mavlink_message_t* msg, + uint8_t command,uint8_t success) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN]; - _mav_put_uint8_t(buf, 0, command); - _mav_put_uint8_t(buf, 1, success); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN]; + _mav_put_uint8_t(buf, 0, command); + _mav_put_uint8_t(buf, 1, success); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN); #else - mavlink_rosflight_cmd_ack_t packet; - packet.command = command; - packet.success = success; + mavlink_rosflight_cmd_ack_t packet; + packet.command = command; + packet.success = success; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_CRC); } /** @@ -106,7 +147,7 @@ static inline uint16_t mavlink_msg_rosflight_cmd_ack_pack_chan(uint8_t system_id */ static inline uint16_t mavlink_msg_rosflight_cmd_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rosflight_cmd_ack_t* rosflight_cmd_ack) { - return mavlink_msg_rosflight_cmd_ack_pack(system_id, component_id, msg, rosflight_cmd_ack->command, rosflight_cmd_ack->success); + return mavlink_msg_rosflight_cmd_ack_pack(system_id, component_id, msg, rosflight_cmd_ack->command, rosflight_cmd_ack->success); } /** @@ -120,46 +161,66 @@ static inline uint16_t mavlink_msg_rosflight_cmd_ack_encode(uint8_t system_id, u */ static inline uint16_t mavlink_msg_rosflight_cmd_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rosflight_cmd_ack_t* rosflight_cmd_ack) { - return mavlink_msg_rosflight_cmd_ack_pack_chan(system_id, component_id, chan, msg, rosflight_cmd_ack->command, rosflight_cmd_ack->success); + return mavlink_msg_rosflight_cmd_ack_pack_chan(system_id, component_id, chan, msg, rosflight_cmd_ack->command, rosflight_cmd_ack->success); +} + +/** + * @brief Encode a rosflight_cmd_ack struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param rosflight_cmd_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rosflight_cmd_ack_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_rosflight_cmd_ack_t* rosflight_cmd_ack) +{ + return mavlink_msg_rosflight_cmd_ack_pack_status(system_id, component_id, _status, msg, rosflight_cmd_ack->command, rosflight_cmd_ack->success); } /** * @brief Send a rosflight_cmd_ack message * @param chan MAVLink channel to send the message * - * @param command - * @param success + * @param command + * @param success */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_rosflight_cmd_ack_send(mavlink_channel_t chan, uint8_t command, uint8_t success) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN]; - _mav_put_uint8_t(buf, 0, command); - _mav_put_uint8_t(buf, 1, success); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN]; + _mav_put_uint8_t(buf, 0, command); + _mav_put_uint8_t(buf, 1, success); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK, buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_CRC); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK, buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK, buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN); + mavlink_rosflight_cmd_ack_t packet; + packet.command = command; + packet.success = success; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_CRC); #endif -#else - mavlink_rosflight_cmd_ack_t packet; - packet.command = command; - packet.success = success; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_CRC); +/** + * @brief Send a rosflight_cmd_ack message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_rosflight_cmd_ack_send_struct(mavlink_channel_t chan, const mavlink_rosflight_cmd_ack_t* rosflight_cmd_ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_rosflight_cmd_ack_send(chan, rosflight_cmd_ack->command, rosflight_cmd_ack->success); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK, (const char *)rosflight_cmd_ack, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_CRC); #endif } #if MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -168,25 +229,17 @@ static inline void mavlink_msg_rosflight_cmd_ack_send(mavlink_channel_t chan, ui static inline void mavlink_msg_rosflight_cmd_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t command, uint8_t success) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, command); - _mav_put_uint8_t(buf, 1, success); + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, command); + _mav_put_uint8_t(buf, 1, success); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK, buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK, buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK, buf, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_CRC); #else - mavlink_rosflight_cmd_ack_t *packet = (mavlink_rosflight_cmd_ack_t *)msgbuf; - packet->command = command; - packet->success = success; + mavlink_rosflight_cmd_ack_t *packet = (mavlink_rosflight_cmd_ack_t *)msgbuf; + packet->command = command; + packet->success = success; -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_CRC); #endif } #endif @@ -199,21 +252,21 @@ static inline void mavlink_msg_rosflight_cmd_ack_send_buf(mavlink_message_t *msg /** * @brief Get field command from rosflight_cmd_ack message * - * @return + * @return */ static inline uint8_t mavlink_msg_rosflight_cmd_ack_get_command(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 0); + return _MAV_RETURN_uint8_t(msg, 0); } /** * @brief Get field success from rosflight_cmd_ack message * - * @return + * @return */ static inline uint8_t mavlink_msg_rosflight_cmd_ack_get_success(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 1); + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -224,10 +277,12 @@ static inline uint8_t mavlink_msg_rosflight_cmd_ack_get_success(const mavlink_me */ static inline void mavlink_msg_rosflight_cmd_ack_decode(const mavlink_message_t* msg, mavlink_rosflight_cmd_ack_t* rosflight_cmd_ack) { -#if MAVLINK_NEED_BYTE_SWAP - rosflight_cmd_ack->command = mavlink_msg_rosflight_cmd_ack_get_command(msg); - rosflight_cmd_ack->success = mavlink_msg_rosflight_cmd_ack_get_success(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + rosflight_cmd_ack->command = mavlink_msg_rosflight_cmd_ack_get_command(msg); + rosflight_cmd_ack->success = mavlink_msg_rosflight_cmd_ack_get_success(msg); #else - memcpy(rosflight_cmd_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN? msg->len : MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN; + memset(rosflight_cmd_ack, 0, MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_LEN); + memcpy(rosflight_cmd_ack, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_gnss.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_gnss.h index 34a35985..50e1419f 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_gnss.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_gnss.h @@ -1,41 +1,47 @@ +#pragma once // MESSAGE ROSFLIGHT_GNSS PACKING #define MAVLINK_MSG_ID_ROSFLIGHT_GNSS 197 -typedef struct __mavlink_rosflight_gnss_t -{ - int64_t seconds; /*< Unix time, in seconds*/ - double lat; /*< In deg DDS format*/ - double lon; /*< In deg DDs format*/ - uint64_t rosflight_timestamp; /*< microseconds, estimated firmware timestamp for the time of validity of the gnss data*/ - int32_t nanos; /*< Fractional Unix time*/ - float height; /*< meters, MSL*/ - float vel_n; /*< meters per second*/ - float vel_e; /*< meters per second*/ - float vel_d; /*< meters per second*/ - float h_acc; /*< meters*/ - float v_acc; /*< meters*/ - float s_acc; /*< meters*/ - uint8_t fix_type; /*< GNSS fix type*/ - uint8_t num_sat; /*< Number of satellites seen*/ + +typedef struct __mavlink_rosflight_gnss_t { + int64_t seconds; /*< Unix time, in seconds*/ + double lat; /*< In deg DDS format*/ + double lon; /*< In deg DDs format*/ + uint64_t rosflight_timestamp; /*< microseconds, estimated firmware timestamp for the time of validity of the gnss data*/ + int32_t nanos; /*< Fractional Unix time*/ + float height; /*< meters, MSL*/ + float vel_n; /*< meters per second*/ + float vel_e; /*< meters per second*/ + float vel_d; /*< meters per second*/ + float h_acc; /*< meters*/ + float v_acc; /*< meters*/ + float s_acc; /*< meters*/ + uint8_t fix_type; /*< GNSS fix type*/ + uint8_t num_sat; /*< Number of satellites seen*/ } mavlink_rosflight_gnss_t; #define MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN 66 +#define MAVLINK_MSG_ID_ROSFLIGHT_GNSS_MIN_LEN 66 #define MAVLINK_MSG_ID_197_LEN 66 +#define MAVLINK_MSG_ID_197_MIN_LEN 66 #define MAVLINK_MSG_ID_ROSFLIGHT_GNSS_CRC 221 #define MAVLINK_MSG_ID_197_CRC 221 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_ROSFLIGHT_GNSS { \ - "ROSFLIGHT_GNSS", \ - 14, \ - { { "seconds", NULL, MAVLINK_TYPE_INT64_T, 0, 0, offsetof(mavlink_rosflight_gnss_t, seconds) }, \ + 197, \ + "ROSFLIGHT_GNSS", \ + 14, \ + { { "seconds", NULL, MAVLINK_TYPE_INT64_T, 0, 0, offsetof(mavlink_rosflight_gnss_t, seconds) }, \ + { "nanos", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_rosflight_gnss_t, nanos) }, \ + { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 64, offsetof(mavlink_rosflight_gnss_t, fix_type) }, \ + { "num_sat", NULL, MAVLINK_TYPE_UINT8_T, 0, 65, offsetof(mavlink_rosflight_gnss_t, num_sat) }, \ { "lat", NULL, MAVLINK_TYPE_DOUBLE, 0, 8, offsetof(mavlink_rosflight_gnss_t, lat) }, \ { "lon", NULL, MAVLINK_TYPE_DOUBLE, 0, 16, offsetof(mavlink_rosflight_gnss_t, lon) }, \ - { "rosflight_timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 24, offsetof(mavlink_rosflight_gnss_t, rosflight_timestamp) }, \ - { "nanos", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_rosflight_gnss_t, nanos) }, \ { "height", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_rosflight_gnss_t, height) }, \ { "vel_n", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_rosflight_gnss_t, vel_n) }, \ { "vel_e", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_rosflight_gnss_t, vel_e) }, \ @@ -43,11 +49,30 @@ typedef struct __mavlink_rosflight_gnss_t { "h_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_rosflight_gnss_t, h_acc) }, \ { "v_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_rosflight_gnss_t, v_acc) }, \ { "s_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_rosflight_gnss_t, s_acc) }, \ + { "rosflight_timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 24, offsetof(mavlink_rosflight_gnss_t, rosflight_timestamp) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_ROSFLIGHT_GNSS { \ + "ROSFLIGHT_GNSS", \ + 14, \ + { { "seconds", NULL, MAVLINK_TYPE_INT64_T, 0, 0, offsetof(mavlink_rosflight_gnss_t, seconds) }, \ + { "nanos", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_rosflight_gnss_t, nanos) }, \ { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 64, offsetof(mavlink_rosflight_gnss_t, fix_type) }, \ { "num_sat", NULL, MAVLINK_TYPE_UINT8_T, 0, 65, offsetof(mavlink_rosflight_gnss_t, num_sat) }, \ + { "lat", NULL, MAVLINK_TYPE_DOUBLE, 0, 8, offsetof(mavlink_rosflight_gnss_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_DOUBLE, 0, 16, offsetof(mavlink_rosflight_gnss_t, lon) }, \ + { "height", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_rosflight_gnss_t, height) }, \ + { "vel_n", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_rosflight_gnss_t, vel_n) }, \ + { "vel_e", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_rosflight_gnss_t, vel_e) }, \ + { "vel_d", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_rosflight_gnss_t, vel_d) }, \ + { "h_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_rosflight_gnss_t, h_acc) }, \ + { "v_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_rosflight_gnss_t, v_acc) }, \ + { "s_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_rosflight_gnss_t, s_acc) }, \ + { "rosflight_timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 24, offsetof(mavlink_rosflight_gnss_t, rosflight_timestamp) }, \ } \ } - +#endif /** * @brief Pack a rosflight_gnss message @@ -55,68 +80,136 @@ typedef struct __mavlink_rosflight_gnss_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param seconds Unix time, in seconds - * @param nanos Fractional Unix time - * @param fix_type GNSS fix type - * @param num_sat Number of satellites seen - * @param lat In deg DDS format - * @param lon In deg DDs format - * @param height meters, MSL - * @param vel_n meters per second - * @param vel_e meters per second - * @param vel_d meters per second - * @param h_acc meters - * @param v_acc meters - * @param s_acc meters - * @param rosflight_timestamp microseconds, estimated firmware timestamp for the time of validity of the gnss data + * @param seconds Unix time, in seconds + * @param nanos Fractional Unix time + * @param fix_type GNSS fix type + * @param num_sat Number of satellites seen + * @param lat In deg DDS format + * @param lon In deg DDs format + * @param height meters, MSL + * @param vel_n meters per second + * @param vel_e meters per second + * @param vel_d meters per second + * @param h_acc meters + * @param v_acc meters + * @param s_acc meters + * @param rosflight_timestamp microseconds, estimated firmware timestamp for the time of validity of the gnss data * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_gnss_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - int64_t seconds, int32_t nanos, uint8_t fix_type, uint8_t num_sat, double lat, double lon, float height, float vel_n, float vel_e, float vel_d, float h_acc, float v_acc, float s_acc, uint64_t rosflight_timestamp) + int64_t seconds, int32_t nanos, uint8_t fix_type, uint8_t num_sat, double lat, double lon, float height, float vel_n, float vel_e, float vel_d, float h_acc, float v_acc, float s_acc, uint64_t rosflight_timestamp) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN]; + _mav_put_int64_t(buf, 0, seconds); + _mav_put_double(buf, 8, lat); + _mav_put_double(buf, 16, lon); + _mav_put_uint64_t(buf, 24, rosflight_timestamp); + _mav_put_int32_t(buf, 32, nanos); + _mav_put_float(buf, 36, height); + _mav_put_float(buf, 40, vel_n); + _mav_put_float(buf, 44, vel_e); + _mav_put_float(buf, 48, vel_d); + _mav_put_float(buf, 52, h_acc); + _mav_put_float(buf, 56, v_acc); + _mav_put_float(buf, 60, s_acc); + _mav_put_uint8_t(buf, 64, fix_type); + _mav_put_uint8_t(buf, 65, num_sat); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN); +#else + mavlink_rosflight_gnss_t packet; + packet.seconds = seconds; + packet.lat = lat; + packet.lon = lon; + packet.rosflight_timestamp = rosflight_timestamp; + packet.nanos = nanos; + packet.height = height; + packet.vel_n = vel_n; + packet.vel_e = vel_e; + packet.vel_d = vel_d; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.s_acc = s_acc; + packet.fix_type = fix_type; + packet.num_sat = num_sat; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_GNSS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_CRC); +} + +/** + * @brief Pack a rosflight_gnss message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param seconds Unix time, in seconds + * @param nanos Fractional Unix time + * @param fix_type GNSS fix type + * @param num_sat Number of satellites seen + * @param lat In deg DDS format + * @param lon In deg DDs format + * @param height meters, MSL + * @param vel_n meters per second + * @param vel_e meters per second + * @param vel_d meters per second + * @param h_acc meters + * @param v_acc meters + * @param s_acc meters + * @param rosflight_timestamp microseconds, estimated firmware timestamp for the time of validity of the gnss data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rosflight_gnss_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + int64_t seconds, int32_t nanos, uint8_t fix_type, uint8_t num_sat, double lat, double lon, float height, float vel_n, float vel_e, float vel_d, float h_acc, float v_acc, float s_acc, uint64_t rosflight_timestamp) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN]; - _mav_put_int64_t(buf, 0, seconds); - _mav_put_double(buf, 8, lat); - _mav_put_double(buf, 16, lon); - _mav_put_uint64_t(buf, 24, rosflight_timestamp); - _mav_put_int32_t(buf, 32, nanos); - _mav_put_float(buf, 36, height); - _mav_put_float(buf, 40, vel_n); - _mav_put_float(buf, 44, vel_e); - _mav_put_float(buf, 48, vel_d); - _mav_put_float(buf, 52, h_acc); - _mav_put_float(buf, 56, v_acc); - _mav_put_float(buf, 60, s_acc); - _mav_put_uint8_t(buf, 64, fix_type); - _mav_put_uint8_t(buf, 65, num_sat); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN]; + _mav_put_int64_t(buf, 0, seconds); + _mav_put_double(buf, 8, lat); + _mav_put_double(buf, 16, lon); + _mav_put_uint64_t(buf, 24, rosflight_timestamp); + _mav_put_int32_t(buf, 32, nanos); + _mav_put_float(buf, 36, height); + _mav_put_float(buf, 40, vel_n); + _mav_put_float(buf, 44, vel_e); + _mav_put_float(buf, 48, vel_d); + _mav_put_float(buf, 52, h_acc); + _mav_put_float(buf, 56, v_acc); + _mav_put_float(buf, 60, s_acc); + _mav_put_uint8_t(buf, 64, fix_type); + _mav_put_uint8_t(buf, 65, num_sat); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN); #else - mavlink_rosflight_gnss_t packet; - packet.seconds = seconds; - packet.lat = lat; - packet.lon = lon; - packet.rosflight_timestamp = rosflight_timestamp; - packet.nanos = nanos; - packet.height = height; - packet.vel_n = vel_n; - packet.vel_e = vel_e; - packet.vel_d = vel_d; - packet.h_acc = h_acc; - packet.v_acc = v_acc; - packet.s_acc = s_acc; - packet.fix_type = fix_type; - packet.num_sat = num_sat; + mavlink_rosflight_gnss_t packet; + packet.seconds = seconds; + packet.lat = lat; + packet.lon = lon; + packet.rosflight_timestamp = rosflight_timestamp; + packet.nanos = nanos; + packet.height = height; + packet.vel_n = vel_n; + packet.vel_e = vel_e; + packet.vel_d = vel_d; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.s_acc = s_acc; + packet.fix_type = fix_type; + packet.num_sat = num_sat; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_GNSS; + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_GNSS; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN); #endif } @@ -126,70 +219,66 @@ static inline uint16_t mavlink_msg_rosflight_gnss_pack(uint8_t system_id, uint8_ * @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 seconds Unix time, in seconds - * @param nanos Fractional Unix time - * @param fix_type GNSS fix type - * @param num_sat Number of satellites seen - * @param lat In deg DDS format - * @param lon In deg DDs format - * @param height meters, MSL - * @param vel_n meters per second - * @param vel_e meters per second - * @param vel_d meters per second - * @param h_acc meters - * @param v_acc meters - * @param s_acc meters - * @param rosflight_timestamp microseconds, estimated firmware timestamp for the time of validity of the gnss data + * @param seconds Unix time, in seconds + * @param nanos Fractional Unix time + * @param fix_type GNSS fix type + * @param num_sat Number of satellites seen + * @param lat In deg DDS format + * @param lon In deg DDs format + * @param height meters, MSL + * @param vel_n meters per second + * @param vel_e meters per second + * @param vel_d meters per second + * @param h_acc meters + * @param v_acc meters + * @param s_acc meters + * @param rosflight_timestamp microseconds, estimated firmware timestamp for the time of validity of the gnss data * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_gnss_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - int64_t seconds,int32_t nanos,uint8_t fix_type,uint8_t num_sat,double lat,double lon,float height,float vel_n,float vel_e,float vel_d,float h_acc,float v_acc,float s_acc,uint64_t rosflight_timestamp) + mavlink_message_t* msg, + int64_t seconds,int32_t nanos,uint8_t fix_type,uint8_t num_sat,double lat,double lon,float height,float vel_n,float vel_e,float vel_d,float h_acc,float v_acc,float s_acc,uint64_t rosflight_timestamp) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN]; - _mav_put_int64_t(buf, 0, seconds); - _mav_put_double(buf, 8, lat); - _mav_put_double(buf, 16, lon); - _mav_put_uint64_t(buf, 24, rosflight_timestamp); - _mav_put_int32_t(buf, 32, nanos); - _mav_put_float(buf, 36, height); - _mav_put_float(buf, 40, vel_n); - _mav_put_float(buf, 44, vel_e); - _mav_put_float(buf, 48, vel_d); - _mav_put_float(buf, 52, h_acc); - _mav_put_float(buf, 56, v_acc); - _mav_put_float(buf, 60, s_acc); - _mav_put_uint8_t(buf, 64, fix_type); - _mav_put_uint8_t(buf, 65, num_sat); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN]; + _mav_put_int64_t(buf, 0, seconds); + _mav_put_double(buf, 8, lat); + _mav_put_double(buf, 16, lon); + _mav_put_uint64_t(buf, 24, rosflight_timestamp); + _mav_put_int32_t(buf, 32, nanos); + _mav_put_float(buf, 36, height); + _mav_put_float(buf, 40, vel_n); + _mav_put_float(buf, 44, vel_e); + _mav_put_float(buf, 48, vel_d); + _mav_put_float(buf, 52, h_acc); + _mav_put_float(buf, 56, v_acc); + _mav_put_float(buf, 60, s_acc); + _mav_put_uint8_t(buf, 64, fix_type); + _mav_put_uint8_t(buf, 65, num_sat); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN); #else - mavlink_rosflight_gnss_t packet; - packet.seconds = seconds; - packet.lat = lat; - packet.lon = lon; - packet.rosflight_timestamp = rosflight_timestamp; - packet.nanos = nanos; - packet.height = height; - packet.vel_n = vel_n; - packet.vel_e = vel_e; - packet.vel_d = vel_d; - packet.h_acc = h_acc; - packet.v_acc = v_acc; - packet.s_acc = s_acc; - packet.fix_type = fix_type; - packet.num_sat = num_sat; + mavlink_rosflight_gnss_t packet; + packet.seconds = seconds; + packet.lat = lat; + packet.lon = lon; + packet.rosflight_timestamp = rosflight_timestamp; + packet.nanos = nanos; + packet.height = height; + packet.vel_n = vel_n; + packet.vel_e = vel_e; + packet.vel_d = vel_d; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.s_acc = s_acc; + packet.fix_type = fix_type; + packet.num_sat = num_sat; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_GNSS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_GNSS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_CRC); } /** @@ -202,7 +291,7 @@ static inline uint16_t mavlink_msg_rosflight_gnss_pack_chan(uint8_t system_id, u */ static inline uint16_t mavlink_msg_rosflight_gnss_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rosflight_gnss_t* rosflight_gnss) { - return mavlink_msg_rosflight_gnss_pack(system_id, component_id, msg, rosflight_gnss->seconds, rosflight_gnss->nanos, rosflight_gnss->fix_type, rosflight_gnss->num_sat, rosflight_gnss->lat, rosflight_gnss->lon, rosflight_gnss->height, rosflight_gnss->vel_n, rosflight_gnss->vel_e, rosflight_gnss->vel_d, rosflight_gnss->h_acc, rosflight_gnss->v_acc, rosflight_gnss->s_acc, rosflight_gnss->rosflight_timestamp); + return mavlink_msg_rosflight_gnss_pack(system_id, component_id, msg, rosflight_gnss->seconds, rosflight_gnss->nanos, rosflight_gnss->fix_type, rosflight_gnss->num_sat, rosflight_gnss->lat, rosflight_gnss->lon, rosflight_gnss->height, rosflight_gnss->vel_n, rosflight_gnss->vel_e, rosflight_gnss->vel_d, rosflight_gnss->h_acc, rosflight_gnss->v_acc, rosflight_gnss->s_acc, rosflight_gnss->rosflight_timestamp); } /** @@ -216,82 +305,102 @@ static inline uint16_t mavlink_msg_rosflight_gnss_encode(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_rosflight_gnss_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rosflight_gnss_t* rosflight_gnss) { - return mavlink_msg_rosflight_gnss_pack_chan(system_id, component_id, chan, msg, rosflight_gnss->seconds, rosflight_gnss->nanos, rosflight_gnss->fix_type, rosflight_gnss->num_sat, rosflight_gnss->lat, rosflight_gnss->lon, rosflight_gnss->height, rosflight_gnss->vel_n, rosflight_gnss->vel_e, rosflight_gnss->vel_d, rosflight_gnss->h_acc, rosflight_gnss->v_acc, rosflight_gnss->s_acc, rosflight_gnss->rosflight_timestamp); + return mavlink_msg_rosflight_gnss_pack_chan(system_id, component_id, chan, msg, rosflight_gnss->seconds, rosflight_gnss->nanos, rosflight_gnss->fix_type, rosflight_gnss->num_sat, rosflight_gnss->lat, rosflight_gnss->lon, rosflight_gnss->height, rosflight_gnss->vel_n, rosflight_gnss->vel_e, rosflight_gnss->vel_d, rosflight_gnss->h_acc, rosflight_gnss->v_acc, rosflight_gnss->s_acc, rosflight_gnss->rosflight_timestamp); +} + +/** + * @brief Encode a rosflight_gnss struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param rosflight_gnss C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rosflight_gnss_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_rosflight_gnss_t* rosflight_gnss) +{ + return mavlink_msg_rosflight_gnss_pack_status(system_id, component_id, _status, msg, rosflight_gnss->seconds, rosflight_gnss->nanos, rosflight_gnss->fix_type, rosflight_gnss->num_sat, rosflight_gnss->lat, rosflight_gnss->lon, rosflight_gnss->height, rosflight_gnss->vel_n, rosflight_gnss->vel_e, rosflight_gnss->vel_d, rosflight_gnss->h_acc, rosflight_gnss->v_acc, rosflight_gnss->s_acc, rosflight_gnss->rosflight_timestamp); } /** * @brief Send a rosflight_gnss message * @param chan MAVLink channel to send the message * - * @param seconds Unix time, in seconds - * @param nanos Fractional Unix time - * @param fix_type GNSS fix type - * @param num_sat Number of satellites seen - * @param lat In deg DDS format - * @param lon In deg DDs format - * @param height meters, MSL - * @param vel_n meters per second - * @param vel_e meters per second - * @param vel_d meters per second - * @param h_acc meters - * @param v_acc meters - * @param s_acc meters - * @param rosflight_timestamp microseconds, estimated firmware timestamp for the time of validity of the gnss data + * @param seconds Unix time, in seconds + * @param nanos Fractional Unix time + * @param fix_type GNSS fix type + * @param num_sat Number of satellites seen + * @param lat In deg DDS format + * @param lon In deg DDs format + * @param height meters, MSL + * @param vel_n meters per second + * @param vel_e meters per second + * @param vel_d meters per second + * @param h_acc meters + * @param v_acc meters + * @param s_acc meters + * @param rosflight_timestamp microseconds, estimated firmware timestamp for the time of validity of the gnss data */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_rosflight_gnss_send(mavlink_channel_t chan, int64_t seconds, int32_t nanos, uint8_t fix_type, uint8_t num_sat, double lat, double lon, float height, float vel_n, float vel_e, float vel_d, float h_acc, float v_acc, float s_acc, uint64_t rosflight_timestamp) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN]; - _mav_put_int64_t(buf, 0, seconds); - _mav_put_double(buf, 8, lat); - _mav_put_double(buf, 16, lon); - _mav_put_uint64_t(buf, 24, rosflight_timestamp); - _mav_put_int32_t(buf, 32, nanos); - _mav_put_float(buf, 36, height); - _mav_put_float(buf, 40, vel_n); - _mav_put_float(buf, 44, vel_e); - _mav_put_float(buf, 48, vel_d); - _mav_put_float(buf, 52, h_acc); - _mav_put_float(buf, 56, v_acc); - _mav_put_float(buf, 60, s_acc); - _mav_put_uint8_t(buf, 64, fix_type); - _mav_put_uint8_t(buf, 65, num_sat); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_GNSS, buf, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_CRC); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN]; + _mav_put_int64_t(buf, 0, seconds); + _mav_put_double(buf, 8, lat); + _mav_put_double(buf, 16, lon); + _mav_put_uint64_t(buf, 24, rosflight_timestamp); + _mav_put_int32_t(buf, 32, nanos); + _mav_put_float(buf, 36, height); + _mav_put_float(buf, 40, vel_n); + _mav_put_float(buf, 44, vel_e); + _mav_put_float(buf, 48, vel_d); + _mav_put_float(buf, 52, h_acc); + _mav_put_float(buf, 56, v_acc); + _mav_put_float(buf, 60, s_acc); + _mav_put_uint8_t(buf, 64, fix_type); + _mav_put_uint8_t(buf, 65, num_sat); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_GNSS, buf, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_GNSS, buf, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN); + mavlink_rosflight_gnss_t packet; + packet.seconds = seconds; + packet.lat = lat; + packet.lon = lon; + packet.rosflight_timestamp = rosflight_timestamp; + packet.nanos = nanos; + packet.height = height; + packet.vel_n = vel_n; + packet.vel_e = vel_e; + packet.vel_d = vel_d; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.s_acc = s_acc; + packet.fix_type = fix_type; + packet.num_sat = num_sat; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_GNSS, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_CRC); #endif -#else - mavlink_rosflight_gnss_t packet; - packet.seconds = seconds; - packet.lat = lat; - packet.lon = lon; - packet.rosflight_timestamp = rosflight_timestamp; - packet.nanos = nanos; - packet.height = height; - packet.vel_n = vel_n; - packet.vel_e = vel_e; - packet.vel_d = vel_d; - packet.h_acc = h_acc; - packet.v_acc = v_acc; - packet.s_acc = s_acc; - packet.fix_type = fix_type; - packet.num_sat = num_sat; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_GNSS, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_CRC); +/** + * @brief Send a rosflight_gnss message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_rosflight_gnss_send_struct(mavlink_channel_t chan, const mavlink_rosflight_gnss_t* rosflight_gnss) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_rosflight_gnss_send(chan, rosflight_gnss->seconds, rosflight_gnss->nanos, rosflight_gnss->fix_type, rosflight_gnss->num_sat, rosflight_gnss->lat, rosflight_gnss->lon, rosflight_gnss->height, rosflight_gnss->vel_n, rosflight_gnss->vel_e, rosflight_gnss->vel_d, rosflight_gnss->h_acc, rosflight_gnss->v_acc, rosflight_gnss->s_acc, rosflight_gnss->rosflight_timestamp); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_GNSS, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_GNSS, (const char *)rosflight_gnss, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_CRC); #endif } #if MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -300,49 +409,41 @@ static inline void mavlink_msg_rosflight_gnss_send(mavlink_channel_t chan, int64 static inline void mavlink_msg_rosflight_gnss_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int64_t seconds, int32_t nanos, uint8_t fix_type, uint8_t num_sat, double lat, double lon, float height, float vel_n, float vel_e, float vel_d, float h_acc, float v_acc, float s_acc, uint64_t rosflight_timestamp) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int64_t(buf, 0, seconds); - _mav_put_double(buf, 8, lat); - _mav_put_double(buf, 16, lon); - _mav_put_uint64_t(buf, 24, rosflight_timestamp); - _mav_put_int32_t(buf, 32, nanos); - _mav_put_float(buf, 36, height); - _mav_put_float(buf, 40, vel_n); - _mav_put_float(buf, 44, vel_e); - _mav_put_float(buf, 48, vel_d); - _mav_put_float(buf, 52, h_acc); - _mav_put_float(buf, 56, v_acc); - _mav_put_float(buf, 60, s_acc); - _mav_put_uint8_t(buf, 64, fix_type); - _mav_put_uint8_t(buf, 65, num_sat); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_GNSS, buf, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_CRC); + char *buf = (char *)msgbuf; + _mav_put_int64_t(buf, 0, seconds); + _mav_put_double(buf, 8, lat); + _mav_put_double(buf, 16, lon); + _mav_put_uint64_t(buf, 24, rosflight_timestamp); + _mav_put_int32_t(buf, 32, nanos); + _mav_put_float(buf, 36, height); + _mav_put_float(buf, 40, vel_n); + _mav_put_float(buf, 44, vel_e); + _mav_put_float(buf, 48, vel_d); + _mav_put_float(buf, 52, h_acc); + _mav_put_float(buf, 56, v_acc); + _mav_put_float(buf, 60, s_acc); + _mav_put_uint8_t(buf, 64, fix_type); + _mav_put_uint8_t(buf, 65, num_sat); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_GNSS, buf, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_GNSS, buf, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN); -#endif -#else - mavlink_rosflight_gnss_t *packet = (mavlink_rosflight_gnss_t *)msgbuf; - packet->seconds = seconds; - packet->lat = lat; - packet->lon = lon; - packet->rosflight_timestamp = rosflight_timestamp; - packet->nanos = nanos; - packet->height = height; - packet->vel_n = vel_n; - packet->vel_e = vel_e; - packet->vel_d = vel_d; - packet->h_acc = h_acc; - packet->v_acc = v_acc; - packet->s_acc = s_acc; - packet->fix_type = fix_type; - packet->num_sat = num_sat; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_GNSS, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_GNSS, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN); -#endif + mavlink_rosflight_gnss_t *packet = (mavlink_rosflight_gnss_t *)msgbuf; + packet->seconds = seconds; + packet->lat = lat; + packet->lon = lon; + packet->rosflight_timestamp = rosflight_timestamp; + packet->nanos = nanos; + packet->height = height; + packet->vel_n = vel_n; + packet->vel_e = vel_e; + packet->vel_d = vel_d; + packet->h_acc = h_acc; + packet->v_acc = v_acc; + packet->s_acc = s_acc; + packet->fix_type = fix_type; + packet->num_sat = num_sat; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_GNSS, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_CRC); #endif } #endif @@ -355,141 +456,141 @@ static inline void mavlink_msg_rosflight_gnss_send_buf(mavlink_message_t *msgbuf /** * @brief Get field seconds from rosflight_gnss message * - * @return Unix time, in seconds + * @return Unix time, in seconds */ static inline int64_t mavlink_msg_rosflight_gnss_get_seconds(const mavlink_message_t* msg) { - return _MAV_RETURN_int64_t(msg, 0); + return _MAV_RETURN_int64_t(msg, 0); } /** * @brief Get field nanos from rosflight_gnss message * - * @return Fractional Unix time + * @return Fractional Unix time */ static inline int32_t mavlink_msg_rosflight_gnss_get_nanos(const mavlink_message_t* msg) { - return _MAV_RETURN_int32_t(msg, 32); + return _MAV_RETURN_int32_t(msg, 32); } /** * @brief Get field fix_type from rosflight_gnss message * - * @return GNSS fix type + * @return GNSS fix type */ static inline uint8_t mavlink_msg_rosflight_gnss_get_fix_type(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 64); + return _MAV_RETURN_uint8_t(msg, 64); } /** * @brief Get field num_sat from rosflight_gnss message * - * @return Number of satellites seen + * @return Number of satellites seen */ static inline uint8_t mavlink_msg_rosflight_gnss_get_num_sat(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 65); + return _MAV_RETURN_uint8_t(msg, 65); } /** * @brief Get field lat from rosflight_gnss message * - * @return In deg DDS format + * @return In deg DDS format */ static inline double mavlink_msg_rosflight_gnss_get_lat(const mavlink_message_t* msg) { - return _MAV_RETURN_double(msg, 8); + return _MAV_RETURN_double(msg, 8); } /** * @brief Get field lon from rosflight_gnss message * - * @return In deg DDs format + * @return In deg DDs format */ static inline double mavlink_msg_rosflight_gnss_get_lon(const mavlink_message_t* msg) { - return _MAV_RETURN_double(msg, 16); + return _MAV_RETURN_double(msg, 16); } /** * @brief Get field height from rosflight_gnss message * - * @return meters, MSL + * @return meters, MSL */ static inline float mavlink_msg_rosflight_gnss_get_height(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 36); + return _MAV_RETURN_float(msg, 36); } /** * @brief Get field vel_n from rosflight_gnss message * - * @return meters per second + * @return meters per second */ static inline float mavlink_msg_rosflight_gnss_get_vel_n(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 40); + return _MAV_RETURN_float(msg, 40); } /** * @brief Get field vel_e from rosflight_gnss message * - * @return meters per second + * @return meters per second */ static inline float mavlink_msg_rosflight_gnss_get_vel_e(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 44); + return _MAV_RETURN_float(msg, 44); } /** * @brief Get field vel_d from rosflight_gnss message * - * @return meters per second + * @return meters per second */ static inline float mavlink_msg_rosflight_gnss_get_vel_d(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 48); + return _MAV_RETURN_float(msg, 48); } /** * @brief Get field h_acc from rosflight_gnss message * - * @return meters + * @return meters */ static inline float mavlink_msg_rosflight_gnss_get_h_acc(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 52); + return _MAV_RETURN_float(msg, 52); } /** * @brief Get field v_acc from rosflight_gnss message * - * @return meters + * @return meters */ static inline float mavlink_msg_rosflight_gnss_get_v_acc(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 56); + return _MAV_RETURN_float(msg, 56); } /** * @brief Get field s_acc from rosflight_gnss message * - * @return meters + * @return meters */ static inline float mavlink_msg_rosflight_gnss_get_s_acc(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 60); + return _MAV_RETURN_float(msg, 60); } /** * @brief Get field rosflight_timestamp from rosflight_gnss message * - * @return microseconds, estimated firmware timestamp for the time of validity of the gnss data + * @return microseconds, estimated firmware timestamp for the time of validity of the gnss data */ static inline uint64_t mavlink_msg_rosflight_gnss_get_rosflight_timestamp(const mavlink_message_t* msg) { - return _MAV_RETURN_uint64_t(msg, 24); + return _MAV_RETURN_uint64_t(msg, 24); } /** @@ -500,22 +601,24 @@ static inline uint64_t mavlink_msg_rosflight_gnss_get_rosflight_timestamp(const */ static inline void mavlink_msg_rosflight_gnss_decode(const mavlink_message_t* msg, mavlink_rosflight_gnss_t* rosflight_gnss) { -#if MAVLINK_NEED_BYTE_SWAP - rosflight_gnss->seconds = mavlink_msg_rosflight_gnss_get_seconds(msg); - rosflight_gnss->lat = mavlink_msg_rosflight_gnss_get_lat(msg); - rosflight_gnss->lon = mavlink_msg_rosflight_gnss_get_lon(msg); - rosflight_gnss->rosflight_timestamp = mavlink_msg_rosflight_gnss_get_rosflight_timestamp(msg); - rosflight_gnss->nanos = mavlink_msg_rosflight_gnss_get_nanos(msg); - rosflight_gnss->height = mavlink_msg_rosflight_gnss_get_height(msg); - rosflight_gnss->vel_n = mavlink_msg_rosflight_gnss_get_vel_n(msg); - rosflight_gnss->vel_e = mavlink_msg_rosflight_gnss_get_vel_e(msg); - rosflight_gnss->vel_d = mavlink_msg_rosflight_gnss_get_vel_d(msg); - rosflight_gnss->h_acc = mavlink_msg_rosflight_gnss_get_h_acc(msg); - rosflight_gnss->v_acc = mavlink_msg_rosflight_gnss_get_v_acc(msg); - rosflight_gnss->s_acc = mavlink_msg_rosflight_gnss_get_s_acc(msg); - rosflight_gnss->fix_type = mavlink_msg_rosflight_gnss_get_fix_type(msg); - rosflight_gnss->num_sat = mavlink_msg_rosflight_gnss_get_num_sat(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + rosflight_gnss->seconds = mavlink_msg_rosflight_gnss_get_seconds(msg); + rosflight_gnss->lat = mavlink_msg_rosflight_gnss_get_lat(msg); + rosflight_gnss->lon = mavlink_msg_rosflight_gnss_get_lon(msg); + rosflight_gnss->rosflight_timestamp = mavlink_msg_rosflight_gnss_get_rosflight_timestamp(msg); + rosflight_gnss->nanos = mavlink_msg_rosflight_gnss_get_nanos(msg); + rosflight_gnss->height = mavlink_msg_rosflight_gnss_get_height(msg); + rosflight_gnss->vel_n = mavlink_msg_rosflight_gnss_get_vel_n(msg); + rosflight_gnss->vel_e = mavlink_msg_rosflight_gnss_get_vel_e(msg); + rosflight_gnss->vel_d = mavlink_msg_rosflight_gnss_get_vel_d(msg); + rosflight_gnss->h_acc = mavlink_msg_rosflight_gnss_get_h_acc(msg); + rosflight_gnss->v_acc = mavlink_msg_rosflight_gnss_get_v_acc(msg); + rosflight_gnss->s_acc = mavlink_msg_rosflight_gnss_get_s_acc(msg); + rosflight_gnss->fix_type = mavlink_msg_rosflight_gnss_get_fix_type(msg); + rosflight_gnss->num_sat = mavlink_msg_rosflight_gnss_get_num_sat(msg); #else - memcpy(rosflight_gnss, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN? msg->len : MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN; + memset(rosflight_gnss, 0, MAVLINK_MSG_ID_ROSFLIGHT_GNSS_LEN); + memcpy(rosflight_gnss, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_hard_error.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_hard_error.h index c61d296f..9658d7b3 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_hard_error.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_hard_error.h @@ -1,33 +1,48 @@ +#pragma once // MESSAGE ROSFLIGHT_HARD_ERROR PACKING #define MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR 196 -typedef struct __mavlink_rosflight_hard_error_t -{ - uint32_t error_code; /*< */ - uint32_t pc; /*< */ - uint32_t reset_count; /*< */ - uint32_t doRearm; /*< */ + +typedef struct __mavlink_rosflight_hard_error_t { + uint32_t error_code; /*< */ + uint32_t pc; /*< */ + uint32_t reset_count; /*< */ + uint32_t doRearm; /*< */ } mavlink_rosflight_hard_error_t; #define MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN 16 +#define MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_MIN_LEN 16 #define MAVLINK_MSG_ID_196_LEN 16 +#define MAVLINK_MSG_ID_196_MIN_LEN 16 #define MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_CRC 10 #define MAVLINK_MSG_ID_196_CRC 10 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_ROSFLIGHT_HARD_ERROR { \ - "ROSFLIGHT_HARD_ERROR", \ - 4, \ - { { "error_code", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_rosflight_hard_error_t, error_code) }, \ + 196, \ + "ROSFLIGHT_HARD_ERROR", \ + 4, \ + { { "error_code", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_rosflight_hard_error_t, error_code) }, \ { "pc", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_rosflight_hard_error_t, pc) }, \ { "reset_count", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_rosflight_hard_error_t, reset_count) }, \ { "doRearm", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_rosflight_hard_error_t, doRearm) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_ROSFLIGHT_HARD_ERROR { \ + "ROSFLIGHT_HARD_ERROR", \ + 4, \ + { { "error_code", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_rosflight_hard_error_t, error_code) }, \ + { "pc", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_rosflight_hard_error_t, pc) }, \ + { "reset_count", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_rosflight_hard_error_t, reset_count) }, \ + { "doRearm", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_rosflight_hard_error_t, doRearm) }, \ + } \ +} +#endif /** * @brief Pack a rosflight_hard_error message @@ -35,38 +50,76 @@ typedef struct __mavlink_rosflight_hard_error_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param error_code - * @param pc - * @param reset_count - * @param doRearm + * @param error_code + * @param pc + * @param reset_count + * @param doRearm * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_hard_error_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t error_code, uint32_t pc, uint32_t reset_count, uint32_t doRearm) + uint32_t error_code, uint32_t pc, uint32_t reset_count, uint32_t doRearm) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN]; - _mav_put_uint32_t(buf, 0, error_code); - _mav_put_uint32_t(buf, 4, pc); - _mav_put_uint32_t(buf, 8, reset_count); - _mav_put_uint32_t(buf, 12, doRearm); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN]; + _mav_put_uint32_t(buf, 0, error_code); + _mav_put_uint32_t(buf, 4, pc); + _mav_put_uint32_t(buf, 8, reset_count); + _mav_put_uint32_t(buf, 12, doRearm); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN); #else - mavlink_rosflight_hard_error_t packet; - packet.error_code = error_code; - packet.pc = pc; - packet.reset_count = reset_count; - packet.doRearm = doRearm; + mavlink_rosflight_hard_error_t packet; + packet.error_code = error_code; + packet.pc = pc; + packet.reset_count = reset_count; + packet.doRearm = doRearm; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR; + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_CRC); +} + +/** + * @brief Pack a rosflight_hard_error message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param error_code + * @param pc + * @param reset_count + * @param doRearm + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rosflight_hard_error_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t error_code, uint32_t pc, uint32_t reset_count, uint32_t doRearm) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN]; + _mav_put_uint32_t(buf, 0, error_code); + _mav_put_uint32_t(buf, 4, pc); + _mav_put_uint32_t(buf, 8, reset_count); + _mav_put_uint32_t(buf, 12, doRearm); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN); +#else + mavlink_rosflight_hard_error_t packet; + packet.error_code = error_code; + packet.pc = pc; + packet.reset_count = reset_count; + packet.doRearm = doRearm; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN); #endif } @@ -76,40 +129,36 @@ static inline uint16_t mavlink_msg_rosflight_hard_error_pack(uint8_t system_id, * @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 error_code - * @param pc - * @param reset_count - * @param doRearm + * @param error_code + * @param pc + * @param reset_count + * @param doRearm * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_hard_error_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t error_code,uint32_t pc,uint32_t reset_count,uint32_t doRearm) + mavlink_message_t* msg, + uint32_t error_code,uint32_t pc,uint32_t reset_count,uint32_t doRearm) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN]; - _mav_put_uint32_t(buf, 0, error_code); - _mav_put_uint32_t(buf, 4, pc); - _mav_put_uint32_t(buf, 8, reset_count); - _mav_put_uint32_t(buf, 12, doRearm); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN]; + _mav_put_uint32_t(buf, 0, error_code); + _mav_put_uint32_t(buf, 4, pc); + _mav_put_uint32_t(buf, 8, reset_count); + _mav_put_uint32_t(buf, 12, doRearm); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN); #else - mavlink_rosflight_hard_error_t packet; - packet.error_code = error_code; - packet.pc = pc; - packet.reset_count = reset_count; - packet.doRearm = doRearm; + mavlink_rosflight_hard_error_t packet; + packet.error_code = error_code; + packet.pc = pc; + packet.reset_count = reset_count; + packet.doRearm = doRearm; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_CRC); } /** @@ -122,7 +171,7 @@ static inline uint16_t mavlink_msg_rosflight_hard_error_pack_chan(uint8_t system */ static inline uint16_t mavlink_msg_rosflight_hard_error_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rosflight_hard_error_t* rosflight_hard_error) { - return mavlink_msg_rosflight_hard_error_pack(system_id, component_id, msg, rosflight_hard_error->error_code, rosflight_hard_error->pc, rosflight_hard_error->reset_count, rosflight_hard_error->doRearm); + return mavlink_msg_rosflight_hard_error_pack(system_id, component_id, msg, rosflight_hard_error->error_code, rosflight_hard_error->pc, rosflight_hard_error->reset_count, rosflight_hard_error->doRearm); } /** @@ -136,52 +185,72 @@ static inline uint16_t mavlink_msg_rosflight_hard_error_encode(uint8_t system_id */ static inline uint16_t mavlink_msg_rosflight_hard_error_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rosflight_hard_error_t* rosflight_hard_error) { - return mavlink_msg_rosflight_hard_error_pack_chan(system_id, component_id, chan, msg, rosflight_hard_error->error_code, rosflight_hard_error->pc, rosflight_hard_error->reset_count, rosflight_hard_error->doRearm); + return mavlink_msg_rosflight_hard_error_pack_chan(system_id, component_id, chan, msg, rosflight_hard_error->error_code, rosflight_hard_error->pc, rosflight_hard_error->reset_count, rosflight_hard_error->doRearm); +} + +/** + * @brief Encode a rosflight_hard_error struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param rosflight_hard_error C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rosflight_hard_error_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_rosflight_hard_error_t* rosflight_hard_error) +{ + return mavlink_msg_rosflight_hard_error_pack_status(system_id, component_id, _status, msg, rosflight_hard_error->error_code, rosflight_hard_error->pc, rosflight_hard_error->reset_count, rosflight_hard_error->doRearm); } /** * @brief Send a rosflight_hard_error message * @param chan MAVLink channel to send the message * - * @param error_code - * @param pc - * @param reset_count - * @param doRearm + * @param error_code + * @param pc + * @param reset_count + * @param doRearm */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_rosflight_hard_error_send(mavlink_channel_t chan, uint32_t error_code, uint32_t pc, uint32_t reset_count, uint32_t doRearm) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN]; - _mav_put_uint32_t(buf, 0, error_code); - _mav_put_uint32_t(buf, 4, pc); - _mav_put_uint32_t(buf, 8, reset_count); - _mav_put_uint32_t(buf, 12, doRearm); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN]; + _mav_put_uint32_t(buf, 0, error_code); + _mav_put_uint32_t(buf, 4, pc); + _mav_put_uint32_t(buf, 8, reset_count); + _mav_put_uint32_t(buf, 12, doRearm); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR, buf, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_CRC); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR, buf, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR, buf, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN); + mavlink_rosflight_hard_error_t packet; + packet.error_code = error_code; + packet.pc = pc; + packet.reset_count = reset_count; + packet.doRearm = doRearm; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_CRC); #endif -#else - mavlink_rosflight_hard_error_t packet; - packet.error_code = error_code; - packet.pc = pc; - packet.reset_count = reset_count; - packet.doRearm = doRearm; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_CRC); +/** + * @brief Send a rosflight_hard_error message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_rosflight_hard_error_send_struct(mavlink_channel_t chan, const mavlink_rosflight_hard_error_t* rosflight_hard_error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_rosflight_hard_error_send(chan, rosflight_hard_error->error_code, rosflight_hard_error->pc, rosflight_hard_error->reset_count, rosflight_hard_error->doRearm); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR, (const char *)rosflight_hard_error, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_CRC); #endif } #if MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -190,29 +259,21 @@ static inline void mavlink_msg_rosflight_hard_error_send(mavlink_channel_t chan, static inline void mavlink_msg_rosflight_hard_error_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t error_code, uint32_t pc, uint32_t reset_count, uint32_t doRearm) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, error_code); - _mav_put_uint32_t(buf, 4, pc); - _mav_put_uint32_t(buf, 8, reset_count); - _mav_put_uint32_t(buf, 12, doRearm); + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, error_code); + _mav_put_uint32_t(buf, 4, pc); + _mav_put_uint32_t(buf, 8, reset_count); + _mav_put_uint32_t(buf, 12, doRearm); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR, buf, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR, buf, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR, buf, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_CRC); #else - mavlink_rosflight_hard_error_t *packet = (mavlink_rosflight_hard_error_t *)msgbuf; - packet->error_code = error_code; - packet->pc = pc; - packet->reset_count = reset_count; - packet->doRearm = doRearm; + mavlink_rosflight_hard_error_t *packet = (mavlink_rosflight_hard_error_t *)msgbuf; + packet->error_code = error_code; + packet->pc = pc; + packet->reset_count = reset_count; + packet->doRearm = doRearm; -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_CRC); #endif } #endif @@ -225,41 +286,41 @@ static inline void mavlink_msg_rosflight_hard_error_send_buf(mavlink_message_t * /** * @brief Get field error_code from rosflight_hard_error message * - * @return + * @return */ static inline uint32_t mavlink_msg_rosflight_hard_error_get_error_code(const mavlink_message_t* msg) { - return _MAV_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** * @brief Get field pc from rosflight_hard_error message * - * @return + * @return */ static inline uint32_t mavlink_msg_rosflight_hard_error_get_pc(const mavlink_message_t* msg) { - return _MAV_RETURN_uint32_t(msg, 4); + return _MAV_RETURN_uint32_t(msg, 4); } /** * @brief Get field reset_count from rosflight_hard_error message * - * @return + * @return */ static inline uint32_t mavlink_msg_rosflight_hard_error_get_reset_count(const mavlink_message_t* msg) { - return _MAV_RETURN_uint32_t(msg, 8); + return _MAV_RETURN_uint32_t(msg, 8); } /** * @brief Get field doRearm from rosflight_hard_error message * - * @return + * @return */ static inline uint32_t mavlink_msg_rosflight_hard_error_get_doRearm(const mavlink_message_t* msg) { - return _MAV_RETURN_uint32_t(msg, 12); + return _MAV_RETURN_uint32_t(msg, 12); } /** @@ -270,12 +331,14 @@ static inline uint32_t mavlink_msg_rosflight_hard_error_get_doRearm(const mavlin */ static inline void mavlink_msg_rosflight_hard_error_decode(const mavlink_message_t* msg, mavlink_rosflight_hard_error_t* rosflight_hard_error) { -#if MAVLINK_NEED_BYTE_SWAP - rosflight_hard_error->error_code = mavlink_msg_rosflight_hard_error_get_error_code(msg); - rosflight_hard_error->pc = mavlink_msg_rosflight_hard_error_get_pc(msg); - rosflight_hard_error->reset_count = mavlink_msg_rosflight_hard_error_get_reset_count(msg); - rosflight_hard_error->doRearm = mavlink_msg_rosflight_hard_error_get_doRearm(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + rosflight_hard_error->error_code = mavlink_msg_rosflight_hard_error_get_error_code(msg); + rosflight_hard_error->pc = mavlink_msg_rosflight_hard_error_get_pc(msg); + rosflight_hard_error->reset_count = mavlink_msg_rosflight_hard_error_get_reset_count(msg); + rosflight_hard_error->doRearm = mavlink_msg_rosflight_hard_error_get_doRearm(msg); #else - memcpy(rosflight_hard_error, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN? msg->len : MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN; + memset(rosflight_hard_error, 0, MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_LEN); + memcpy(rosflight_hard_error, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_output_raw.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_output_raw.h index 405cec64..33ad1669 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_output_raw.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_output_raw.h @@ -1,29 +1,42 @@ +#pragma once // MESSAGE ROSFLIGHT_OUTPUT_RAW PACKING #define MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW 190 -typedef struct __mavlink_rosflight_output_raw_t -{ - uint64_t stamp; /*< */ - float values[14]; /*< */ + +typedef struct __mavlink_rosflight_output_raw_t { + uint64_t stamp; /*< */ + float values[14]; /*< */ } mavlink_rosflight_output_raw_t; #define MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN 64 +#define MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_MIN_LEN 64 #define MAVLINK_MSG_ID_190_LEN 64 +#define MAVLINK_MSG_ID_190_MIN_LEN 64 #define MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_CRC 181 #define MAVLINK_MSG_ID_190_CRC 181 #define MAVLINK_MSG_ROSFLIGHT_OUTPUT_RAW_FIELD_VALUES_LEN 14 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_ROSFLIGHT_OUTPUT_RAW { \ - "ROSFLIGHT_OUTPUT_RAW", \ - 2, \ - { { "stamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_rosflight_output_raw_t, stamp) }, \ + 190, \ + "ROSFLIGHT_OUTPUT_RAW", \ + 2, \ + { { "stamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_rosflight_output_raw_t, stamp) }, \ { "values", NULL, MAVLINK_TYPE_FLOAT, 14, 8, offsetof(mavlink_rosflight_output_raw_t, values) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_ROSFLIGHT_OUTPUT_RAW { \ + "ROSFLIGHT_OUTPUT_RAW", \ + 2, \ + { { "stamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_rosflight_output_raw_t, stamp) }, \ + { "values", NULL, MAVLINK_TYPE_FLOAT, 14, 8, offsetof(mavlink_rosflight_output_raw_t, values) }, \ + } \ +} +#endif /** * @brief Pack a rosflight_output_raw message @@ -31,30 +44,60 @@ typedef struct __mavlink_rosflight_output_raw_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param stamp - * @param values + * @param stamp + * @param values * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_output_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t stamp, const float *values) + uint64_t stamp, const float *values) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN]; - _mav_put_uint64_t(buf, 0, stamp); - _mav_put_float_array(buf, 8, values, 14); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN]; + _mav_put_uint64_t(buf, 0, stamp); + _mav_put_float_array(buf, 8, values, 14); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN); #else - mavlink_rosflight_output_raw_t packet; - packet.stamp = stamp; - mav_array_memcpy(packet.values, values, sizeof(float)*14); + mavlink_rosflight_output_raw_t packet; + packet.stamp = stamp; + mav_array_memcpy(packet.values, values, sizeof(float)*14); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW; + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_CRC); +} + +/** + * @brief Pack a rosflight_output_raw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param stamp + * @param values + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rosflight_output_raw_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t stamp, const float *values) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN]; + _mav_put_uint64_t(buf, 0, stamp); + _mav_put_float_array(buf, 8, values, 14); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN); +#else + mavlink_rosflight_output_raw_t packet; + packet.stamp = stamp; + mav_array_memcpy(packet.values, values, sizeof(float)*14); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN); #endif } @@ -64,32 +107,28 @@ static inline uint16_t mavlink_msg_rosflight_output_raw_pack(uint8_t system_id, * @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 stamp - * @param values + * @param stamp + * @param values * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_output_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t stamp,const float *values) + mavlink_message_t* msg, + uint64_t stamp,const float *values) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN]; - _mav_put_uint64_t(buf, 0, stamp); - _mav_put_float_array(buf, 8, values, 14); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN]; + _mav_put_uint64_t(buf, 0, stamp); + _mav_put_float_array(buf, 8, values, 14); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN); #else - mavlink_rosflight_output_raw_t packet; - packet.stamp = stamp; - mav_array_memcpy(packet.values, values, sizeof(float)*14); + mavlink_rosflight_output_raw_t packet; + packet.stamp = stamp; + mav_array_memcpy(packet.values, values, sizeof(float)*14); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_CRC); } /** @@ -102,7 +141,7 @@ static inline uint16_t mavlink_msg_rosflight_output_raw_pack_chan(uint8_t system */ static inline uint16_t mavlink_msg_rosflight_output_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rosflight_output_raw_t* rosflight_output_raw) { - return mavlink_msg_rosflight_output_raw_pack(system_id, component_id, msg, rosflight_output_raw->stamp, rosflight_output_raw->values); + return mavlink_msg_rosflight_output_raw_pack(system_id, component_id, msg, rosflight_output_raw->stamp, rosflight_output_raw->values); } /** @@ -116,44 +155,64 @@ static inline uint16_t mavlink_msg_rosflight_output_raw_encode(uint8_t system_id */ static inline uint16_t mavlink_msg_rosflight_output_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rosflight_output_raw_t* rosflight_output_raw) { - return mavlink_msg_rosflight_output_raw_pack_chan(system_id, component_id, chan, msg, rosflight_output_raw->stamp, rosflight_output_raw->values); + return mavlink_msg_rosflight_output_raw_pack_chan(system_id, component_id, chan, msg, rosflight_output_raw->stamp, rosflight_output_raw->values); +} + +/** + * @brief Encode a rosflight_output_raw struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param rosflight_output_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rosflight_output_raw_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_rosflight_output_raw_t* rosflight_output_raw) +{ + return mavlink_msg_rosflight_output_raw_pack_status(system_id, component_id, _status, msg, rosflight_output_raw->stamp, rosflight_output_raw->values); } /** * @brief Send a rosflight_output_raw message * @param chan MAVLink channel to send the message * - * @param stamp - * @param values + * @param stamp + * @param values */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_rosflight_output_raw_send(mavlink_channel_t chan, uint64_t stamp, const float *values) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN]; - _mav_put_uint64_t(buf, 0, stamp); - _mav_put_float_array(buf, 8, values, 14); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW, buf, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_CRC); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN]; + _mav_put_uint64_t(buf, 0, stamp); + _mav_put_float_array(buf, 8, values, 14); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW, buf, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW, buf, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN); + mavlink_rosflight_output_raw_t packet; + packet.stamp = stamp; + mav_array_memcpy(packet.values, values, sizeof(float)*14); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_CRC); #endif +} + +/** + * @brief Send a rosflight_output_raw message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_rosflight_output_raw_send_struct(mavlink_channel_t chan, const mavlink_rosflight_output_raw_t* rosflight_output_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_rosflight_output_raw_send(chan, rosflight_output_raw->stamp, rosflight_output_raw->values); #else - mavlink_rosflight_output_raw_t packet; - packet.stamp = stamp; - mav_array_memcpy(packet.values, values, sizeof(float)*14); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW, (const char *)rosflight_output_raw, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_CRC); #endif } #if MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -162,23 +221,15 @@ static inline void mavlink_msg_rosflight_output_raw_send(mavlink_channel_t chan, static inline void mavlink_msg_rosflight_output_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t stamp, const float *values) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, stamp); - _mav_put_float_array(buf, 8, values, 14); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW, buf, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_CRC); + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, stamp); + _mav_put_float_array(buf, 8, values, 14); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW, buf, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW, buf, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN); -#endif -#else - mavlink_rosflight_output_raw_t *packet = (mavlink_rosflight_output_raw_t *)msgbuf; - packet->stamp = stamp; - mav_array_memcpy(packet->values, values, sizeof(float)*14); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN); -#endif + mavlink_rosflight_output_raw_t *packet = (mavlink_rosflight_output_raw_t *)msgbuf; + packet->stamp = stamp; + mav_array_memcpy(packet->values, values, sizeof(float)*14); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_CRC); #endif } #endif @@ -191,21 +242,21 @@ static inline void mavlink_msg_rosflight_output_raw_send_buf(mavlink_message_t * /** * @brief Get field stamp from rosflight_output_raw message * - * @return + * @return */ static inline uint64_t mavlink_msg_rosflight_output_raw_get_stamp(const mavlink_message_t* msg) { - return _MAV_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** * @brief Get field values from rosflight_output_raw message * - * @return + * @return */ static inline uint16_t mavlink_msg_rosflight_output_raw_get_values(const mavlink_message_t* msg, float *values) { - return _MAV_RETURN_float_array(msg, values, 14, 8); + return _MAV_RETURN_float_array(msg, values, 14, 8); } /** @@ -216,10 +267,12 @@ static inline uint16_t mavlink_msg_rosflight_output_raw_get_values(const mavlink */ static inline void mavlink_msg_rosflight_output_raw_decode(const mavlink_message_t* msg, mavlink_rosflight_output_raw_t* rosflight_output_raw) { -#if MAVLINK_NEED_BYTE_SWAP - rosflight_output_raw->stamp = mavlink_msg_rosflight_output_raw_get_stamp(msg); - mavlink_msg_rosflight_output_raw_get_values(msg, rosflight_output_raw->values); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + rosflight_output_raw->stamp = mavlink_msg_rosflight_output_raw_get_stamp(msg); + mavlink_msg_rosflight_output_raw_get_values(msg, rosflight_output_raw->values); #else - memcpy(rosflight_output_raw, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN? msg->len : MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN; + memset(rosflight_output_raw, 0, MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_LEN); + memcpy(rosflight_output_raw, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_status.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_status.h index 0e53c950..cf604656 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_status.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_status.h @@ -1,41 +1,60 @@ +#pragma once // MESSAGE ROSFLIGHT_STATUS PACKING #define MAVLINK_MSG_ID_ROSFLIGHT_STATUS 191 -typedef struct __mavlink_rosflight_status_t -{ - int16_t num_errors; /*< */ - int16_t loop_time_us; /*< */ - uint8_t armed; /*< */ - uint8_t failsafe; /*< */ - uint8_t rc_override; /*< */ - uint8_t offboard; /*< */ - uint8_t error_code; /*< */ - uint8_t control_mode; /*< */ + +typedef struct __mavlink_rosflight_status_t { + int16_t num_errors; /*< */ + int16_t loop_time_us; /*< */ + uint8_t armed; /*< */ + uint8_t failsafe; /*< */ + uint8_t rc_override; /*< */ + uint8_t offboard; /*< */ + uint8_t error_code; /*< */ + uint8_t control_mode; /*< */ } mavlink_rosflight_status_t; #define MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN 10 +#define MAVLINK_MSG_ID_ROSFLIGHT_STATUS_MIN_LEN 10 #define MAVLINK_MSG_ID_191_LEN 10 +#define MAVLINK_MSG_ID_191_MIN_LEN 10 #define MAVLINK_MSG_ID_ROSFLIGHT_STATUS_CRC 183 #define MAVLINK_MSG_ID_191_CRC 183 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_ROSFLIGHT_STATUS { \ - "ROSFLIGHT_STATUS", \ - 8, \ - { { "num_errors", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_rosflight_status_t, num_errors) }, \ + 191, \ + "ROSFLIGHT_STATUS", \ + 8, \ + { { "armed", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_rosflight_status_t, armed) }, \ + { "failsafe", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_rosflight_status_t, failsafe) }, \ + { "rc_override", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_rosflight_status_t, rc_override) }, \ + { "offboard", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_rosflight_status_t, offboard) }, \ + { "error_code", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_rosflight_status_t, error_code) }, \ + { "control_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_rosflight_status_t, control_mode) }, \ + { "num_errors", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_rosflight_status_t, num_errors) }, \ { "loop_time_us", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_rosflight_status_t, loop_time_us) }, \ - { "armed", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_rosflight_status_t, armed) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_ROSFLIGHT_STATUS { \ + "ROSFLIGHT_STATUS", \ + 8, \ + { { "armed", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_rosflight_status_t, armed) }, \ { "failsafe", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_rosflight_status_t, failsafe) }, \ { "rc_override", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_rosflight_status_t, rc_override) }, \ { "offboard", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_rosflight_status_t, offboard) }, \ { "error_code", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_rosflight_status_t, error_code) }, \ { "control_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_rosflight_status_t, control_mode) }, \ + { "num_errors", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_rosflight_status_t, num_errors) }, \ + { "loop_time_us", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_rosflight_status_t, loop_time_us) }, \ } \ } - +#endif /** * @brief Pack a rosflight_status message @@ -43,50 +62,100 @@ typedef struct __mavlink_rosflight_status_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param armed - * @param failsafe - * @param rc_override - * @param offboard - * @param error_code - * @param control_mode - * @param num_errors - * @param loop_time_us + * @param armed + * @param failsafe + * @param rc_override + * @param offboard + * @param error_code + * @param control_mode + * @param num_errors + * @param loop_time_us * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t armed, uint8_t failsafe, uint8_t rc_override, uint8_t offboard, uint8_t error_code, uint8_t control_mode, int16_t num_errors, int16_t loop_time_us) + uint8_t armed, uint8_t failsafe, uint8_t rc_override, uint8_t offboard, uint8_t error_code, uint8_t control_mode, int16_t num_errors, int16_t loop_time_us) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN]; + _mav_put_int16_t(buf, 0, num_errors); + _mav_put_int16_t(buf, 2, loop_time_us); + _mav_put_uint8_t(buf, 4, armed); + _mav_put_uint8_t(buf, 5, failsafe); + _mav_put_uint8_t(buf, 6, rc_override); + _mav_put_uint8_t(buf, 7, offboard); + _mav_put_uint8_t(buf, 8, error_code); + _mav_put_uint8_t(buf, 9, control_mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN); +#else + mavlink_rosflight_status_t packet; + packet.num_errors = num_errors; + packet.loop_time_us = loop_time_us; + packet.armed = armed; + packet.failsafe = failsafe; + packet.rc_override = rc_override; + packet.offboard = offboard; + packet.error_code = error_code; + packet.control_mode = control_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_CRC); +} + +/** + * @brief Pack a rosflight_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param armed + * @param failsafe + * @param rc_override + * @param offboard + * @param error_code + * @param control_mode + * @param num_errors + * @param loop_time_us + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rosflight_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t armed, uint8_t failsafe, uint8_t rc_override, uint8_t offboard, uint8_t error_code, uint8_t control_mode, int16_t num_errors, int16_t loop_time_us) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN]; - _mav_put_int16_t(buf, 0, num_errors); - _mav_put_int16_t(buf, 2, loop_time_us); - _mav_put_uint8_t(buf, 4, armed); - _mav_put_uint8_t(buf, 5, failsafe); - _mav_put_uint8_t(buf, 6, rc_override); - _mav_put_uint8_t(buf, 7, offboard); - _mav_put_uint8_t(buf, 8, error_code); - _mav_put_uint8_t(buf, 9, control_mode); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN]; + _mav_put_int16_t(buf, 0, num_errors); + _mav_put_int16_t(buf, 2, loop_time_us); + _mav_put_uint8_t(buf, 4, armed); + _mav_put_uint8_t(buf, 5, failsafe); + _mav_put_uint8_t(buf, 6, rc_override); + _mav_put_uint8_t(buf, 7, offboard); + _mav_put_uint8_t(buf, 8, error_code); + _mav_put_uint8_t(buf, 9, control_mode); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN); #else - mavlink_rosflight_status_t packet; - packet.num_errors = num_errors; - packet.loop_time_us = loop_time_us; - packet.armed = armed; - packet.failsafe = failsafe; - packet.rc_override = rc_override; - packet.offboard = offboard; - packet.error_code = error_code; - packet.control_mode = control_mode; + mavlink_rosflight_status_t packet; + packet.num_errors = num_errors; + packet.loop_time_us = loop_time_us; + packet.armed = armed; + packet.failsafe = failsafe; + packet.rc_override = rc_override; + packet.offboard = offboard; + packet.error_code = error_code; + packet.control_mode = control_mode; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_STATUS; + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_STATUS; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN); #endif } @@ -96,52 +165,48 @@ static inline uint16_t mavlink_msg_rosflight_status_pack(uint8_t system_id, uint * @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 armed - * @param failsafe - * @param rc_override - * @param offboard - * @param error_code - * @param control_mode - * @param num_errors - * @param loop_time_us + * @param armed + * @param failsafe + * @param rc_override + * @param offboard + * @param error_code + * @param control_mode + * @param num_errors + * @param loop_time_us * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t armed,uint8_t failsafe,uint8_t rc_override,uint8_t offboard,uint8_t error_code,uint8_t control_mode,int16_t num_errors,int16_t loop_time_us) + mavlink_message_t* msg, + uint8_t armed,uint8_t failsafe,uint8_t rc_override,uint8_t offboard,uint8_t error_code,uint8_t control_mode,int16_t num_errors,int16_t loop_time_us) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN]; - _mav_put_int16_t(buf, 0, num_errors); - _mav_put_int16_t(buf, 2, loop_time_us); - _mav_put_uint8_t(buf, 4, armed); - _mav_put_uint8_t(buf, 5, failsafe); - _mav_put_uint8_t(buf, 6, rc_override); - _mav_put_uint8_t(buf, 7, offboard); - _mav_put_uint8_t(buf, 8, error_code); - _mav_put_uint8_t(buf, 9, control_mode); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN]; + _mav_put_int16_t(buf, 0, num_errors); + _mav_put_int16_t(buf, 2, loop_time_us); + _mav_put_uint8_t(buf, 4, armed); + _mav_put_uint8_t(buf, 5, failsafe); + _mav_put_uint8_t(buf, 6, rc_override); + _mav_put_uint8_t(buf, 7, offboard); + _mav_put_uint8_t(buf, 8, error_code); + _mav_put_uint8_t(buf, 9, control_mode); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN); #else - mavlink_rosflight_status_t packet; - packet.num_errors = num_errors; - packet.loop_time_us = loop_time_us; - packet.armed = armed; - packet.failsafe = failsafe; - packet.rc_override = rc_override; - packet.offboard = offboard; - packet.error_code = error_code; - packet.control_mode = control_mode; + mavlink_rosflight_status_t packet; + packet.num_errors = num_errors; + packet.loop_time_us = loop_time_us; + packet.armed = armed; + packet.failsafe = failsafe; + packet.rc_override = rc_override; + packet.offboard = offboard; + packet.error_code = error_code; + packet.control_mode = control_mode; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_STATUS; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_CRC); } /** @@ -154,7 +219,7 @@ static inline uint16_t mavlink_msg_rosflight_status_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_rosflight_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rosflight_status_t* rosflight_status) { - return mavlink_msg_rosflight_status_pack(system_id, component_id, msg, rosflight_status->armed, rosflight_status->failsafe, rosflight_status->rc_override, rosflight_status->offboard, rosflight_status->error_code, rosflight_status->control_mode, rosflight_status->num_errors, rosflight_status->loop_time_us); + return mavlink_msg_rosflight_status_pack(system_id, component_id, msg, rosflight_status->armed, rosflight_status->failsafe, rosflight_status->rc_override, rosflight_status->offboard, rosflight_status->error_code, rosflight_status->control_mode, rosflight_status->num_errors, rosflight_status->loop_time_us); } /** @@ -168,64 +233,84 @@ static inline uint16_t mavlink_msg_rosflight_status_encode(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_rosflight_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rosflight_status_t* rosflight_status) { - return mavlink_msg_rosflight_status_pack_chan(system_id, component_id, chan, msg, rosflight_status->armed, rosflight_status->failsafe, rosflight_status->rc_override, rosflight_status->offboard, rosflight_status->error_code, rosflight_status->control_mode, rosflight_status->num_errors, rosflight_status->loop_time_us); + return mavlink_msg_rosflight_status_pack_chan(system_id, component_id, chan, msg, rosflight_status->armed, rosflight_status->failsafe, rosflight_status->rc_override, rosflight_status->offboard, rosflight_status->error_code, rosflight_status->control_mode, rosflight_status->num_errors, rosflight_status->loop_time_us); +} + +/** + * @brief Encode a rosflight_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param rosflight_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rosflight_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_rosflight_status_t* rosflight_status) +{ + return mavlink_msg_rosflight_status_pack_status(system_id, component_id, _status, msg, rosflight_status->armed, rosflight_status->failsafe, rosflight_status->rc_override, rosflight_status->offboard, rosflight_status->error_code, rosflight_status->control_mode, rosflight_status->num_errors, rosflight_status->loop_time_us); } /** * @brief Send a rosflight_status message * @param chan MAVLink channel to send the message * - * @param armed - * @param failsafe - * @param rc_override - * @param offboard - * @param error_code - * @param control_mode - * @param num_errors - * @param loop_time_us + * @param armed + * @param failsafe + * @param rc_override + * @param offboard + * @param error_code + * @param control_mode + * @param num_errors + * @param loop_time_us */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_rosflight_status_send(mavlink_channel_t chan, uint8_t armed, uint8_t failsafe, uint8_t rc_override, uint8_t offboard, uint8_t error_code, uint8_t control_mode, int16_t num_errors, int16_t loop_time_us) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN]; - _mav_put_int16_t(buf, 0, num_errors); - _mav_put_int16_t(buf, 2, loop_time_us); - _mav_put_uint8_t(buf, 4, armed); - _mav_put_uint8_t(buf, 5, failsafe); - _mav_put_uint8_t(buf, 6, rc_override); - _mav_put_uint8_t(buf, 7, offboard); - _mav_put_uint8_t(buf, 8, error_code); - _mav_put_uint8_t(buf, 9, control_mode); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_STATUS, buf, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_CRC); + char buf[MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN]; + _mav_put_int16_t(buf, 0, num_errors); + _mav_put_int16_t(buf, 2, loop_time_us); + _mav_put_uint8_t(buf, 4, armed); + _mav_put_uint8_t(buf, 5, failsafe); + _mav_put_uint8_t(buf, 6, rc_override); + _mav_put_uint8_t(buf, 7, offboard); + _mav_put_uint8_t(buf, 8, error_code); + _mav_put_uint8_t(buf, 9, control_mode); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_STATUS, buf, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_STATUS, buf, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN); + mavlink_rosflight_status_t packet; + packet.num_errors = num_errors; + packet.loop_time_us = loop_time_us; + packet.armed = armed; + packet.failsafe = failsafe; + packet.rc_override = rc_override; + packet.offboard = offboard; + packet.error_code = error_code; + packet.control_mode = control_mode; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_CRC); #endif -#else - mavlink_rosflight_status_t packet; - packet.num_errors = num_errors; - packet.loop_time_us = loop_time_us; - packet.armed = armed; - packet.failsafe = failsafe; - packet.rc_override = rc_override; - packet.offboard = offboard; - packet.error_code = error_code; - packet.control_mode = control_mode; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_CRC); +/** + * @brief Send a rosflight_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_rosflight_status_send_struct(mavlink_channel_t chan, const mavlink_rosflight_status_t* rosflight_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_rosflight_status_send(chan, rosflight_status->armed, rosflight_status->failsafe, rosflight_status->rc_override, rosflight_status->offboard, rosflight_status->error_code, rosflight_status->control_mode, rosflight_status->num_errors, rosflight_status->loop_time_us); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_STATUS, (const char *)rosflight_status, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_CRC); #endif } #if MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -234,37 +319,29 @@ static inline void mavlink_msg_rosflight_status_send(mavlink_channel_t chan, uin static inline void mavlink_msg_rosflight_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t armed, uint8_t failsafe, uint8_t rc_override, uint8_t offboard, uint8_t error_code, uint8_t control_mode, int16_t num_errors, int16_t loop_time_us) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int16_t(buf, 0, num_errors); - _mav_put_int16_t(buf, 2, loop_time_us); - _mav_put_uint8_t(buf, 4, armed); - _mav_put_uint8_t(buf, 5, failsafe); - _mav_put_uint8_t(buf, 6, rc_override); - _mav_put_uint8_t(buf, 7, offboard); - _mav_put_uint8_t(buf, 8, error_code); - _mav_put_uint8_t(buf, 9, control_mode); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_STATUS, buf, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_STATUS, buf, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN); -#endif -#else - mavlink_rosflight_status_t *packet = (mavlink_rosflight_status_t *)msgbuf; - packet->num_errors = num_errors; - packet->loop_time_us = loop_time_us; - packet->armed = armed; - packet->failsafe = failsafe; - packet->rc_override = rc_override; - packet->offboard = offboard; - packet->error_code = error_code; - packet->control_mode = control_mode; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_STATUS, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_CRC); + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, num_errors); + _mav_put_int16_t(buf, 2, loop_time_us); + _mav_put_uint8_t(buf, 4, armed); + _mav_put_uint8_t(buf, 5, failsafe); + _mav_put_uint8_t(buf, 6, rc_override); + _mav_put_uint8_t(buf, 7, offboard); + _mav_put_uint8_t(buf, 8, error_code); + _mav_put_uint8_t(buf, 9, control_mode); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_STATUS, buf, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_STATUS, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN); -#endif + mavlink_rosflight_status_t *packet = (mavlink_rosflight_status_t *)msgbuf; + packet->num_errors = num_errors; + packet->loop_time_us = loop_time_us; + packet->armed = armed; + packet->failsafe = failsafe; + packet->rc_override = rc_override; + packet->offboard = offboard; + packet->error_code = error_code; + packet->control_mode = control_mode; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_STATUS, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_CRC); #endif } #endif @@ -277,81 +354,81 @@ static inline void mavlink_msg_rosflight_status_send_buf(mavlink_message_t *msgb /** * @brief Get field armed from rosflight_status message * - * @return + * @return */ static inline uint8_t mavlink_msg_rosflight_status_get_armed(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 4); + return _MAV_RETURN_uint8_t(msg, 4); } /** * @brief Get field failsafe from rosflight_status message * - * @return + * @return */ static inline uint8_t mavlink_msg_rosflight_status_get_failsafe(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 5); + return _MAV_RETURN_uint8_t(msg, 5); } /** * @brief Get field rc_override from rosflight_status message * - * @return + * @return */ static inline uint8_t mavlink_msg_rosflight_status_get_rc_override(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 6); + return _MAV_RETURN_uint8_t(msg, 6); } /** * @brief Get field offboard from rosflight_status message * - * @return + * @return */ static inline uint8_t mavlink_msg_rosflight_status_get_offboard(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 7); + return _MAV_RETURN_uint8_t(msg, 7); } /** * @brief Get field error_code from rosflight_status message * - * @return + * @return */ static inline uint8_t mavlink_msg_rosflight_status_get_error_code(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 8); + return _MAV_RETURN_uint8_t(msg, 8); } /** * @brief Get field control_mode from rosflight_status message * - * @return + * @return */ static inline uint8_t mavlink_msg_rosflight_status_get_control_mode(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 9); + return _MAV_RETURN_uint8_t(msg, 9); } /** * @brief Get field num_errors from rosflight_status message * - * @return + * @return */ static inline int16_t mavlink_msg_rosflight_status_get_num_errors(const mavlink_message_t* msg) { - return _MAV_RETURN_int16_t(msg, 0); + return _MAV_RETURN_int16_t(msg, 0); } /** * @brief Get field loop_time_us from rosflight_status message * - * @return + * @return */ static inline int16_t mavlink_msg_rosflight_status_get_loop_time_us(const mavlink_message_t* msg) { - return _MAV_RETURN_int16_t(msg, 2); + return _MAV_RETURN_int16_t(msg, 2); } /** @@ -362,16 +439,18 @@ static inline int16_t mavlink_msg_rosflight_status_get_loop_time_us(const mavlin */ static inline void mavlink_msg_rosflight_status_decode(const mavlink_message_t* msg, mavlink_rosflight_status_t* rosflight_status) { -#if MAVLINK_NEED_BYTE_SWAP - rosflight_status->num_errors = mavlink_msg_rosflight_status_get_num_errors(msg); - rosflight_status->loop_time_us = mavlink_msg_rosflight_status_get_loop_time_us(msg); - rosflight_status->armed = mavlink_msg_rosflight_status_get_armed(msg); - rosflight_status->failsafe = mavlink_msg_rosflight_status_get_failsafe(msg); - rosflight_status->rc_override = mavlink_msg_rosflight_status_get_rc_override(msg); - rosflight_status->offboard = mavlink_msg_rosflight_status_get_offboard(msg); - rosflight_status->error_code = mavlink_msg_rosflight_status_get_error_code(msg); - rosflight_status->control_mode = mavlink_msg_rosflight_status_get_control_mode(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + rosflight_status->num_errors = mavlink_msg_rosflight_status_get_num_errors(msg); + rosflight_status->loop_time_us = mavlink_msg_rosflight_status_get_loop_time_us(msg); + rosflight_status->armed = mavlink_msg_rosflight_status_get_armed(msg); + rosflight_status->failsafe = mavlink_msg_rosflight_status_get_failsafe(msg); + rosflight_status->rc_override = mavlink_msg_rosflight_status_get_rc_override(msg); + rosflight_status->offboard = mavlink_msg_rosflight_status_get_offboard(msg); + rosflight_status->error_code = mavlink_msg_rosflight_status_get_error_code(msg); + rosflight_status->control_mode = mavlink_msg_rosflight_status_get_control_mode(msg); #else - memcpy(rosflight_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN? msg->len : MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN; + memset(rosflight_status, 0, MAVLINK_MSG_ID_ROSFLIGHT_STATUS_LEN); + memcpy(rosflight_status, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_version.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_version.h index 3d9fcb93..74cd0145 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_version.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_rosflight_version.h @@ -1,27 +1,39 @@ +#pragma once // MESSAGE ROSFLIGHT_VERSION PACKING #define MAVLINK_MSG_ID_ROSFLIGHT_VERSION 192 -typedef struct __mavlink_rosflight_version_t -{ - char version[50]; /*< */ + +typedef struct __mavlink_rosflight_version_t { + char version[50]; /*< */ } mavlink_rosflight_version_t; #define MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN 50 +#define MAVLINK_MSG_ID_ROSFLIGHT_VERSION_MIN_LEN 50 #define MAVLINK_MSG_ID_192_LEN 50 +#define MAVLINK_MSG_ID_192_MIN_LEN 50 #define MAVLINK_MSG_ID_ROSFLIGHT_VERSION_CRC 134 #define MAVLINK_MSG_ID_192_CRC 134 #define MAVLINK_MSG_ROSFLIGHT_VERSION_FIELD_VERSION_LEN 50 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_ROSFLIGHT_VERSION { \ - "ROSFLIGHT_VERSION", \ - 1, \ - { { "version", NULL, MAVLINK_TYPE_CHAR, 50, 0, offsetof(mavlink_rosflight_version_t, version) }, \ + 192, \ + "ROSFLIGHT_VERSION", \ + 1, \ + { { "version", NULL, MAVLINK_TYPE_CHAR, 50, 0, offsetof(mavlink_rosflight_version_t, version) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_ROSFLIGHT_VERSION { \ + "ROSFLIGHT_VERSION", \ + 1, \ + { { "version", NULL, MAVLINK_TYPE_CHAR, 50, 0, offsetof(mavlink_rosflight_version_t, version) }, \ + } \ +} +#endif /** * @brief Pack a rosflight_version message @@ -29,29 +41,58 @@ typedef struct __mavlink_rosflight_version_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param version + * @param version * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_version_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const char *version) + const char *version) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN]; + + _mav_put_char_array(buf, 0, version, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN); +#else + mavlink_rosflight_version_t packet; + + mav_array_memcpy(packet.version, version, sizeof(char)*50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_VERSION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_CRC); +} + +/** + * @brief Pack a rosflight_version message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param version + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rosflight_version_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + const char *version) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN]; + char buf[MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN]; - _mav_put_char_array(buf, 0, version, 50); + _mav_put_char_array(buf, 0, version, 50); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN); #else - mavlink_rosflight_version_t packet; + mavlink_rosflight_version_t packet; - mav_array_memcpy(packet.version, version, sizeof(char)*50); + mav_array_memcpy(packet.version, version, sizeof(char)*50); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_VERSION; + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_VERSION; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN); #endif } @@ -61,31 +102,27 @@ static inline uint16_t mavlink_msg_rosflight_version_pack(uint8_t system_id, uin * @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 + * @param version * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rosflight_version_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const char *version) + mavlink_message_t* msg, + const char *version) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN]; + char buf[MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN]; - _mav_put_char_array(buf, 0, version, 50); + _mav_put_char_array(buf, 0, version, 50); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN); #else - mavlink_rosflight_version_t packet; + mavlink_rosflight_version_t packet; - mav_array_memcpy(packet.version, version, sizeof(char)*50); + mav_array_memcpy(packet.version, version, sizeof(char)*50); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_VERSION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_ROSFLIGHT_VERSION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_CRC); } /** @@ -98,7 +135,7 @@ static inline uint16_t mavlink_msg_rosflight_version_pack_chan(uint8_t system_id */ static inline uint16_t mavlink_msg_rosflight_version_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rosflight_version_t* rosflight_version) { - return mavlink_msg_rosflight_version_pack(system_id, component_id, msg, rosflight_version->version); + return mavlink_msg_rosflight_version_pack(system_id, component_id, msg, rosflight_version->version); } /** @@ -112,43 +149,63 @@ static inline uint16_t mavlink_msg_rosflight_version_encode(uint8_t system_id, u */ static inline uint16_t mavlink_msg_rosflight_version_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rosflight_version_t* rosflight_version) { - return mavlink_msg_rosflight_version_pack_chan(system_id, component_id, chan, msg, rosflight_version->version); + return mavlink_msg_rosflight_version_pack_chan(system_id, component_id, chan, msg, rosflight_version->version); +} + +/** + * @brief Encode a rosflight_version struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param rosflight_version C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rosflight_version_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_rosflight_version_t* rosflight_version) +{ + return mavlink_msg_rosflight_version_pack_status(system_id, component_id, _status, msg, rosflight_version->version); } /** * @brief Send a rosflight_version message * @param chan MAVLink channel to send the message * - * @param version + * @param version */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_rosflight_version_send(mavlink_channel_t chan, const char *version) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN]; + char buf[MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN]; - _mav_put_char_array(buf, 0, version, 50); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_VERSION, buf, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_CRC); + _mav_put_char_array(buf, 0, version, 50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_VERSION, buf, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_VERSION, buf, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN); + mavlink_rosflight_version_t packet; + + mav_array_memcpy(packet.version, version, sizeof(char)*50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_VERSION, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_CRC); #endif -#else - mavlink_rosflight_version_t packet; +} - mav_array_memcpy(packet.version, version, sizeof(char)*50); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_VERSION, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_CRC); +/** + * @brief Send a rosflight_version message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_rosflight_version_send_struct(mavlink_channel_t chan, const mavlink_rosflight_version_t* rosflight_version) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_rosflight_version_send(chan, rosflight_version->version); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_VERSION, (const char *)&packet, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_VERSION, (const char *)rosflight_version, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_CRC); #endif } #if MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -157,23 +214,15 @@ static inline void mavlink_msg_rosflight_version_send(mavlink_channel_t chan, co static inline void mavlink_msg_rosflight_version_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *version) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; + char *buf = (char *)msgbuf; - _mav_put_char_array(buf, 0, version, 50); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_VERSION, buf, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_VERSION, buf, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN); -#endif + _mav_put_char_array(buf, 0, version, 50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_VERSION, buf, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_CRC); #else - mavlink_rosflight_version_t *packet = (mavlink_rosflight_version_t *)msgbuf; + mavlink_rosflight_version_t *packet = (mavlink_rosflight_version_t *)msgbuf; - mav_array_memcpy(packet->version, version, sizeof(char)*50); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_VERSION, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_VERSION, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN); -#endif + mav_array_memcpy(packet->version, version, sizeof(char)*50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROSFLIGHT_VERSION, (const char *)packet, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_MIN_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_CRC); #endif } #endif @@ -186,11 +235,11 @@ static inline void mavlink_msg_rosflight_version_send_buf(mavlink_message_t *msg /** * @brief Get field version from rosflight_version message * - * @return + * @return */ static inline uint16_t mavlink_msg_rosflight_version_get_version(const mavlink_message_t* msg, char *version) { - return _MAV_RETURN_char_array(msg, version, 50, 0); + return _MAV_RETURN_char_array(msg, version, 50, 0); } /** @@ -201,9 +250,11 @@ static inline uint16_t mavlink_msg_rosflight_version_get_version(const mavlink_m */ static inline void mavlink_msg_rosflight_version_decode(const mavlink_message_t* msg, mavlink_rosflight_version_t* rosflight_version) { -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_rosflight_version_get_version(msg, rosflight_version->version); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_rosflight_version_get_version(msg, rosflight_version->version); #else - memcpy(rosflight_version, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN? msg->len : MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN; + memset(rosflight_version, 0, MAVLINK_MSG_ID_ROSFLIGHT_VERSION_LEN); + memcpy(rosflight_version, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_small_baro.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_small_baro.h index 19ecc321..672353a5 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_small_baro.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_small_baro.h @@ -1,31 +1,45 @@ +#pragma once // MESSAGE SMALL_BARO PACKING #define MAVLINK_MSG_ID_SMALL_BARO 183 -typedef struct __mavlink_small_baro_t -{ - float altitude; /*< Calculated Altitude (m)*/ - float pressure; /*< Measured Differential Pressure (Pa)*/ - float temperature; /*< Measured Temperature (K)*/ + +typedef struct __mavlink_small_baro_t { + float altitude; /*< Calculated Altitude (m)*/ + float pressure; /*< Measured Differential Pressure (Pa)*/ + float temperature; /*< Measured Temperature (K)*/ } mavlink_small_baro_t; #define MAVLINK_MSG_ID_SMALL_BARO_LEN 12 +#define MAVLINK_MSG_ID_SMALL_BARO_MIN_LEN 12 #define MAVLINK_MSG_ID_183_LEN 12 +#define MAVLINK_MSG_ID_183_MIN_LEN 12 #define MAVLINK_MSG_ID_SMALL_BARO_CRC 206 #define MAVLINK_MSG_ID_183_CRC 206 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_SMALL_BARO { \ - "SMALL_BARO", \ - 3, \ - { { "altitude", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_small_baro_t, altitude) }, \ + 183, \ + "SMALL_BARO", \ + 3, \ + { { "altitude", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_small_baro_t, altitude) }, \ { "pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_small_baro_t, pressure) }, \ { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_small_baro_t, temperature) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_SMALL_BARO { \ + "SMALL_BARO", \ + 3, \ + { { "altitude", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_small_baro_t, altitude) }, \ + { "pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_small_baro_t, pressure) }, \ + { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_small_baro_t, temperature) }, \ + } \ +} +#endif /** * @brief Pack a small_baro message @@ -33,35 +47,70 @@ typedef struct __mavlink_small_baro_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param altitude Calculated Altitude (m) - * @param pressure Measured Differential Pressure (Pa) - * @param temperature Measured Temperature (K) + * @param altitude Calculated Altitude (m) + * @param pressure Measured Differential Pressure (Pa) + * @param temperature Measured Temperature (K) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_small_baro_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float altitude, float pressure, float temperature) + float altitude, float pressure, float temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SMALL_BARO_LEN]; - _mav_put_float(buf, 0, altitude); - _mav_put_float(buf, 4, pressure); - _mav_put_float(buf, 8, temperature); + char buf[MAVLINK_MSG_ID_SMALL_BARO_LEN]; + _mav_put_float(buf, 0, altitude); + _mav_put_float(buf, 4, pressure); + _mav_put_float(buf, 8, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMALL_BARO_LEN); #else - mavlink_small_baro_t packet; - packet.altitude = altitude; - packet.pressure = pressure; - packet.temperature = temperature; + mavlink_small_baro_t packet; + packet.altitude = altitude; + packet.pressure = pressure; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMALL_BARO_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_SMALL_BARO; + msg->msgid = MAVLINK_MSG_ID_SMALL_BARO; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SMALL_BARO_MIN_LEN, MAVLINK_MSG_ID_SMALL_BARO_LEN, MAVLINK_MSG_ID_SMALL_BARO_CRC); +} + +/** + * @brief Pack a small_baro message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param altitude Calculated Altitude (m) + * @param pressure Measured Differential Pressure (Pa) + * @param temperature Measured Temperature (K) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_small_baro_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + float altitude, float pressure, float temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SMALL_BARO_LEN]; + _mav_put_float(buf, 0, altitude); + _mav_put_float(buf, 4, pressure); + _mav_put_float(buf, 8, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMALL_BARO_LEN); +#else + mavlink_small_baro_t packet; + packet.altitude = altitude; + packet.pressure = pressure; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMALL_BARO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SMALL_BARO; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SMALL_BARO_LEN, MAVLINK_MSG_ID_SMALL_BARO_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SMALL_BARO_MIN_LEN, MAVLINK_MSG_ID_SMALL_BARO_LEN, MAVLINK_MSG_ID_SMALL_BARO_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SMALL_BARO_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SMALL_BARO_MIN_LEN, MAVLINK_MSG_ID_SMALL_BARO_LEN); #endif } @@ -71,37 +120,33 @@ static inline uint16_t mavlink_msg_small_baro_pack(uint8_t system_id, uint8_t co * @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 altitude Calculated Altitude (m) - * @param pressure Measured Differential Pressure (Pa) - * @param temperature Measured Temperature (K) + * @param altitude Calculated Altitude (m) + * @param pressure Measured Differential Pressure (Pa) + * @param temperature Measured Temperature (K) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_small_baro_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float altitude,float pressure,float temperature) + mavlink_message_t* msg, + float altitude,float pressure,float temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SMALL_BARO_LEN]; - _mav_put_float(buf, 0, altitude); - _mav_put_float(buf, 4, pressure); - _mav_put_float(buf, 8, temperature); + char buf[MAVLINK_MSG_ID_SMALL_BARO_LEN]; + _mav_put_float(buf, 0, altitude); + _mav_put_float(buf, 4, pressure); + _mav_put_float(buf, 8, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMALL_BARO_LEN); #else - mavlink_small_baro_t packet; - packet.altitude = altitude; - packet.pressure = pressure; - packet.temperature = temperature; + mavlink_small_baro_t packet; + packet.altitude = altitude; + packet.pressure = pressure; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMALL_BARO_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_SMALL_BARO; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SMALL_BARO_LEN, MAVLINK_MSG_ID_SMALL_BARO_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SMALL_BARO_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_SMALL_BARO; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SMALL_BARO_MIN_LEN, MAVLINK_MSG_ID_SMALL_BARO_LEN, MAVLINK_MSG_ID_SMALL_BARO_CRC); } /** @@ -114,7 +159,7 @@ static inline uint16_t mavlink_msg_small_baro_pack_chan(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_small_baro_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_small_baro_t* small_baro) { - return mavlink_msg_small_baro_pack(system_id, component_id, msg, small_baro->altitude, small_baro->pressure, small_baro->temperature); + return mavlink_msg_small_baro_pack(system_id, component_id, msg, small_baro->altitude, small_baro->pressure, small_baro->temperature); } /** @@ -128,49 +173,69 @@ static inline uint16_t mavlink_msg_small_baro_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_small_baro_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_small_baro_t* small_baro) { - return mavlink_msg_small_baro_pack_chan(system_id, component_id, chan, msg, small_baro->altitude, small_baro->pressure, small_baro->temperature); + return mavlink_msg_small_baro_pack_chan(system_id, component_id, chan, msg, small_baro->altitude, small_baro->pressure, small_baro->temperature); +} + +/** + * @brief Encode a small_baro struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param small_baro C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_small_baro_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_small_baro_t* small_baro) +{ + return mavlink_msg_small_baro_pack_status(system_id, component_id, _status, msg, small_baro->altitude, small_baro->pressure, small_baro->temperature); } /** * @brief Send a small_baro message * @param chan MAVLink channel to send the message * - * @param altitude Calculated Altitude (m) - * @param pressure Measured Differential Pressure (Pa) - * @param temperature Measured Temperature (K) + * @param altitude Calculated Altitude (m) + * @param pressure Measured Differential Pressure (Pa) + * @param temperature Measured Temperature (K) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_small_baro_send(mavlink_channel_t chan, float altitude, float pressure, float temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SMALL_BARO_LEN]; - _mav_put_float(buf, 0, altitude); - _mav_put_float(buf, 4, pressure); - _mav_put_float(buf, 8, temperature); + char buf[MAVLINK_MSG_ID_SMALL_BARO_LEN]; + _mav_put_float(buf, 0, altitude); + _mav_put_float(buf, 4, pressure); + _mav_put_float(buf, 8, temperature); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_BARO, buf, MAVLINK_MSG_ID_SMALL_BARO_LEN, MAVLINK_MSG_ID_SMALL_BARO_CRC); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_BARO, buf, MAVLINK_MSG_ID_SMALL_BARO_MIN_LEN, MAVLINK_MSG_ID_SMALL_BARO_LEN, MAVLINK_MSG_ID_SMALL_BARO_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_BARO, buf, MAVLINK_MSG_ID_SMALL_BARO_LEN); + mavlink_small_baro_t packet; + packet.altitude = altitude; + packet.pressure = pressure; + packet.temperature = temperature; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_BARO, (const char *)&packet, MAVLINK_MSG_ID_SMALL_BARO_MIN_LEN, MAVLINK_MSG_ID_SMALL_BARO_LEN, MAVLINK_MSG_ID_SMALL_BARO_CRC); #endif -#else - mavlink_small_baro_t packet; - packet.altitude = altitude; - packet.pressure = pressure; - packet.temperature = temperature; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_BARO, (const char *)&packet, MAVLINK_MSG_ID_SMALL_BARO_LEN, MAVLINK_MSG_ID_SMALL_BARO_CRC); +/** + * @brief Send a small_baro message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_small_baro_send_struct(mavlink_channel_t chan, const mavlink_small_baro_t* small_baro) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_small_baro_send(chan, small_baro->altitude, small_baro->pressure, small_baro->temperature); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_BARO, (const char *)&packet, MAVLINK_MSG_ID_SMALL_BARO_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_BARO, (const char *)small_baro, MAVLINK_MSG_ID_SMALL_BARO_MIN_LEN, MAVLINK_MSG_ID_SMALL_BARO_LEN, MAVLINK_MSG_ID_SMALL_BARO_CRC); #endif } #if MAVLINK_MSG_ID_SMALL_BARO_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -179,27 +244,19 @@ static inline void mavlink_msg_small_baro_send(mavlink_channel_t chan, float alt static inline void mavlink_msg_small_baro_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float altitude, float pressure, float temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, altitude); - _mav_put_float(buf, 4, pressure); - _mav_put_float(buf, 8, temperature); + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, altitude); + _mav_put_float(buf, 4, pressure); + _mav_put_float(buf, 8, temperature); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_BARO, buf, MAVLINK_MSG_ID_SMALL_BARO_LEN, MAVLINK_MSG_ID_SMALL_BARO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_BARO, buf, MAVLINK_MSG_ID_SMALL_BARO_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_BARO, buf, MAVLINK_MSG_ID_SMALL_BARO_MIN_LEN, MAVLINK_MSG_ID_SMALL_BARO_LEN, MAVLINK_MSG_ID_SMALL_BARO_CRC); #else - mavlink_small_baro_t *packet = (mavlink_small_baro_t *)msgbuf; - packet->altitude = altitude; - packet->pressure = pressure; - packet->temperature = temperature; + mavlink_small_baro_t *packet = (mavlink_small_baro_t *)msgbuf; + packet->altitude = altitude; + packet->pressure = pressure; + packet->temperature = temperature; -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_BARO, (const char *)packet, MAVLINK_MSG_ID_SMALL_BARO_LEN, MAVLINK_MSG_ID_SMALL_BARO_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_BARO, (const char *)packet, MAVLINK_MSG_ID_SMALL_BARO_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_BARO, (const char *)packet, MAVLINK_MSG_ID_SMALL_BARO_MIN_LEN, MAVLINK_MSG_ID_SMALL_BARO_LEN, MAVLINK_MSG_ID_SMALL_BARO_CRC); #endif } #endif @@ -212,31 +269,31 @@ static inline void mavlink_msg_small_baro_send_buf(mavlink_message_t *msgbuf, ma /** * @brief Get field altitude from small_baro message * - * @return Calculated Altitude (m) + * @return Calculated Altitude (m) */ static inline float mavlink_msg_small_baro_get_altitude(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** * @brief Get field pressure from small_baro message * - * @return Measured Differential Pressure (Pa) + * @return Measured Differential Pressure (Pa) */ static inline float mavlink_msg_small_baro_get_pressure(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** * @brief Get field temperature from small_baro message * - * @return Measured Temperature (K) + * @return Measured Temperature (K) */ static inline float mavlink_msg_small_baro_get_temperature(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -247,11 +304,13 @@ static inline float mavlink_msg_small_baro_get_temperature(const mavlink_message */ static inline void mavlink_msg_small_baro_decode(const mavlink_message_t* msg, mavlink_small_baro_t* small_baro) { -#if MAVLINK_NEED_BYTE_SWAP - small_baro->altitude = mavlink_msg_small_baro_get_altitude(msg); - small_baro->pressure = mavlink_msg_small_baro_get_pressure(msg); - small_baro->temperature = mavlink_msg_small_baro_get_temperature(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + small_baro->altitude = mavlink_msg_small_baro_get_altitude(msg); + small_baro->pressure = mavlink_msg_small_baro_get_pressure(msg); + small_baro->temperature = mavlink_msg_small_baro_get_temperature(msg); #else - memcpy(small_baro, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SMALL_BARO_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_SMALL_BARO_LEN? msg->len : MAVLINK_MSG_ID_SMALL_BARO_LEN; + memset(small_baro, 0, MAVLINK_MSG_ID_SMALL_BARO_LEN); + memcpy(small_baro, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_small_imu.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_small_imu.h index c60cfdec..f548a522 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_small_imu.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_small_imu.h @@ -1,31 +1,36 @@ +#pragma once // MESSAGE SMALL_IMU PACKING #define MAVLINK_MSG_ID_SMALL_IMU 181 -typedef struct __mavlink_small_imu_t -{ - uint64_t time_boot_us; /*< Measurement timestamp as microseconds since boot*/ - float xacc; /*< Acceleration along X axis*/ - float yacc; /*< Acceleration along Y axis*/ - float zacc; /*< Acceleration along Z axis*/ - float xgyro; /*< Angular speed around X axis*/ - float ygyro; /*< Angular speed around Y axis*/ - float zgyro; /*< Angular speed around Z axis*/ - float temperature; /*< Internal temperature measurement*/ + +typedef struct __mavlink_small_imu_t { + uint64_t time_boot_us; /*< Measurement timestamp as microseconds since boot*/ + float xacc; /*< Acceleration along X axis*/ + float yacc; /*< Acceleration along Y axis*/ + float zacc; /*< Acceleration along Z axis*/ + float xgyro; /*< Angular speed around X axis*/ + float ygyro; /*< Angular speed around Y axis*/ + float zgyro; /*< Angular speed around Z axis*/ + float temperature; /*< Internal temperature measurement*/ } mavlink_small_imu_t; #define MAVLINK_MSG_ID_SMALL_IMU_LEN 36 +#define MAVLINK_MSG_ID_SMALL_IMU_MIN_LEN 36 #define MAVLINK_MSG_ID_181_LEN 36 +#define MAVLINK_MSG_ID_181_MIN_LEN 36 #define MAVLINK_MSG_ID_SMALL_IMU_CRC 67 #define MAVLINK_MSG_ID_181_CRC 67 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_SMALL_IMU { \ - "SMALL_IMU", \ - 8, \ - { { "time_boot_us", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_small_imu_t, time_boot_us) }, \ + 181, \ + "SMALL_IMU", \ + 8, \ + { { "time_boot_us", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_small_imu_t, time_boot_us) }, \ { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_small_imu_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_small_imu_t, yacc) }, \ { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_small_imu_t, zacc) }, \ @@ -35,7 +40,21 @@ typedef struct __mavlink_small_imu_t { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_small_imu_t, temperature) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_SMALL_IMU { \ + "SMALL_IMU", \ + 8, \ + { { "time_boot_us", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_small_imu_t, time_boot_us) }, \ + { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_small_imu_t, xacc) }, \ + { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_small_imu_t, yacc) }, \ + { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_small_imu_t, zacc) }, \ + { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_small_imu_t, xgyro) }, \ + { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_small_imu_t, ygyro) }, \ + { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_small_imu_t, zgyro) }, \ + { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_small_imu_t, temperature) }, \ + } \ +} +#endif /** * @brief Pack a small_imu message @@ -43,50 +62,100 @@ typedef struct __mavlink_small_imu_t * @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_us Measurement timestamp as microseconds since boot - * @param xacc Acceleration along X axis - * @param yacc Acceleration along Y axis - * @param zacc Acceleration along Z axis - * @param xgyro Angular speed around X axis - * @param ygyro Angular speed around Y axis - * @param zgyro Angular speed around Z axis - * @param temperature Internal temperature measurement + * @param time_boot_us Measurement timestamp as microseconds since boot + * @param xacc Acceleration along X axis + * @param yacc Acceleration along Y axis + * @param zacc Acceleration along Z axis + * @param xgyro Angular speed around X axis + * @param ygyro Angular speed around Y axis + * @param zgyro Angular speed around Z axis + * @param temperature Internal temperature measurement * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_small_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_boot_us, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float temperature) + uint64_t time_boot_us, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SMALL_IMU_LEN]; + _mav_put_uint64_t(buf, 0, time_boot_us); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMALL_IMU_LEN); +#else + mavlink_small_imu_t packet; + packet.time_boot_us = time_boot_us; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMALL_IMU_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SMALL_IMU; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SMALL_IMU_MIN_LEN, MAVLINK_MSG_ID_SMALL_IMU_LEN, MAVLINK_MSG_ID_SMALL_IMU_CRC); +} + +/** + * @brief Pack a small_imu message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_us Measurement timestamp as microseconds since boot + * @param xacc Acceleration along X axis + * @param yacc Acceleration along Y axis + * @param zacc Acceleration along Z axis + * @param xgyro Angular speed around X axis + * @param ygyro Angular speed around Y axis + * @param zgyro Angular speed around Z axis + * @param temperature Internal temperature measurement + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_small_imu_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_boot_us, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SMALL_IMU_LEN]; - _mav_put_uint64_t(buf, 0, time_boot_us); - _mav_put_float(buf, 8, xacc); - _mav_put_float(buf, 12, yacc); - _mav_put_float(buf, 16, zacc); - _mav_put_float(buf, 20, xgyro); - _mav_put_float(buf, 24, ygyro); - _mav_put_float(buf, 28, zgyro); - _mav_put_float(buf, 32, temperature); + char buf[MAVLINK_MSG_ID_SMALL_IMU_LEN]; + _mav_put_uint64_t(buf, 0, time_boot_us); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMALL_IMU_LEN); #else - mavlink_small_imu_t packet; - packet.time_boot_us = time_boot_us; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.temperature = temperature; + mavlink_small_imu_t packet; + packet.time_boot_us = time_boot_us; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMALL_IMU_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_SMALL_IMU; + msg->msgid = MAVLINK_MSG_ID_SMALL_IMU; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SMALL_IMU_LEN, MAVLINK_MSG_ID_SMALL_IMU_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SMALL_IMU_MIN_LEN, MAVLINK_MSG_ID_SMALL_IMU_LEN, MAVLINK_MSG_ID_SMALL_IMU_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SMALL_IMU_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SMALL_IMU_MIN_LEN, MAVLINK_MSG_ID_SMALL_IMU_LEN); #endif } @@ -96,52 +165,48 @@ static inline uint16_t mavlink_msg_small_imu_pack(uint8_t system_id, uint8_t com * @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_us Measurement timestamp as microseconds since boot - * @param xacc Acceleration along X axis - * @param yacc Acceleration along Y axis - * @param zacc Acceleration along Z axis - * @param xgyro Angular speed around X axis - * @param ygyro Angular speed around Y axis - * @param zgyro Angular speed around Z axis - * @param temperature Internal temperature measurement + * @param time_boot_us Measurement timestamp as microseconds since boot + * @param xacc Acceleration along X axis + * @param yacc Acceleration along Y axis + * @param zacc Acceleration along Z axis + * @param xgyro Angular speed around X axis + * @param ygyro Angular speed around Y axis + * @param zgyro Angular speed around Z axis + * @param temperature Internal temperature measurement * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_small_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t time_boot_us,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float temperature) + mavlink_message_t* msg, + uint64_t time_boot_us,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SMALL_IMU_LEN]; - _mav_put_uint64_t(buf, 0, time_boot_us); - _mav_put_float(buf, 8, xacc); - _mav_put_float(buf, 12, yacc); - _mav_put_float(buf, 16, zacc); - _mav_put_float(buf, 20, xgyro); - _mav_put_float(buf, 24, ygyro); - _mav_put_float(buf, 28, zgyro); - _mav_put_float(buf, 32, temperature); + char buf[MAVLINK_MSG_ID_SMALL_IMU_LEN]; + _mav_put_uint64_t(buf, 0, time_boot_us); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMALL_IMU_LEN); #else - mavlink_small_imu_t packet; - packet.time_boot_us = time_boot_us; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.temperature = temperature; + mavlink_small_imu_t packet; + packet.time_boot_us = time_boot_us; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMALL_IMU_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_SMALL_IMU; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SMALL_IMU_LEN, MAVLINK_MSG_ID_SMALL_IMU_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SMALL_IMU_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_SMALL_IMU; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SMALL_IMU_MIN_LEN, MAVLINK_MSG_ID_SMALL_IMU_LEN, MAVLINK_MSG_ID_SMALL_IMU_CRC); } /** @@ -154,7 +219,7 @@ static inline uint16_t mavlink_msg_small_imu_pack_chan(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_small_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_small_imu_t* small_imu) { - return mavlink_msg_small_imu_pack(system_id, component_id, msg, small_imu->time_boot_us, small_imu->xacc, small_imu->yacc, small_imu->zacc, small_imu->xgyro, small_imu->ygyro, small_imu->zgyro, small_imu->temperature); + return mavlink_msg_small_imu_pack(system_id, component_id, msg, small_imu->time_boot_us, small_imu->xacc, small_imu->yacc, small_imu->zacc, small_imu->xgyro, small_imu->ygyro, small_imu->zgyro, small_imu->temperature); } /** @@ -168,64 +233,84 @@ static inline uint16_t mavlink_msg_small_imu_encode(uint8_t system_id, uint8_t c */ static inline uint16_t mavlink_msg_small_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_small_imu_t* small_imu) { - return mavlink_msg_small_imu_pack_chan(system_id, component_id, chan, msg, small_imu->time_boot_us, small_imu->xacc, small_imu->yacc, small_imu->zacc, small_imu->xgyro, small_imu->ygyro, small_imu->zgyro, small_imu->temperature); + return mavlink_msg_small_imu_pack_chan(system_id, component_id, chan, msg, small_imu->time_boot_us, small_imu->xacc, small_imu->yacc, small_imu->zacc, small_imu->xgyro, small_imu->ygyro, small_imu->zgyro, small_imu->temperature); +} + +/** + * @brief Encode a small_imu struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param small_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_small_imu_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_small_imu_t* small_imu) +{ + return mavlink_msg_small_imu_pack_status(system_id, component_id, _status, msg, small_imu->time_boot_us, small_imu->xacc, small_imu->yacc, small_imu->zacc, small_imu->xgyro, small_imu->ygyro, small_imu->zgyro, small_imu->temperature); } /** * @brief Send a small_imu message * @param chan MAVLink channel to send the message * - * @param time_boot_us Measurement timestamp as microseconds since boot - * @param xacc Acceleration along X axis - * @param yacc Acceleration along Y axis - * @param zacc Acceleration along Z axis - * @param xgyro Angular speed around X axis - * @param ygyro Angular speed around Y axis - * @param zgyro Angular speed around Z axis - * @param temperature Internal temperature measurement + * @param time_boot_us Measurement timestamp as microseconds since boot + * @param xacc Acceleration along X axis + * @param yacc Acceleration along Y axis + * @param zacc Acceleration along Z axis + * @param xgyro Angular speed around X axis + * @param ygyro Angular speed around Y axis + * @param zgyro Angular speed around Z axis + * @param temperature Internal temperature measurement */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_small_imu_send(mavlink_channel_t chan, uint64_t time_boot_us, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SMALL_IMU_LEN]; - _mav_put_uint64_t(buf, 0, time_boot_us); - _mav_put_float(buf, 8, xacc); - _mav_put_float(buf, 12, yacc); - _mav_put_float(buf, 16, zacc); - _mav_put_float(buf, 20, xgyro); - _mav_put_float(buf, 24, ygyro); - _mav_put_float(buf, 28, zgyro); - _mav_put_float(buf, 32, temperature); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_IMU, buf, MAVLINK_MSG_ID_SMALL_IMU_LEN, MAVLINK_MSG_ID_SMALL_IMU_CRC); + char buf[MAVLINK_MSG_ID_SMALL_IMU_LEN]; + _mav_put_uint64_t(buf, 0, time_boot_us); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, temperature); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_IMU, buf, MAVLINK_MSG_ID_SMALL_IMU_MIN_LEN, MAVLINK_MSG_ID_SMALL_IMU_LEN, MAVLINK_MSG_ID_SMALL_IMU_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_IMU, buf, MAVLINK_MSG_ID_SMALL_IMU_LEN); + mavlink_small_imu_t packet; + packet.time_boot_us = time_boot_us; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.temperature = temperature; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_IMU, (const char *)&packet, MAVLINK_MSG_ID_SMALL_IMU_MIN_LEN, MAVLINK_MSG_ID_SMALL_IMU_LEN, MAVLINK_MSG_ID_SMALL_IMU_CRC); #endif -#else - mavlink_small_imu_t packet; - packet.time_boot_us = time_boot_us; - packet.xacc = xacc; - packet.yacc = yacc; - packet.zacc = zacc; - packet.xgyro = xgyro; - packet.ygyro = ygyro; - packet.zgyro = zgyro; - packet.temperature = temperature; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_IMU, (const char *)&packet, MAVLINK_MSG_ID_SMALL_IMU_LEN, MAVLINK_MSG_ID_SMALL_IMU_CRC); +/** + * @brief Send a small_imu message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_small_imu_send_struct(mavlink_channel_t chan, const mavlink_small_imu_t* small_imu) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_small_imu_send(chan, small_imu->time_boot_us, small_imu->xacc, small_imu->yacc, small_imu->zacc, small_imu->xgyro, small_imu->ygyro, small_imu->zgyro, small_imu->temperature); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_IMU, (const char *)&packet, MAVLINK_MSG_ID_SMALL_IMU_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_IMU, (const char *)small_imu, MAVLINK_MSG_ID_SMALL_IMU_MIN_LEN, MAVLINK_MSG_ID_SMALL_IMU_LEN, MAVLINK_MSG_ID_SMALL_IMU_CRC); #endif } #if MAVLINK_MSG_ID_SMALL_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -234,37 +319,29 @@ static inline void mavlink_msg_small_imu_send(mavlink_channel_t chan, uint64_t t static inline void mavlink_msg_small_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_boot_us, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint64_t(buf, 0, time_boot_us); - _mav_put_float(buf, 8, xacc); - _mav_put_float(buf, 12, yacc); - _mav_put_float(buf, 16, zacc); - _mav_put_float(buf, 20, xgyro); - _mav_put_float(buf, 24, ygyro); - _mav_put_float(buf, 28, zgyro); - _mav_put_float(buf, 32, temperature); - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_IMU, buf, MAVLINK_MSG_ID_SMALL_IMU_LEN, MAVLINK_MSG_ID_SMALL_IMU_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_IMU, buf, MAVLINK_MSG_ID_SMALL_IMU_LEN); -#endif -#else - mavlink_small_imu_t *packet = (mavlink_small_imu_t *)msgbuf; - packet->time_boot_us = time_boot_us; - packet->xacc = xacc; - packet->yacc = yacc; - packet->zacc = zacc; - packet->xgyro = xgyro; - packet->ygyro = ygyro; - packet->zgyro = zgyro; - packet->temperature = temperature; - -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_IMU, (const char *)packet, MAVLINK_MSG_ID_SMALL_IMU_LEN, MAVLINK_MSG_ID_SMALL_IMU_CRC); + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_boot_us); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, temperature); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_IMU, buf, MAVLINK_MSG_ID_SMALL_IMU_MIN_LEN, MAVLINK_MSG_ID_SMALL_IMU_LEN, MAVLINK_MSG_ID_SMALL_IMU_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_IMU, (const char *)packet, MAVLINK_MSG_ID_SMALL_IMU_LEN); -#endif + mavlink_small_imu_t *packet = (mavlink_small_imu_t *)msgbuf; + packet->time_boot_us = time_boot_us; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->temperature = temperature; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_IMU, (const char *)packet, MAVLINK_MSG_ID_SMALL_IMU_MIN_LEN, MAVLINK_MSG_ID_SMALL_IMU_LEN, MAVLINK_MSG_ID_SMALL_IMU_CRC); #endif } #endif @@ -277,81 +354,81 @@ static inline void mavlink_msg_small_imu_send_buf(mavlink_message_t *msgbuf, mav /** * @brief Get field time_boot_us from small_imu message * - * @return Measurement timestamp as microseconds since boot + * @return Measurement timestamp as microseconds since boot */ static inline uint64_t mavlink_msg_small_imu_get_time_boot_us(const mavlink_message_t* msg) { - return _MAV_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** * @brief Get field xacc from small_imu message * - * @return Acceleration along X axis + * @return Acceleration along X axis */ static inline float mavlink_msg_small_imu_get_xacc(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** * @brief Get field yacc from small_imu message * - * @return Acceleration along Y axis + * @return Acceleration along Y axis */ static inline float mavlink_msg_small_imu_get_yacc(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** * @brief Get field zacc from small_imu message * - * @return Acceleration along Z axis + * @return Acceleration along Z axis */ static inline float mavlink_msg_small_imu_get_zacc(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** * @brief Get field xgyro from small_imu message * - * @return Angular speed around X axis + * @return Angular speed around X axis */ static inline float mavlink_msg_small_imu_get_xgyro(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** * @brief Get field ygyro from small_imu message * - * @return Angular speed around Y axis + * @return Angular speed around Y axis */ static inline float mavlink_msg_small_imu_get_ygyro(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 24); + return _MAV_RETURN_float(msg, 24); } /** * @brief Get field zgyro from small_imu message * - * @return Angular speed around Z axis + * @return Angular speed around Z axis */ static inline float mavlink_msg_small_imu_get_zgyro(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 28); + return _MAV_RETURN_float(msg, 28); } /** * @brief Get field temperature from small_imu message * - * @return Internal temperature measurement + * @return Internal temperature measurement */ static inline float mavlink_msg_small_imu_get_temperature(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 32); + return _MAV_RETURN_float(msg, 32); } /** @@ -362,16 +439,18 @@ static inline float mavlink_msg_small_imu_get_temperature(const mavlink_message_ */ static inline void mavlink_msg_small_imu_decode(const mavlink_message_t* msg, mavlink_small_imu_t* small_imu) { -#if MAVLINK_NEED_BYTE_SWAP - small_imu->time_boot_us = mavlink_msg_small_imu_get_time_boot_us(msg); - small_imu->xacc = mavlink_msg_small_imu_get_xacc(msg); - small_imu->yacc = mavlink_msg_small_imu_get_yacc(msg); - small_imu->zacc = mavlink_msg_small_imu_get_zacc(msg); - small_imu->xgyro = mavlink_msg_small_imu_get_xgyro(msg); - small_imu->ygyro = mavlink_msg_small_imu_get_ygyro(msg); - small_imu->zgyro = mavlink_msg_small_imu_get_zgyro(msg); - small_imu->temperature = mavlink_msg_small_imu_get_temperature(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + small_imu->time_boot_us = mavlink_msg_small_imu_get_time_boot_us(msg); + small_imu->xacc = mavlink_msg_small_imu_get_xacc(msg); + small_imu->yacc = mavlink_msg_small_imu_get_yacc(msg); + small_imu->zacc = mavlink_msg_small_imu_get_zacc(msg); + small_imu->xgyro = mavlink_msg_small_imu_get_xgyro(msg); + small_imu->ygyro = mavlink_msg_small_imu_get_ygyro(msg); + small_imu->zgyro = mavlink_msg_small_imu_get_zgyro(msg); + small_imu->temperature = mavlink_msg_small_imu_get_temperature(msg); #else - memcpy(small_imu, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SMALL_IMU_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_SMALL_IMU_LEN? msg->len : MAVLINK_MSG_ID_SMALL_IMU_LEN; + memset(small_imu, 0, MAVLINK_MSG_ID_SMALL_IMU_LEN); + memcpy(small_imu, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_small_mag.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_small_mag.h index acd1f983..53d9f0a8 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_small_mag.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_small_mag.h @@ -1,31 +1,45 @@ +#pragma once // MESSAGE SMALL_MAG PACKING #define MAVLINK_MSG_ID_SMALL_MAG 182 -typedef struct __mavlink_small_mag_t -{ - float xmag; /*< Magnetic field along X axis*/ - float ymag; /*< Magnetic field along Y axis*/ - float zmag; /*< Magnetic field along Z axis*/ + +typedef struct __mavlink_small_mag_t { + float xmag; /*< Magnetic field along X axis*/ + float ymag; /*< Magnetic field along Y axis*/ + float zmag; /*< Magnetic field along Z axis*/ } mavlink_small_mag_t; #define MAVLINK_MSG_ID_SMALL_MAG_LEN 12 +#define MAVLINK_MSG_ID_SMALL_MAG_MIN_LEN 12 #define MAVLINK_MSG_ID_182_LEN 12 +#define MAVLINK_MSG_ID_182_MIN_LEN 12 #define MAVLINK_MSG_ID_SMALL_MAG_CRC 218 #define MAVLINK_MSG_ID_182_CRC 218 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_SMALL_MAG { \ - "SMALL_MAG", \ - 3, \ - { { "xmag", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_small_mag_t, xmag) }, \ + 182, \ + "SMALL_MAG", \ + 3, \ + { { "xmag", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_small_mag_t, xmag) }, \ { "ymag", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_small_mag_t, ymag) }, \ { "zmag", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_small_mag_t, zmag) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_SMALL_MAG { \ + "SMALL_MAG", \ + 3, \ + { { "xmag", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_small_mag_t, xmag) }, \ + { "ymag", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_small_mag_t, ymag) }, \ + { "zmag", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_small_mag_t, zmag) }, \ + } \ +} +#endif /** * @brief Pack a small_mag message @@ -33,35 +47,70 @@ typedef struct __mavlink_small_mag_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param xmag Magnetic field along X axis - * @param ymag Magnetic field along Y axis - * @param zmag Magnetic field along Z axis + * @param xmag Magnetic field along X axis + * @param ymag Magnetic field along Y axis + * @param zmag Magnetic field along Z axis * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_small_mag_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float xmag, float ymag, float zmag) + float xmag, float ymag, float zmag) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SMALL_MAG_LEN]; - _mav_put_float(buf, 0, xmag); - _mav_put_float(buf, 4, ymag); - _mav_put_float(buf, 8, zmag); + char buf[MAVLINK_MSG_ID_SMALL_MAG_LEN]; + _mav_put_float(buf, 0, xmag); + _mav_put_float(buf, 4, ymag); + _mav_put_float(buf, 8, zmag); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMALL_MAG_LEN); #else - mavlink_small_mag_t packet; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; + mavlink_small_mag_t packet; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMALL_MAG_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_SMALL_MAG; + msg->msgid = MAVLINK_MSG_ID_SMALL_MAG; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SMALL_MAG_MIN_LEN, MAVLINK_MSG_ID_SMALL_MAG_LEN, MAVLINK_MSG_ID_SMALL_MAG_CRC); +} + +/** + * @brief Pack a small_mag message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param xmag Magnetic field along X axis + * @param ymag Magnetic field along Y axis + * @param zmag Magnetic field along Z axis + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_small_mag_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + float xmag, float ymag, float zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SMALL_MAG_LEN]; + _mav_put_float(buf, 0, xmag); + _mav_put_float(buf, 4, ymag); + _mav_put_float(buf, 8, zmag); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMALL_MAG_LEN); +#else + mavlink_small_mag_t packet; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMALL_MAG_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SMALL_MAG; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SMALL_MAG_LEN, MAVLINK_MSG_ID_SMALL_MAG_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SMALL_MAG_MIN_LEN, MAVLINK_MSG_ID_SMALL_MAG_LEN, MAVLINK_MSG_ID_SMALL_MAG_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SMALL_MAG_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SMALL_MAG_MIN_LEN, MAVLINK_MSG_ID_SMALL_MAG_LEN); #endif } @@ -71,37 +120,33 @@ static inline uint16_t mavlink_msg_small_mag_pack(uint8_t system_id, uint8_t com * @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 xmag Magnetic field along X axis - * @param ymag Magnetic field along Y axis - * @param zmag Magnetic field along Z axis + * @param xmag Magnetic field along X axis + * @param ymag Magnetic field along Y axis + * @param zmag Magnetic field along Z axis * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_small_mag_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float xmag,float ymag,float zmag) + mavlink_message_t* msg, + float xmag,float ymag,float zmag) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SMALL_MAG_LEN]; - _mav_put_float(buf, 0, xmag); - _mav_put_float(buf, 4, ymag); - _mav_put_float(buf, 8, zmag); + char buf[MAVLINK_MSG_ID_SMALL_MAG_LEN]; + _mav_put_float(buf, 0, xmag); + _mav_put_float(buf, 4, ymag); + _mav_put_float(buf, 8, zmag); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMALL_MAG_LEN); #else - mavlink_small_mag_t packet; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; + mavlink_small_mag_t packet; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMALL_MAG_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_SMALL_MAG; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SMALL_MAG_LEN, MAVLINK_MSG_ID_SMALL_MAG_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SMALL_MAG_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_SMALL_MAG; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SMALL_MAG_MIN_LEN, MAVLINK_MSG_ID_SMALL_MAG_LEN, MAVLINK_MSG_ID_SMALL_MAG_CRC); } /** @@ -114,7 +159,7 @@ static inline uint16_t mavlink_msg_small_mag_pack_chan(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_small_mag_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_small_mag_t* small_mag) { - return mavlink_msg_small_mag_pack(system_id, component_id, msg, small_mag->xmag, small_mag->ymag, small_mag->zmag); + return mavlink_msg_small_mag_pack(system_id, component_id, msg, small_mag->xmag, small_mag->ymag, small_mag->zmag); } /** @@ -128,49 +173,69 @@ static inline uint16_t mavlink_msg_small_mag_encode(uint8_t system_id, uint8_t c */ static inline uint16_t mavlink_msg_small_mag_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_small_mag_t* small_mag) { - return mavlink_msg_small_mag_pack_chan(system_id, component_id, chan, msg, small_mag->xmag, small_mag->ymag, small_mag->zmag); + return mavlink_msg_small_mag_pack_chan(system_id, component_id, chan, msg, small_mag->xmag, small_mag->ymag, small_mag->zmag); +} + +/** + * @brief Encode a small_mag struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param small_mag C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_small_mag_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_small_mag_t* small_mag) +{ + return mavlink_msg_small_mag_pack_status(system_id, component_id, _status, msg, small_mag->xmag, small_mag->ymag, small_mag->zmag); } /** * @brief Send a small_mag message * @param chan MAVLink channel to send the message * - * @param xmag Magnetic field along X axis - * @param ymag Magnetic field along Y axis - * @param zmag Magnetic field along Z axis + * @param xmag Magnetic field along X axis + * @param ymag Magnetic field along Y axis + * @param zmag Magnetic field along Z axis */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_small_mag_send(mavlink_channel_t chan, float xmag, float ymag, float zmag) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SMALL_MAG_LEN]; - _mav_put_float(buf, 0, xmag); - _mav_put_float(buf, 4, ymag); - _mav_put_float(buf, 8, zmag); + char buf[MAVLINK_MSG_ID_SMALL_MAG_LEN]; + _mav_put_float(buf, 0, xmag); + _mav_put_float(buf, 4, ymag); + _mav_put_float(buf, 8, zmag); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_MAG, buf, MAVLINK_MSG_ID_SMALL_MAG_LEN, MAVLINK_MSG_ID_SMALL_MAG_CRC); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_MAG, buf, MAVLINK_MSG_ID_SMALL_MAG_MIN_LEN, MAVLINK_MSG_ID_SMALL_MAG_LEN, MAVLINK_MSG_ID_SMALL_MAG_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_MAG, buf, MAVLINK_MSG_ID_SMALL_MAG_LEN); + mavlink_small_mag_t packet; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_MAG, (const char *)&packet, MAVLINK_MSG_ID_SMALL_MAG_MIN_LEN, MAVLINK_MSG_ID_SMALL_MAG_LEN, MAVLINK_MSG_ID_SMALL_MAG_CRC); #endif -#else - mavlink_small_mag_t packet; - packet.xmag = xmag; - packet.ymag = ymag; - packet.zmag = zmag; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_MAG, (const char *)&packet, MAVLINK_MSG_ID_SMALL_MAG_LEN, MAVLINK_MSG_ID_SMALL_MAG_CRC); +/** + * @brief Send a small_mag message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_small_mag_send_struct(mavlink_channel_t chan, const mavlink_small_mag_t* small_mag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_small_mag_send(chan, small_mag->xmag, small_mag->ymag, small_mag->zmag); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_MAG, (const char *)&packet, MAVLINK_MSG_ID_SMALL_MAG_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_MAG, (const char *)small_mag, MAVLINK_MSG_ID_SMALL_MAG_MIN_LEN, MAVLINK_MSG_ID_SMALL_MAG_LEN, MAVLINK_MSG_ID_SMALL_MAG_CRC); #endif } #if MAVLINK_MSG_ID_SMALL_MAG_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -179,27 +244,19 @@ static inline void mavlink_msg_small_mag_send(mavlink_channel_t chan, float xmag static inline void mavlink_msg_small_mag_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float xmag, float ymag, float zmag) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, xmag); - _mav_put_float(buf, 4, ymag); - _mav_put_float(buf, 8, zmag); + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, xmag); + _mav_put_float(buf, 4, ymag); + _mav_put_float(buf, 8, zmag); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_MAG, buf, MAVLINK_MSG_ID_SMALL_MAG_LEN, MAVLINK_MSG_ID_SMALL_MAG_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_MAG, buf, MAVLINK_MSG_ID_SMALL_MAG_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_MAG, buf, MAVLINK_MSG_ID_SMALL_MAG_MIN_LEN, MAVLINK_MSG_ID_SMALL_MAG_LEN, MAVLINK_MSG_ID_SMALL_MAG_CRC); #else - mavlink_small_mag_t *packet = (mavlink_small_mag_t *)msgbuf; - packet->xmag = xmag; - packet->ymag = ymag; - packet->zmag = zmag; + mavlink_small_mag_t *packet = (mavlink_small_mag_t *)msgbuf; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_MAG, (const char *)packet, MAVLINK_MSG_ID_SMALL_MAG_LEN, MAVLINK_MSG_ID_SMALL_MAG_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_MAG, (const char *)packet, MAVLINK_MSG_ID_SMALL_MAG_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_MAG, (const char *)packet, MAVLINK_MSG_ID_SMALL_MAG_MIN_LEN, MAVLINK_MSG_ID_SMALL_MAG_LEN, MAVLINK_MSG_ID_SMALL_MAG_CRC); #endif } #endif @@ -212,31 +269,31 @@ static inline void mavlink_msg_small_mag_send_buf(mavlink_message_t *msgbuf, mav /** * @brief Get field xmag from small_mag message * - * @return Magnetic field along X axis + * @return Magnetic field along X axis */ static inline float mavlink_msg_small_mag_get_xmag(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** * @brief Get field ymag from small_mag message * - * @return Magnetic field along Y axis + * @return Magnetic field along Y axis */ static inline float mavlink_msg_small_mag_get_ymag(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** * @brief Get field zmag from small_mag message * - * @return Magnetic field along Z axis + * @return Magnetic field along Z axis */ static inline float mavlink_msg_small_mag_get_zmag(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -247,11 +304,13 @@ static inline float mavlink_msg_small_mag_get_zmag(const mavlink_message_t* msg) */ static inline void mavlink_msg_small_mag_decode(const mavlink_message_t* msg, mavlink_small_mag_t* small_mag) { -#if MAVLINK_NEED_BYTE_SWAP - small_mag->xmag = mavlink_msg_small_mag_get_xmag(msg); - small_mag->ymag = mavlink_msg_small_mag_get_ymag(msg); - small_mag->zmag = mavlink_msg_small_mag_get_zmag(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + small_mag->xmag = mavlink_msg_small_mag_get_xmag(msg); + small_mag->ymag = mavlink_msg_small_mag_get_ymag(msg); + small_mag->zmag = mavlink_msg_small_mag_get_zmag(msg); #else - memcpy(small_mag, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SMALL_MAG_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_SMALL_MAG_LEN? msg->len : MAVLINK_MSG_ID_SMALL_MAG_LEN; + memset(small_mag, 0, MAVLINK_MSG_ID_SMALL_MAG_LEN); + memcpy(small_mag, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_small_range.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_small_range.h index 66934733..5c993579 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_small_range.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_small_range.h @@ -1,33 +1,48 @@ +#pragma once // MESSAGE SMALL_RANGE PACKING #define MAVLINK_MSG_ID_SMALL_RANGE 187 -typedef struct __mavlink_small_range_t -{ - float range; /*< Range Measurement (m)*/ - float max_range; /*< Max Range (m)*/ - float min_range; /*< Min Range (m)*/ - uint8_t type; /*< Sensor type*/ + +typedef struct __mavlink_small_range_t { + float range; /*< Range Measurement (m)*/ + float max_range; /*< Max Range (m)*/ + float min_range; /*< Min Range (m)*/ + uint8_t type; /*< Sensor type*/ } mavlink_small_range_t; #define MAVLINK_MSG_ID_SMALL_RANGE_LEN 13 +#define MAVLINK_MSG_ID_SMALL_RANGE_MIN_LEN 13 #define MAVLINK_MSG_ID_187_LEN 13 +#define MAVLINK_MSG_ID_187_MIN_LEN 13 #define MAVLINK_MSG_ID_SMALL_RANGE_CRC 60 #define MAVLINK_MSG_ID_187_CRC 60 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_SMALL_RANGE { \ - "SMALL_RANGE", \ - 4, \ - { { "range", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_small_range_t, range) }, \ + 187, \ + "SMALL_RANGE", \ + 4, \ + { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_small_range_t, type) }, \ + { "range", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_small_range_t, range) }, \ { "max_range", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_small_range_t, max_range) }, \ { "min_range", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_small_range_t, min_range) }, \ - { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_small_range_t, type) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_SMALL_RANGE { \ + "SMALL_RANGE", \ + 4, \ + { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_small_range_t, type) }, \ + { "range", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_small_range_t, range) }, \ + { "max_range", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_small_range_t, max_range) }, \ + { "min_range", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_small_range_t, min_range) }, \ + } \ +} +#endif /** * @brief Pack a small_range message @@ -35,38 +50,76 @@ typedef struct __mavlink_small_range_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param type Sensor type - * @param range Range Measurement (m) - * @param max_range Max Range (m) - * @param min_range Min Range (m) + * @param type Sensor type + * @param range Range Measurement (m) + * @param max_range Max Range (m) + * @param min_range Min Range (m) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_small_range_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t type, float range, float max_range, float min_range) + uint8_t type, float range, float max_range, float min_range) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SMALL_RANGE_LEN]; - _mav_put_float(buf, 0, range); - _mav_put_float(buf, 4, max_range); - _mav_put_float(buf, 8, min_range); - _mav_put_uint8_t(buf, 12, type); + char buf[MAVLINK_MSG_ID_SMALL_RANGE_LEN]; + _mav_put_float(buf, 0, range); + _mav_put_float(buf, 4, max_range); + _mav_put_float(buf, 8, min_range); + _mav_put_uint8_t(buf, 12, type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMALL_RANGE_LEN); #else - mavlink_small_range_t packet; - packet.range = range; - packet.max_range = max_range; - packet.min_range = min_range; - packet.type = type; + mavlink_small_range_t packet; + packet.range = range; + packet.max_range = max_range; + packet.min_range = min_range; + packet.type = type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMALL_RANGE_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_SMALL_RANGE; + msg->msgid = MAVLINK_MSG_ID_SMALL_RANGE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SMALL_RANGE_MIN_LEN, MAVLINK_MSG_ID_SMALL_RANGE_LEN, MAVLINK_MSG_ID_SMALL_RANGE_CRC); +} + +/** + * @brief Pack a small_range message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param type Sensor type + * @param range Range Measurement (m) + * @param max_range Max Range (m) + * @param min_range Min Range (m) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_small_range_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t type, float range, float max_range, float min_range) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SMALL_RANGE_LEN]; + _mav_put_float(buf, 0, range); + _mav_put_float(buf, 4, max_range); + _mav_put_float(buf, 8, min_range); + _mav_put_uint8_t(buf, 12, type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMALL_RANGE_LEN); +#else + mavlink_small_range_t packet; + packet.range = range; + packet.max_range = max_range; + packet.min_range = min_range; + packet.type = type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMALL_RANGE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SMALL_RANGE; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SMALL_RANGE_LEN, MAVLINK_MSG_ID_SMALL_RANGE_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SMALL_RANGE_MIN_LEN, MAVLINK_MSG_ID_SMALL_RANGE_LEN, MAVLINK_MSG_ID_SMALL_RANGE_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SMALL_RANGE_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SMALL_RANGE_MIN_LEN, MAVLINK_MSG_ID_SMALL_RANGE_LEN); #endif } @@ -76,40 +129,36 @@ static inline uint16_t mavlink_msg_small_range_pack(uint8_t system_id, uint8_t c * @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 type Sensor type - * @param range Range Measurement (m) - * @param max_range Max Range (m) - * @param min_range Min Range (m) + * @param type Sensor type + * @param range Range Measurement (m) + * @param max_range Max Range (m) + * @param min_range Min Range (m) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_small_range_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t type,float range,float max_range,float min_range) + mavlink_message_t* msg, + uint8_t type,float range,float max_range,float min_range) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SMALL_RANGE_LEN]; - _mav_put_float(buf, 0, range); - _mav_put_float(buf, 4, max_range); - _mav_put_float(buf, 8, min_range); - _mav_put_uint8_t(buf, 12, type); + char buf[MAVLINK_MSG_ID_SMALL_RANGE_LEN]; + _mav_put_float(buf, 0, range); + _mav_put_float(buf, 4, max_range); + _mav_put_float(buf, 8, min_range); + _mav_put_uint8_t(buf, 12, type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMALL_RANGE_LEN); #else - mavlink_small_range_t packet; - packet.range = range; - packet.max_range = max_range; - packet.min_range = min_range; - packet.type = type; + mavlink_small_range_t packet; + packet.range = range; + packet.max_range = max_range; + packet.min_range = min_range; + packet.type = type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMALL_RANGE_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_SMALL_RANGE; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SMALL_RANGE_LEN, MAVLINK_MSG_ID_SMALL_RANGE_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SMALL_RANGE_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_SMALL_RANGE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SMALL_RANGE_MIN_LEN, MAVLINK_MSG_ID_SMALL_RANGE_LEN, MAVLINK_MSG_ID_SMALL_RANGE_CRC); } /** @@ -122,7 +171,7 @@ static inline uint16_t mavlink_msg_small_range_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_small_range_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_small_range_t* small_range) { - return mavlink_msg_small_range_pack(system_id, component_id, msg, small_range->type, small_range->range, small_range->max_range, small_range->min_range); + return mavlink_msg_small_range_pack(system_id, component_id, msg, small_range->type, small_range->range, small_range->max_range, small_range->min_range); } /** @@ -136,52 +185,72 @@ static inline uint16_t mavlink_msg_small_range_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_small_range_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_small_range_t* small_range) { - return mavlink_msg_small_range_pack_chan(system_id, component_id, chan, msg, small_range->type, small_range->range, small_range->max_range, small_range->min_range); + return mavlink_msg_small_range_pack_chan(system_id, component_id, chan, msg, small_range->type, small_range->range, small_range->max_range, small_range->min_range); +} + +/** + * @brief Encode a small_range struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param small_range C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_small_range_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_small_range_t* small_range) +{ + return mavlink_msg_small_range_pack_status(system_id, component_id, _status, msg, small_range->type, small_range->range, small_range->max_range, small_range->min_range); } /** * @brief Send a small_range message * @param chan MAVLink channel to send the message * - * @param type Sensor type - * @param range Range Measurement (m) - * @param max_range Max Range (m) - * @param min_range Min Range (m) + * @param type Sensor type + * @param range Range Measurement (m) + * @param max_range Max Range (m) + * @param min_range Min Range (m) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_small_range_send(mavlink_channel_t chan, uint8_t type, float range, float max_range, float min_range) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_SMALL_RANGE_LEN]; - _mav_put_float(buf, 0, range); - _mav_put_float(buf, 4, max_range); - _mav_put_float(buf, 8, min_range); - _mav_put_uint8_t(buf, 12, type); + char buf[MAVLINK_MSG_ID_SMALL_RANGE_LEN]; + _mav_put_float(buf, 0, range); + _mav_put_float(buf, 4, max_range); + _mav_put_float(buf, 8, min_range); + _mav_put_uint8_t(buf, 12, type); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_RANGE, buf, MAVLINK_MSG_ID_SMALL_RANGE_LEN, MAVLINK_MSG_ID_SMALL_RANGE_CRC); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_RANGE, buf, MAVLINK_MSG_ID_SMALL_RANGE_MIN_LEN, MAVLINK_MSG_ID_SMALL_RANGE_LEN, MAVLINK_MSG_ID_SMALL_RANGE_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_RANGE, buf, MAVLINK_MSG_ID_SMALL_RANGE_LEN); + mavlink_small_range_t packet; + packet.range = range; + packet.max_range = max_range; + packet.min_range = min_range; + packet.type = type; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_RANGE, (const char *)&packet, MAVLINK_MSG_ID_SMALL_RANGE_MIN_LEN, MAVLINK_MSG_ID_SMALL_RANGE_LEN, MAVLINK_MSG_ID_SMALL_RANGE_CRC); #endif -#else - mavlink_small_range_t packet; - packet.range = range; - packet.max_range = max_range; - packet.min_range = min_range; - packet.type = type; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_RANGE, (const char *)&packet, MAVLINK_MSG_ID_SMALL_RANGE_LEN, MAVLINK_MSG_ID_SMALL_RANGE_CRC); +/** + * @brief Send a small_range message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_small_range_send_struct(mavlink_channel_t chan, const mavlink_small_range_t* small_range) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_small_range_send(chan, small_range->type, small_range->range, small_range->max_range, small_range->min_range); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_RANGE, (const char *)&packet, MAVLINK_MSG_ID_SMALL_RANGE_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_RANGE, (const char *)small_range, MAVLINK_MSG_ID_SMALL_RANGE_MIN_LEN, MAVLINK_MSG_ID_SMALL_RANGE_LEN, MAVLINK_MSG_ID_SMALL_RANGE_CRC); #endif } #if MAVLINK_MSG_ID_SMALL_RANGE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -190,29 +259,21 @@ static inline void mavlink_msg_small_range_send(mavlink_channel_t chan, uint8_t static inline void mavlink_msg_small_range_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, float range, float max_range, float min_range) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, range); - _mav_put_float(buf, 4, max_range); - _mav_put_float(buf, 8, min_range); - _mav_put_uint8_t(buf, 12, type); + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, range); + _mav_put_float(buf, 4, max_range); + _mav_put_float(buf, 8, min_range); + _mav_put_uint8_t(buf, 12, type); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_RANGE, buf, MAVLINK_MSG_ID_SMALL_RANGE_LEN, MAVLINK_MSG_ID_SMALL_RANGE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_RANGE, buf, MAVLINK_MSG_ID_SMALL_RANGE_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_RANGE, buf, MAVLINK_MSG_ID_SMALL_RANGE_MIN_LEN, MAVLINK_MSG_ID_SMALL_RANGE_LEN, MAVLINK_MSG_ID_SMALL_RANGE_CRC); #else - mavlink_small_range_t *packet = (mavlink_small_range_t *)msgbuf; - packet->range = range; - packet->max_range = max_range; - packet->min_range = min_range; - packet->type = type; + mavlink_small_range_t *packet = (mavlink_small_range_t *)msgbuf; + packet->range = range; + packet->max_range = max_range; + packet->min_range = min_range; + packet->type = type; -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_RANGE, (const char *)packet, MAVLINK_MSG_ID_SMALL_RANGE_LEN, MAVLINK_MSG_ID_SMALL_RANGE_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_RANGE, (const char *)packet, MAVLINK_MSG_ID_SMALL_RANGE_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMALL_RANGE, (const char *)packet, MAVLINK_MSG_ID_SMALL_RANGE_MIN_LEN, MAVLINK_MSG_ID_SMALL_RANGE_LEN, MAVLINK_MSG_ID_SMALL_RANGE_CRC); #endif } #endif @@ -225,41 +286,41 @@ static inline void mavlink_msg_small_range_send_buf(mavlink_message_t *msgbuf, m /** * @brief Get field type from small_range message * - * @return Sensor type + * @return Sensor type */ static inline uint8_t mavlink_msg_small_range_get_type(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 12); + return _MAV_RETURN_uint8_t(msg, 12); } /** * @brief Get field range from small_range message * - * @return Range Measurement (m) + * @return Range Measurement (m) */ static inline float mavlink_msg_small_range_get_range(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** * @brief Get field max_range from small_range message * - * @return Max Range (m) + * @return Max Range (m) */ static inline float mavlink_msg_small_range_get_max_range(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** * @brief Get field min_range from small_range message * - * @return Min Range (m) + * @return Min Range (m) */ static inline float mavlink_msg_small_range_get_min_range(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -270,12 +331,14 @@ static inline float mavlink_msg_small_range_get_min_range(const mavlink_message_ */ static inline void mavlink_msg_small_range_decode(const mavlink_message_t* msg, mavlink_small_range_t* small_range) { -#if MAVLINK_NEED_BYTE_SWAP - small_range->range = mavlink_msg_small_range_get_range(msg); - small_range->max_range = mavlink_msg_small_range_get_max_range(msg); - small_range->min_range = mavlink_msg_small_range_get_min_range(msg); - small_range->type = mavlink_msg_small_range_get_type(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + small_range->range = mavlink_msg_small_range_get_range(msg); + small_range->max_range = mavlink_msg_small_range_get_max_range(msg); + small_range->min_range = mavlink_msg_small_range_get_min_range(msg); + small_range->type = mavlink_msg_small_range_get_type(msg); #else - memcpy(small_range, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SMALL_RANGE_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_SMALL_RANGE_LEN? msg->len : MAVLINK_MSG_ID_SMALL_RANGE_LEN; + memset(small_range, 0, MAVLINK_MSG_ID_SMALL_RANGE_LEN); + memcpy(small_range, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_statustext.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_statustext.h index 9dcbff89..c343ec05 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_statustext.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_statustext.h @@ -1,29 +1,42 @@ +#pragma once // MESSAGE STATUSTEXT PACKING #define MAVLINK_MSG_ID_STATUSTEXT 253 -typedef struct __mavlink_statustext_t -{ - uint8_t severity; /*< Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY.*/ - char text[50]; /*< Status text message, without null termination character*/ + +typedef struct __mavlink_statustext_t { + uint8_t severity; /*< Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY.*/ + char text[50]; /*< Status text message, without null termination character*/ } mavlink_statustext_t; #define MAVLINK_MSG_ID_STATUSTEXT_LEN 51 +#define MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN 51 #define MAVLINK_MSG_ID_253_LEN 51 +#define MAVLINK_MSG_ID_253_MIN_LEN 51 #define MAVLINK_MSG_ID_STATUSTEXT_CRC 83 #define MAVLINK_MSG_ID_253_CRC 83 #define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 50 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_STATUSTEXT { \ - "STATUSTEXT", \ - 2, \ - { { "severity", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_statustext_t, severity) }, \ + 253, \ + "STATUSTEXT", \ + 2, \ + { { "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) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_STATUSTEXT { \ + "STATUSTEXT", \ + 2, \ + { { "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) }, \ + } \ +} +#endif /** * @brief Pack a statustext message @@ -31,30 +44,60 @@ typedef struct __mavlink_statustext_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. - * @param text Status text message, without null termination character + * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. + * @param text Status text message, without null termination character * @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) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; - _mav_put_uint8_t(buf, 0, severity); - _mav_put_char_array(buf, 1, text, 50); + char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; + _mav_put_uint8_t(buf, 0, severity); + _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; - mav_array_memcpy(packet.text, text, sizeof(char)*50); + mavlink_statustext_t packet; + packet.severity = severity; + mav_array_memcpy(packet.text, text, sizeof(char)*50); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; + msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); +} + +/** + * @brief Pack a statustext message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. + * @param text Status text message, without null termination character + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_statustext_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t severity, const char *text) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; + _mav_put_uint8_t(buf, 0, severity); + _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; + mav_array_memcpy(packet.text, text, sizeof(char)*50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN); #endif } @@ -64,32 +107,28 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co * @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 severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. - * @param text Status text message, without null termination character + * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. + * @param text Status text message, without null termination character * @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) + mavlink_message_t* msg, + uint8_t severity,const char *text) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; - _mav_put_uint8_t(buf, 0, severity); - _mav_put_char_array(buf, 1, text, 50); + char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; + _mav_put_uint8_t(buf, 0, severity); + _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; - mav_array_memcpy(packet.text, text, sizeof(char)*50); + mavlink_statustext_t packet; + packet.severity = severity; + mav_array_memcpy(packet.text, text, sizeof(char)*50); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); } /** @@ -102,7 +141,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); } /** @@ -116,44 +155,64 @@ 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); +} + +/** + * @brief Encode a statustext struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param statustext C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_statustext_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_statustext_t* statustext) +{ + return mavlink_msg_statustext_pack_status(system_id, component_id, _status, msg, statustext->severity, statustext->text); } /** * @brief Send a statustext message * @param chan MAVLink channel to send the message * - * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. - * @param text Status text message, without null termination character + * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. + * @param text Status text message, without null termination character */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char *text) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; - _mav_put_uint8_t(buf, 0, severity); - _mav_put_char_array(buf, 1, text, 50); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); + char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; + _mav_put_uint8_t(buf, 0, severity); + _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 - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); + mavlink_statustext_t packet; + packet.severity = severity; + 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 +} + +/** + * @brief Send a statustext message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +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); #else - mavlink_statustext_t packet; - packet.severity = severity; - mav_array_memcpy(packet.text, text, sizeof(char)*50); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)&packet, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)&packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); -#endif + _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 } #if MAVLINK_MSG_ID_STATUSTEXT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -162,23 +221,15 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s static inline void mavlink_msg_statustext_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t severity, const char *text) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint8_t(buf, 0, severity); - _mav_put_char_array(buf, 1, text, 50); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, severity); + _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 - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); -#endif -#else - mavlink_statustext_t *packet = (mavlink_statustext_t *)msgbuf; - packet->severity = severity; - mav_array_memcpy(packet->text, text, sizeof(char)*50); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)packet, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); -#endif + mavlink_statustext_t *packet = (mavlink_statustext_t *)msgbuf; + packet->severity = severity; + 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 } #endif @@ -191,21 +242,21 @@ static inline void mavlink_msg_statustext_send_buf(mavlink_message_t *msgbuf, ma /** * @brief Get field severity from statustext message * - * @return Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. + * @return Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. */ static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 0); + return _MAV_RETURN_uint8_t(msg, 0); } /** * @brief Get field text from statustext message * - * @return Status text message, without null termination character + * @return Status text message, without null termination character */ static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, char *text) { - return _MAV_RETURN_char_array(msg, text, 50, 1); + return _MAV_RETURN_char_array(msg, text, 50, 1); } /** @@ -216,10 +267,12 @@ static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* */ static inline void mavlink_msg_statustext_decode(const mavlink_message_t* msg, mavlink_statustext_t* statustext) { -#if MAVLINK_NEED_BYTE_SWAP - statustext->severity = mavlink_msg_statustext_get_severity(msg); - mavlink_msg_statustext_get_text(msg, statustext->text); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + statustext->severity = mavlink_msg_statustext_get_severity(msg); + mavlink_msg_statustext_get_text(msg, statustext->text); #else - memcpy(statustext, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_STATUSTEXT_LEN); + 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); + memcpy(statustext, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/mavlink_msg_timesync.h b/comms/mavlink/v1.0/rosflight/mavlink_msg_timesync.h index be284428..f6d7d434 100644 --- a/comms/mavlink/v1.0/rosflight/mavlink_msg_timesync.h +++ b/comms/mavlink/v1.0/rosflight/mavlink_msg_timesync.h @@ -1,29 +1,42 @@ +#pragma once // MESSAGE TIMESYNC PACKING #define MAVLINK_MSG_ID_TIMESYNC 111 -typedef struct __mavlink_timesync_t -{ - int64_t tc1; /*< Time sync timestamp 1*/ - int64_t ts1; /*< Time sync timestamp 2*/ + +typedef struct __mavlink_timesync_t { + int64_t tc1; /*< Time sync timestamp 1*/ + int64_t ts1; /*< Time sync timestamp 2*/ } mavlink_timesync_t; #define MAVLINK_MSG_ID_TIMESYNC_LEN 16 +#define MAVLINK_MSG_ID_TIMESYNC_MIN_LEN 16 #define MAVLINK_MSG_ID_111_LEN 16 +#define MAVLINK_MSG_ID_111_MIN_LEN 16 #define MAVLINK_MSG_ID_TIMESYNC_CRC 34 #define MAVLINK_MSG_ID_111_CRC 34 +#if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_TIMESYNC { \ - "TIMESYNC", \ - 2, \ - { { "tc1", NULL, MAVLINK_TYPE_INT64_T, 0, 0, offsetof(mavlink_timesync_t, tc1) }, \ + 111, \ + "TIMESYNC", \ + 2, \ + { { "tc1", NULL, MAVLINK_TYPE_INT64_T, 0, 0, offsetof(mavlink_timesync_t, tc1) }, \ { "ts1", NULL, MAVLINK_TYPE_INT64_T, 0, 8, offsetof(mavlink_timesync_t, ts1) }, \ } \ } - +#else +#define MAVLINK_MESSAGE_INFO_TIMESYNC { \ + "TIMESYNC", \ + 2, \ + { { "tc1", NULL, MAVLINK_TYPE_INT64_T, 0, 0, offsetof(mavlink_timesync_t, tc1) }, \ + { "ts1", NULL, MAVLINK_TYPE_INT64_T, 0, 8, offsetof(mavlink_timesync_t, ts1) }, \ + } \ +} +#endif /** * @brief Pack a timesync message @@ -31,32 +44,64 @@ typedef struct __mavlink_timesync_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param tc1 Time sync timestamp 1 - * @param ts1 Time sync timestamp 2 + * @param tc1 Time sync timestamp 1 + * @param ts1 Time sync timestamp 2 * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_timesync_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - int64_t tc1, int64_t ts1) + int64_t tc1, int64_t ts1) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TIMESYNC_LEN]; + _mav_put_int64_t(buf, 0, tc1); + _mav_put_int64_t(buf, 8, ts1); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TIMESYNC_LEN); +#else + mavlink_timesync_t packet; + packet.tc1 = tc1; + packet.ts1 = ts1; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TIMESYNC_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TIMESYNC; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); +} + +/** + * @brief Pack a timesync message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param tc1 Time sync timestamp 1 + * @param ts1 Time sync timestamp 2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_timesync_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + int64_t tc1, int64_t ts1) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_TIMESYNC_LEN]; - _mav_put_int64_t(buf, 0, tc1); - _mav_put_int64_t(buf, 8, ts1); + char buf[MAVLINK_MSG_ID_TIMESYNC_LEN]; + _mav_put_int64_t(buf, 0, tc1); + _mav_put_int64_t(buf, 8, ts1); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TIMESYNC_LEN); #else - mavlink_timesync_t packet; - packet.tc1 = tc1; - packet.ts1 = ts1; + mavlink_timesync_t packet; + packet.tc1 = tc1; + packet.ts1 = ts1; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TIMESYNC_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_TIMESYNC; + msg->msgid = MAVLINK_MSG_ID_TIMESYNC; #if MAVLINK_CRC_EXTRA - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); #else - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TIMESYNC_LEN); + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN); #endif } @@ -66,34 +111,30 @@ static inline uint16_t mavlink_msg_timesync_pack(uint8_t system_id, uint8_t comp * @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 tc1 Time sync timestamp 1 - * @param ts1 Time sync timestamp 2 + * @param tc1 Time sync timestamp 1 + * @param ts1 Time sync timestamp 2 * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_timesync_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - int64_t tc1,int64_t ts1) + mavlink_message_t* msg, + int64_t tc1,int64_t ts1) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_TIMESYNC_LEN]; - _mav_put_int64_t(buf, 0, tc1); - _mav_put_int64_t(buf, 8, ts1); + char buf[MAVLINK_MSG_ID_TIMESYNC_LEN]; + _mav_put_int64_t(buf, 0, tc1); + _mav_put_int64_t(buf, 8, ts1); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TIMESYNC_LEN); #else - mavlink_timesync_t packet; - packet.tc1 = tc1; - packet.ts1 = ts1; + mavlink_timesync_t packet; + packet.tc1 = tc1; + packet.ts1 = ts1; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TIMESYNC_LEN); #endif - msg->msgid = MAVLINK_MSG_ID_TIMESYNC; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); -#else - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TIMESYNC_LEN); -#endif + msg->msgid = MAVLINK_MSG_ID_TIMESYNC; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); } /** @@ -106,7 +147,7 @@ static inline uint16_t mavlink_msg_timesync_pack_chan(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_timesync_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_timesync_t* timesync) { - return mavlink_msg_timesync_pack(system_id, component_id, msg, timesync->tc1, timesync->ts1); + return mavlink_msg_timesync_pack(system_id, component_id, msg, timesync->tc1, timesync->ts1); } /** @@ -120,46 +161,66 @@ static inline uint16_t mavlink_msg_timesync_encode(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_timesync_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_timesync_t* timesync) { - return mavlink_msg_timesync_pack_chan(system_id, component_id, chan, msg, timesync->tc1, timesync->ts1); + return mavlink_msg_timesync_pack_chan(system_id, component_id, chan, msg, timesync->tc1, timesync->ts1); +} + +/** + * @brief Encode a timesync struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param timesync C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_timesync_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_timesync_t* timesync) +{ + return mavlink_msg_timesync_pack_status(system_id, component_id, _status, msg, timesync->tc1, timesync->ts1); } /** * @brief Send a timesync message * @param chan MAVLink channel to send the message * - * @param tc1 Time sync timestamp 1 - * @param ts1 Time sync timestamp 2 + * @param tc1 Time sync timestamp 1 + * @param ts1 Time sync timestamp 2 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_timesync_send(mavlink_channel_t chan, int64_t tc1, int64_t ts1) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_TIMESYNC_LEN]; - _mav_put_int64_t(buf, 0, tc1); - _mav_put_int64_t(buf, 8, ts1); + char buf[MAVLINK_MSG_ID_TIMESYNC_LEN]; + _mav_put_int64_t(buf, 0, tc1); + _mav_put_int64_t(buf, 8, ts1); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, buf, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, buf, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, buf, MAVLINK_MSG_ID_TIMESYNC_LEN); + mavlink_timesync_t packet; + packet.tc1 = tc1; + packet.ts1 = ts1; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, (const char *)&packet, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); #endif -#else - mavlink_timesync_t packet; - packet.tc1 = tc1; - packet.ts1 = ts1; +} -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, (const char *)&packet, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); +/** + * @brief Send a timesync message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_timesync_send_struct(mavlink_channel_t chan, const mavlink_timesync_t* timesync) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_timesync_send(chan, timesync->tc1, timesync->ts1); #else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, (const char *)&packet, MAVLINK_MSG_ID_TIMESYNC_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, (const char *)timesync, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); #endif } #if MAVLINK_MSG_ID_TIMESYNC_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant 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 @@ -168,25 +229,17 @@ static inline void mavlink_msg_timesync_send(mavlink_channel_t chan, int64_t tc1 static inline void mavlink_msg_timesync_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int64_t tc1, int64_t ts1) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int64_t(buf, 0, tc1); - _mav_put_int64_t(buf, 8, ts1); + char *buf = (char *)msgbuf; + _mav_put_int64_t(buf, 0, tc1); + _mav_put_int64_t(buf, 8, ts1); -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, buf, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, buf, MAVLINK_MSG_ID_TIMESYNC_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, buf, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); #else - mavlink_timesync_t *packet = (mavlink_timesync_t *)msgbuf; - packet->tc1 = tc1; - packet->ts1 = ts1; + mavlink_timesync_t *packet = (mavlink_timesync_t *)msgbuf; + packet->tc1 = tc1; + packet->ts1 = ts1; -#if MAVLINK_CRC_EXTRA - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, (const char *)packet, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, (const char *)packet, MAVLINK_MSG_ID_TIMESYNC_LEN); -#endif + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, (const char *)packet, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); #endif } #endif @@ -199,21 +252,21 @@ static inline void mavlink_msg_timesync_send_buf(mavlink_message_t *msgbuf, mavl /** * @brief Get field tc1 from timesync message * - * @return Time sync timestamp 1 + * @return Time sync timestamp 1 */ static inline int64_t mavlink_msg_timesync_get_tc1(const mavlink_message_t* msg) { - return _MAV_RETURN_int64_t(msg, 0); + return _MAV_RETURN_int64_t(msg, 0); } /** * @brief Get field ts1 from timesync message * - * @return Time sync timestamp 2 + * @return Time sync timestamp 2 */ static inline int64_t mavlink_msg_timesync_get_ts1(const mavlink_message_t* msg) { - return _MAV_RETURN_int64_t(msg, 8); + return _MAV_RETURN_int64_t(msg, 8); } /** @@ -224,10 +277,12 @@ static inline int64_t mavlink_msg_timesync_get_ts1(const mavlink_message_t* msg) */ static inline void mavlink_msg_timesync_decode(const mavlink_message_t* msg, mavlink_timesync_t* timesync) { -#if MAVLINK_NEED_BYTE_SWAP - timesync->tc1 = mavlink_msg_timesync_get_tc1(msg); - timesync->ts1 = mavlink_msg_timesync_get_ts1(msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + timesync->tc1 = mavlink_msg_timesync_get_tc1(msg); + timesync->ts1 = mavlink_msg_timesync_get_ts1(msg); #else - memcpy(timesync, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_TIMESYNC_LEN); + uint8_t len = msg->len < MAVLINK_MSG_ID_TIMESYNC_LEN? msg->len : MAVLINK_MSG_ID_TIMESYNC_LEN; + memset(timesync, 0, MAVLINK_MSG_ID_TIMESYNC_LEN); + memcpy(timesync, _MAV_PAYLOAD(msg), len); #endif } diff --git a/comms/mavlink/v1.0/rosflight/rosflight.h b/comms/mavlink/v1.0/rosflight/rosflight.h index a5d97ebe..f29b2759 100644 --- a/comms/mavlink/v1.0/rosflight/rosflight.h +++ b/comms/mavlink/v1.0/rosflight/rosflight.h @@ -1,7 +1,8 @@ /** @file - * @brief MAVLink comm protocol generated from rosflight.xml - * @see http://mavlink.org + * @brief MAVLink comm protocol generated from rosflight.xml + * @see http://mavlink.org */ +#pragma once #ifndef MAVLINK_ROSFLIGHT_H #define MAVLINK_ROSFLIGHT_H @@ -9,6 +10,8 @@ #error Wrong include order: MAVLINK_ROSFLIGHT.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 +#define MAVLINK_ROSFLIGHT_XML_HASH 9006179629977798615 + #ifdef __cplusplus extern "C" { #endif @@ -23,10 +26,6 @@ extern "C" { #define MAVLINK_MESSAGE_CRCS {50, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 0, 0, 0, 0, 0, 0, 0, 246, 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, 118, 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, 34, 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, 0, 190, 67, 218, 206, 169, 0, 0, 60, 249, 113, 181, 183, 134, 1, 0, 65, 10, 221, 0, 48, 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, 83, 0, 0} #endif -#ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, {"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_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {"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_ATTITUDE_QUATERNION, {"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_RC_CHANNELS, {"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_TIMESYNC, {"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}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OFFBOARD_CONTROL, MAVLINK_MESSAGE_INFO_SMALL_IMU, MAVLINK_MESSAGE_INFO_SMALL_MAG, MAVLINK_MESSAGE_INFO_SMALL_BARO, MAVLINK_MESSAGE_INFO_DIFF_PRESSURE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SMALL_RANGE, MAVLINK_MESSAGE_INFO_ROSFLIGHT_CMD, MAVLINK_MESSAGE_INFO_ROSFLIGHT_CMD_ACK, MAVLINK_MESSAGE_INFO_ROSFLIGHT_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_ROSFLIGHT_STATUS, MAVLINK_MESSAGE_INFO_ROSFLIGHT_VERSION, MAVLINK_MESSAGE_INFO_ROSFLIGHT_AUX_CMD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_EXTERNAL_ATTITUDE, MAVLINK_MESSAGE_INFO_ROSFLIGHT_HARD_ERROR, MAVLINK_MESSAGE_INFO_ROSFLIGHT_GNSS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROSFLIGHT_BATTERY_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}}}, MAVLINK_MESSAGE_INFO_STATUSTEXT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} -#endif - #include "../protocol.h" #define MAVLINK_ENABLED_ROSFLIGHT @@ -39,20 +38,20 @@ extern "C" { #define HAVE_ENUM_ROSFLIGHT_CMD typedef enum ROSFLIGHT_CMD { - ROSFLIGHT_CMD_RC_CALIBRATION=0, /* | */ - ROSFLIGHT_CMD_ACCEL_CALIBRATION=1, /* | */ - ROSFLIGHT_CMD_GYRO_CALIBRATION=2, /* | */ - ROSFLIGHT_CMD_BARO_CALIBRATION=3, /* | */ - ROSFLIGHT_CMD_AIRSPEED_CALIBRATION=4, /* | */ - ROSFLIGHT_CMD_READ_PARAMS=5, /* | */ - ROSFLIGHT_CMD_WRITE_PARAMS=6, /* | */ - ROSFLIGHT_CMD_SET_PARAM_DEFAULTS=7, /* | */ - ROSFLIGHT_CMD_REBOOT=8, /* | */ - ROSFLIGHT_CMD_REBOOT_TO_BOOTLOADER=9, /* | */ - ROSFLIGHT_CMD_SEND_VERSION=10, /* | */ - ROSFLIGHT_CMD_RESET_ORIGIN=11, /* | */ - ROSFLIGHT_CMD_SEND_ALL_CONFIG_INFOS=12, /* | */ - ROSFLIGHT_CMD_ENUM_END=13, /* | */ + ROSFLIGHT_CMD_RC_CALIBRATION=0, /* | */ + ROSFLIGHT_CMD_ACCEL_CALIBRATION=1, /* | */ + ROSFLIGHT_CMD_GYRO_CALIBRATION=2, /* | */ + ROSFLIGHT_CMD_BARO_CALIBRATION=3, /* | */ + ROSFLIGHT_CMD_AIRSPEED_CALIBRATION=4, /* | */ + ROSFLIGHT_CMD_READ_PARAMS=5, /* | */ + ROSFLIGHT_CMD_WRITE_PARAMS=6, /* | */ + ROSFLIGHT_CMD_SET_PARAM_DEFAULTS=7, /* | */ + ROSFLIGHT_CMD_REBOOT=8, /* | */ + ROSFLIGHT_CMD_REBOOT_TO_BOOTLOADER=9, /* | */ + ROSFLIGHT_CMD_SEND_VERSION=10, /* | */ + ROSFLIGHT_CMD_RESET_ORIGIN=11, /* | */ + ROSFLIGHT_CMD_SEND_ALL_CONFIG_INFOS=12, /* | */ + ROSFLIGHT_CMD_ENUM_END=13, /* | */ } ROSFLIGHT_CMD; #endif @@ -61,10 +60,10 @@ typedef enum ROSFLIGHT_CMD #define HAVE_ENUM_ROSFLIGHT_AUX_CMD_TYPE typedef enum ROSFLIGHT_AUX_CMD_TYPE { - DISABLED=0, /* | */ - SERVO=1, /* | */ - MOTOR=2, /* | */ - ROSFLIGHT_AUX_CMD_TYPE_ENUM_END=3, /* | */ + DISABLED=0, /* | */ + SERVO=1, /* | */ + MOTOR=2, /* | */ + ROSFLIGHT_AUX_CMD_TYPE_ENUM_END=3, /* | */ } ROSFLIGHT_AUX_CMD_TYPE; #endif @@ -73,9 +72,9 @@ typedef enum ROSFLIGHT_AUX_CMD_TYPE #define HAVE_ENUM_ROSFLIGHT_CMD_RESPONSE typedef enum ROSFLIGHT_CMD_RESPONSE { - ROSFLIGHT_CMD_FAILED=0, /* | */ - ROSFLIGHT_CMD_SUCCESS=1, /* | */ - ROSFLIGHT_CMD_RESPONSE_ENUM_END=2, /* | */ + ROSFLIGHT_CMD_FAILED=0, /* | */ + ROSFLIGHT_CMD_SUCCESS=1, /* | */ + ROSFLIGHT_CMD_RESPONSE_ENUM_END=2, /* | */ } ROSFLIGHT_CMD_RESPONSE; #endif @@ -84,13 +83,13 @@ typedef enum ROSFLIGHT_CMD_RESPONSE #define HAVE_ENUM_OFFBOARD_CONTROL_MODE typedef enum OFFBOARD_CONTROL_MODE { - MODE_PASS_THROUGH=0, /* Pass commanded values directly to actuators | */ - MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE=1, /* Command roll rate, pitch rate, yaw rate, and throttle | */ - MODE_ROLL_PITCH_YAWRATE_THROTTLE=2, /* Command roll angle, pitch angle, yaw rate, and throttle | */ - MODE_ROLL_PITCH_YAWRATE_ALTITUDE=3, /* Command roll angle, pitch angle, yaw rate, and altitude above ground | */ - MODE_XVEL_YVEL_YAWRATE_ALTITUDE=4, /* Command body-fixed, x and y velocity, and yaw rate, and altitude above ground | */ - MODE_XPOS_YPOS_YAW_ALTITUDE=5, /* Command inertial x, y position (m) wrt origin, yaw angle wrt north, and altitude above ground | */ - OFFBOARD_CONTROL_MODE_ENUM_END=6, /* | */ + MODE_PASS_THROUGH=0, /* Pass commanded values directly to actuators | */ + MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE=1, /* Command roll rate, pitch rate, yaw rate, and throttle | */ + MODE_ROLL_PITCH_YAWRATE_THROTTLE=2, /* Command roll angle, pitch angle, yaw rate, and throttle | */ + MODE_ROLL_PITCH_YAWRATE_ALTITUDE=3, /* Command roll angle, pitch angle, yaw rate, and altitude above ground | */ + MODE_XVEL_YVEL_YAWRATE_ALTITUDE=4, /* Command body-fixed, x and y velocity, and yaw rate, and altitude above ground | */ + MODE_XPOS_YPOS_YAW_ALTITUDE=5, /* Command inertial x, y position (m) wrt origin, yaw angle wrt north, and altitude above ground | */ + OFFBOARD_CONTROL_MODE_ENUM_END=6, /* | */ } OFFBOARD_CONTROL_MODE; #endif @@ -99,14 +98,14 @@ typedef enum OFFBOARD_CONTROL_MODE #define HAVE_ENUM_OFFBOARD_CONTROL_IGNORE typedef enum OFFBOARD_CONTROL_IGNORE { - IGNORE_NONE=0, /* Convenience value for specifying no fields should be ignored | */ - IGNORE_VALUE1=1, /* Field value1 should be ignored | */ - IGNORE_VALUE2=2, /* Field value2 should be ignored | */ - IGNORE_VALUE3=4, /* Field value3 should be ignored | */ - IGNORE_VALUE4=8, /* Field value4 should be ignored | */ - IGNORE_VALUE5=22, /* Field value5 should be ignored | */ - IGNORE_VALUE6=50, /* Field value6 should be ignored | */ - OFFBOARD_CONTROL_IGNORE_ENUM_END=51, /* | */ + IGNORE_NONE=0, /* Convenience value for specifying no fields should be ignored | */ + IGNORE_VALUE1=1, /* Field value1 should be ignored | */ + IGNORE_VALUE2=2, /* Field value2 should be ignored | */ + IGNORE_VALUE3=4, /* Field value3 should be ignored | */ + IGNORE_VALUE4=8, /* Field value4 should be ignored | */ + IGNORE_VALUE5=22, /* Field value5 should be ignored | */ + IGNORE_VALUE6=50, /* Field value6 should be ignored | */ + OFFBOARD_CONTROL_IGNORE_ENUM_END=51, /* | */ } OFFBOARD_CONTROL_IGNORE; #endif @@ -115,16 +114,16 @@ typedef enum OFFBOARD_CONTROL_IGNORE #define HAVE_ENUM_ROSFLIGHT_ERROR_CODE typedef enum ROSFLIGHT_ERROR_CODE { - ROSFLIGHT_ERROR_NONE=0, /* | */ - ROSFLIGHT_ERROR_INVALID_MIXER=1, /* | */ - ROSFLIGHT_ERROR_IMU_NOT_RESPONDING=2, /* | */ - ROSFLIGHT_ERROR_RC_LOST=4, /* | */ - ROSFLIGHT_ERROR_UNHEALTHY_ESTIMATOR=8, /* | */ - ROSFLIGHT_ERROR_TIME_GOING_BACKWARDS=16, /* | */ - ROSFLIGHT_ERROR_UNCALIBRATED_IMU=32, /* | */ - ROSFLIGHT_ERROR_BUFFER_OVERRUN=64, /* | */ - ROSFLIGHT_ERROR_INVALID_FAILSAFE=128, /* | */ - ROSFLIGHT_ERROR_CODE_ENUM_END=129, /* | */ + ROSFLIGHT_ERROR_NONE=0, /* | */ + ROSFLIGHT_ERROR_INVALID_MIXER=1, /* | */ + ROSFLIGHT_ERROR_IMU_NOT_RESPONDING=2, /* | */ + ROSFLIGHT_ERROR_RC_LOST=4, /* | */ + ROSFLIGHT_ERROR_UNHEALTHY_ESTIMATOR=8, /* | */ + ROSFLIGHT_ERROR_TIME_GOING_BACKWARDS=16, /* | */ + ROSFLIGHT_ERROR_UNCALIBRATED_IMU=32, /* | */ + ROSFLIGHT_ERROR_BUFFER_OVERRUN=64, /* | */ + ROSFLIGHT_ERROR_INVALID_FAILSAFE=128, /* | */ + ROSFLIGHT_ERROR_CODE_ENUM_END=129, /* | */ } ROSFLIGHT_ERROR_CODE; #endif @@ -133,9 +132,9 @@ typedef enum ROSFLIGHT_ERROR_CODE #define HAVE_ENUM_ROSFLIGHT_RANGE_TYPE typedef enum ROSFLIGHT_RANGE_TYPE { - ROSFLIGHT_RANGE_SONAR=0, /* | */ - ROSFLIGHT_RANGE_LIDAR=1, /* | */ - ROSFLIGHT_RANGE_TYPE_ENUM_END=2, /* | */ + ROSFLIGHT_RANGE_SONAR=0, /* | */ + ROSFLIGHT_RANGE_LIDAR=1, /* | */ + ROSFLIGHT_RANGE_TYPE_ENUM_END=2, /* | */ } ROSFLIGHT_RANGE_TYPE; #endif @@ -144,40 +143,25 @@ typedef enum ROSFLIGHT_RANGE_TYPE #define HAVE_ENUM_GNSS_FIX_TYPE typedef enum GNSS_FIX_TYPE { - GNSS_FIX_NO_FIX=0, /* | */ - GNSS_FIX_DEAD_RECKONING_ONLY=1, /* | */ - GNSS_FIX_2D_FIX=2, /* | */ - GNSS_FIX_3D_FIX=3, /* | */ - GNSS_FIX_GNSS_PLUS_DEAD_RECKONING=4, /* | */ - GNSS_FIX_TIME_FIX_ONLY=5, /* | */ - GNSS_FIX_TYPE_ENUM_END=6, /* | */ + GNSS_FIX_NO_FIX=0, /* | */ + GNSS_FIX_DEAD_RECKONING_ONLY=1, /* | */ + GNSS_FIX_2D_FIX=2, /* | */ + GNSS_FIX_3D_FIX=3, /* | */ + GNSS_FIX_GNSS_PLUS_DEAD_RECKONING=4, /* | */ + GNSS_FIX_TIME_FIX_ONLY=5, /* | */ + GNSS_FIX_TYPE_ENUM_END=6, /* | */ } GNSS_FIX_TYPE; #endif -/** @brief */ -#ifndef HAVE_ENUM_UBLOX_FIX_TYPE -#define HAVE_ENUM_UBLOX_FIX_TYPE -typedef enum UBLOX_FIX_TYPE -{ - NO_FIX=0, /* | */ - DEAD_RECKONING_ONLY=1, /* | */ - FIX_2D=2, /* | */ - FIX_3D=3, /* | */ - GNSS_AND_DEAD_RECKONING=4, /* | */ - TIME_ONLY=5, /* | */ - UBLOX_FIX_TYPE_ENUM_END=6, /* | */ -} UBLOX_FIX_TYPE; -#endif - /** @brief Generic micro air vehicle. */ #ifndef HAVE_ENUM_MAV_TYPE #define HAVE_ENUM_MAV_TYPE typedef enum MAV_TYPE { - MAV_TYPE_GENERIC=0, /* | */ - MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ - MAV_TYPE_QUADROTOR=2, /* Quadrotor | */ - MAV_TYPE_ENUM_END=3, /* | */ + MAV_TYPE_GENERIC=0, /* | */ + MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ + MAV_TYPE_QUADROTOR=2, /* Quadrotor | */ + MAV_TYPE_ENUM_END=3, /* | */ } MAV_TYPE; #endif @@ -186,17 +170,17 @@ typedef enum MAV_TYPE #define HAVE_ENUM_MAV_PARAM_TYPE typedef enum MAV_PARAM_TYPE { - MAV_PARAM_TYPE_UINT8=1, /* 8-bit unsigned integer | */ - MAV_PARAM_TYPE_INT8=2, /* 8-bit signed integer | */ - MAV_PARAM_TYPE_UINT16=3, /* 16-bit unsigned integer | */ - MAV_PARAM_TYPE_INT16=4, /* 16-bit signed integer | */ - MAV_PARAM_TYPE_UINT32=5, /* 32-bit unsigned integer | */ - MAV_PARAM_TYPE_INT32=6, /* 32-bit signed integer | */ - MAV_PARAM_TYPE_UINT64=7, /* 64-bit unsigned integer | */ - MAV_PARAM_TYPE_INT64=8, /* 64-bit signed integer | */ - MAV_PARAM_TYPE_REAL32=9, /* 32-bit floating-point | */ - MAV_PARAM_TYPE_REAL64=10, /* 64-bit floating-point | */ - MAV_PARAM_TYPE_ENUM_END=11, /* | */ + MAV_PARAM_TYPE_UINT8=1, /* 8-bit unsigned integer | */ + MAV_PARAM_TYPE_INT8=2, /* 8-bit signed integer | */ + MAV_PARAM_TYPE_UINT16=3, /* 16-bit unsigned integer | */ + MAV_PARAM_TYPE_INT16=4, /* 16-bit signed integer | */ + MAV_PARAM_TYPE_UINT32=5, /* 32-bit unsigned integer | */ + MAV_PARAM_TYPE_INT32=6, /* 32-bit signed integer | */ + MAV_PARAM_TYPE_UINT64=7, /* 64-bit unsigned integer | */ + MAV_PARAM_TYPE_INT64=8, /* 64-bit signed integer | */ + MAV_PARAM_TYPE_REAL32=9, /* 32-bit floating-point | */ + MAV_PARAM_TYPE_REAL64=10, /* 64-bit floating-point | */ + MAV_PARAM_TYPE_ENUM_END=11, /* | */ } MAV_PARAM_TYPE; #endif @@ -205,15 +189,15 @@ typedef enum MAV_PARAM_TYPE #define HAVE_ENUM_MAV_SEVERITY typedef enum MAV_SEVERITY { - MAV_SEVERITY_EMERGENCY=0, /* System is unusable. This is a "panic" condition. | */ - MAV_SEVERITY_ALERT=1, /* Action should be taken immediately. Indicates error in non-critical systems. | */ - MAV_SEVERITY_CRITICAL=2, /* Action must be taken immediately. Indicates failure in a primary system. | */ - MAV_SEVERITY_ERROR=3, /* Indicates an error in secondary/redundant systems. | */ - MAV_SEVERITY_WARNING=4, /* Indicates about a possible future error if this is not resolved within a given timeframe. Example would be a low battery warning. | */ - MAV_SEVERITY_NOTICE=5, /* An unusual event has occured, though not an error condition. This should be investigated for the root cause. | */ - MAV_SEVERITY_INFO=6, /* Normal operational messages. Useful for logging. No action is required for these messages. | */ - MAV_SEVERITY_DEBUG=7, /* Useful non-operational messages that can assist in debugging. These should not occur during normal operation. | */ - MAV_SEVERITY_ENUM_END=8, /* | */ + MAV_SEVERITY_EMERGENCY=0, /* System is unusable. This is a "panic" condition. | */ + MAV_SEVERITY_ALERT=1, /* Action should be taken immediately. Indicates error in non-critical systems. | */ + MAV_SEVERITY_CRITICAL=2, /* Action must be taken immediately. Indicates failure in a primary system. | */ + MAV_SEVERITY_ERROR=3, /* Indicates an error in secondary/redundant systems. | */ + MAV_SEVERITY_WARNING=4, /* Indicates about a possible future error if this is not resolved within a given timeframe. Example would be a low battery warning. | */ + MAV_SEVERITY_NOTICE=5, /* An unusual event has occured, though not an error condition. This should be investigated for the root cause. | */ + MAV_SEVERITY_INFO=6, /* Normal operational messages. Useful for logging. No action is required for these messages. | */ + MAV_SEVERITY_DEBUG=7, /* Useful non-operational messages that can assist in debugging. These should not occur during normal operation. | */ + MAV_SEVERITY_ENUM_END=8, /* | */ } MAV_SEVERITY; #endif @@ -222,12 +206,12 @@ typedef enum MAV_SEVERITY #define HAVE_ENUM_MAV_VTOL_STATE typedef enum MAV_VTOL_STATE { - MAV_VTOL_STATE_UNDEFINED=0, /* MAV is not configured as VTOL | */ - MAV_VTOL_STATE_TRANSITION_TO_FW=1, /* VTOL is in transition from multicopter to fixed-wing | */ - MAV_VTOL_STATE_TRANSITION_TO_MC=2, /* VTOL is in transition from fixed-wing to multicopter | */ - MAV_VTOL_STATE_MC=3, /* VTOL is in multicopter state | */ - MAV_VTOL_STATE_FW=4, /* VTOL is in fixed-wing state | */ - MAV_VTOL_STATE_ENUM_END=5, /* | */ + MAV_VTOL_STATE_UNDEFINED=0, /* MAV is not configured as VTOL | */ + MAV_VTOL_STATE_TRANSITION_TO_FW=1, /* VTOL is in transition from multicopter to fixed-wing | */ + MAV_VTOL_STATE_TRANSITION_TO_MC=2, /* VTOL is in transition from fixed-wing to multicopter | */ + MAV_VTOL_STATE_MC=3, /* VTOL is in multicopter state | */ + MAV_VTOL_STATE_FW=4, /* VTOL is in fixed-wing state | */ + MAV_VTOL_STATE_ENUM_END=5, /* | */ } MAV_VTOL_STATE; #endif @@ -236,14 +220,12 @@ typedef enum MAV_VTOL_STATE #define HAVE_ENUM_MAV_COMPONENT typedef enum MAV_COMPONENT { - MAV_COMP_ID_ALL=0, /* | */ - MAV_COMP_ID_ROSFLIGHT_FIRMWARE=250, /* Rosflight Firmware ID | */ - MAV_COMPONENT_ENUM_END=251, /* | */ + MAV_COMP_ID_ALL=0, /* | */ + MAV_COMP_ID_ROSFLIGHT_FIRMWARE=250, /* Rosflight Firmware ID | */ + MAV_COMPONENT_ENUM_END=251, /* | */ } MAV_COMPONENT; #endif - - // MAVLINK VERSION #ifndef MAVLINK_VERSION @@ -282,6 +264,18 @@ typedef enum MAV_COMPONENT #include "./mavlink_msg_timesync.h" #include "./mavlink_msg_statustext.h" +// base include + + + +#if MAVLINK_ROSFLIGHT_XML_HASH == MAVLINK_PRIMARY_XML_HASH +# define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, {"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_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {"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_ATTITUDE_QUATERNION, {"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_RC_CHANNELS, {"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_TIMESYNC, {"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}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OFFBOARD_CONTROL, MAVLINK_MESSAGE_INFO_SMALL_IMU, MAVLINK_MESSAGE_INFO_SMALL_MAG, MAVLINK_MESSAGE_INFO_SMALL_BARO, MAVLINK_MESSAGE_INFO_DIFF_PRESSURE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SMALL_RANGE, MAVLINK_MESSAGE_INFO_ROSFLIGHT_CMD, MAVLINK_MESSAGE_INFO_ROSFLIGHT_CMD_ACK, MAVLINK_MESSAGE_INFO_ROSFLIGHT_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_ROSFLIGHT_STATUS, MAVLINK_MESSAGE_INFO_ROSFLIGHT_VERSION, MAVLINK_MESSAGE_INFO_ROSFLIGHT_AUX_CMD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_EXTERNAL_ATTITUDE, MAVLINK_MESSAGE_INFO_ROSFLIGHT_HARD_ERROR, MAVLINK_MESSAGE_INFO_ROSFLIGHT_GNSS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROSFLIGHT_BATTERY_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}}}, MAVLINK_MESSAGE_INFO_STATUSTEXT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +# define MAVLINK_MESSAGE_NAMES {{ "ATTITUDE_QUATERNION", 31 }, { "DIFF_PRESSURE", 184 }, { "EXTERNAL_ATTITUDE", 195 }, { "HEARTBEAT", 0 }, { "OFFBOARD_CONTROL", 180 }, { "PARAM_REQUEST_LIST", 21 }, { "PARAM_REQUEST_READ", 20 }, { "PARAM_SET", 23 }, { "PARAM_VALUE", 22 }, { "RC_CHANNELS", 65 }, { "ROSFLIGHT_AUX_CMD", 193 }, { "ROSFLIGHT_BATTERY_STATUS", 199 }, { "ROSFLIGHT_CMD", 188 }, { "ROSFLIGHT_CMD_ACK", 189 }, { "ROSFLIGHT_GNSS", 197 }, { "ROSFLIGHT_HARD_ERROR", 196 }, { "ROSFLIGHT_OUTPUT_RAW", 190 }, { "ROSFLIGHT_STATUS", 191 }, { "ROSFLIGHT_VERSION", 192 }, { "SMALL_BARO", 183 }, { "SMALL_IMU", 181 }, { "SMALL_MAG", 182 }, { "SMALL_RANGE", 187 }, { "STATUSTEXT", 253 }, { "TIMESYNC", 111 }} +# if MAVLINK_COMMAND_24BIT +# include "../mavlink_get_info.h" +# endif +#endif + #ifdef __cplusplus } #endif // __cplusplus diff --git a/comms/mavlink/v1.0/rosflight/testsuite.h b/comms/mavlink/v1.0/rosflight/testsuite.h index 18ea14c7..780d6934 100644 --- a/comms/mavlink/v1.0/rosflight/testsuite.h +++ b/comms/mavlink/v1.0/rosflight/testsuite.h @@ -1,7 +1,8 @@ /** @file - * @brief MAVLink comm protocol testsuite generated from rosflight.xml - * @see http://qgroundcontrol.org/mavlink/ + * @brief MAVLink comm protocol testsuite generated from rosflight.xml + * @see https://mavlink.io/en/ */ +#pragma once #ifndef ROSFLIGHT_TESTSUITE_H #define ROSFLIGHT_TESTSUITE_H @@ -17,7 +18,7 @@ static void mavlink_test_rosflight(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_rosflight(system_id, component_id, last_msg); + mavlink_test_rosflight(system_id, component_id, last_msg); } #endif @@ -26,1205 +27,1605 @@ static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_me static void mavlink_test_offboard_control(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { - mavlink_message_t 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_OFFBOARD_CONTROL >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_offboard_control_t packet_in = { - 17.0,45.0,73.0,101.0,129.0,157.0,77,144 + mavlink_offboard_control_t packet_in = { + 17.0,45.0,73.0,101.0,129.0,157.0,77,144 }; - mavlink_offboard_control_t packet1, packet2; + mavlink_offboard_control_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.Qx = packet_in.Qx; - packet1.Qy = packet_in.Qy; - packet1.Qz = packet_in.Qz; - packet1.Fx = packet_in.Fx; - packet1.Fy = packet_in.Fy; - packet1.Fz = packet_in.Fz; - packet1.mode = packet_in.mode; - packet1.ignore = packet_in.ignore; - - - + packet1.Qx = packet_in.Qx; + packet1.Qy = packet_in.Qy; + packet1.Qz = packet_in.Qz; + packet1.Fx = packet_in.Fx; + packet1.Fy = packet_in.Fy; + packet1.Fz = packet_in.Fz; + packet1.mode = packet_in.mode; + packet1.ignore = packet_in.ignore; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OFFBOARD_CONTROL_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OFFBOARD_CONTROL_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_offboard_control_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_offboard_control_decode(&msg, &packet2); + mavlink_msg_offboard_control_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_offboard_control_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_offboard_control_pack(system_id, component_id, &msg , packet1.mode , packet1.ignore , packet1.Qx , packet1.Qy , packet1.Qz , packet1.Fx , packet1.Fy , packet1.Fz ); - mavlink_msg_offboard_control_decode(&msg, &packet2); + mavlink_msg_offboard_control_pack(system_id, component_id, &msg , packet1.mode , packet1.ignore , packet1.Qx , packet1.Qy , packet1.Qz , packet1.Fx , packet1.Fy , packet1.Fz ); + mavlink_msg_offboard_control_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_offboard_control_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.mode , packet1.ignore , packet1.Qx , packet1.Qy , packet1.Qz , packet1.Fx , packet1.Fy , packet1.Fz ); - mavlink_msg_offboard_control_decode(&msg, &packet2); + mavlink_msg_offboard_control_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.mode , packet1.ignore , packet1.Qx , packet1.Qy , packet1.Qz , packet1.Fx , packet1.Fy , packet1.Fz ); + mavlink_msg_offboard_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_SMALL_IMU >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_small_imu_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,241.0 + mavlink_small_imu_t packet_in = { + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,241.0 }; - mavlink_small_imu_t packet1, packet2; + mavlink_small_imu_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.time_boot_us = packet_in.time_boot_us; - packet1.xacc = packet_in.xacc; - packet1.yacc = packet_in.yacc; - packet1.zacc = packet_in.zacc; - packet1.xgyro = packet_in.xgyro; - packet1.ygyro = packet_in.ygyro; - packet1.zgyro = packet_in.zgyro; - packet1.temperature = packet_in.temperature; - - - + packet1.time_boot_us = packet_in.time_boot_us; + packet1.xacc = packet_in.xacc; + packet1.yacc = packet_in.yacc; + packet1.zacc = packet_in.zacc; + packet1.xgyro = packet_in.xgyro; + packet1.ygyro = packet_in.ygyro; + packet1.zgyro = packet_in.zgyro; + 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_SMALL_IMU_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_SMALL_IMU_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_small_imu_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_small_imu_decode(&msg, &packet2); + mavlink_msg_small_imu_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_small_imu_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_small_imu_pack(system_id, component_id, &msg , packet1.time_boot_us , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.temperature ); - mavlink_msg_small_imu_decode(&msg, &packet2); + mavlink_msg_small_imu_pack(system_id, component_id, &msg , packet1.time_boot_us , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.temperature ); + mavlink_msg_small_imu_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_small_imu_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_us , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.temperature ); - mavlink_msg_small_imu_decode(&msg, &packet2); + mavlink_msg_small_imu_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_us , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.temperature ); + mavlink_msg_small_imu_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_SMALL_MAG >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_small_mag_t packet_in = { - 17.0,45.0,73.0 + mavlink_small_mag_t packet_in = { + 17.0,45.0,73.0 }; - mavlink_small_mag_t packet1, packet2; + mavlink_small_mag_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.xmag = packet_in.xmag; - packet1.ymag = packet_in.ymag; - packet1.zmag = packet_in.zmag; + packet1.xmag = packet_in.xmag; + packet1.ymag = packet_in.ymag; + packet1.zmag = packet_in.zmag; - +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_SMALL_MAG_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_SMALL_MAG_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_small_mag_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_small_mag_decode(&msg, &packet2); + mavlink_msg_small_mag_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_small_mag_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_small_mag_pack(system_id, component_id, &msg , packet1.xmag , packet1.ymag , packet1.zmag ); - mavlink_msg_small_mag_decode(&msg, &packet2); + mavlink_msg_small_mag_pack(system_id, component_id, &msg , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_small_mag_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_small_mag_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.xmag , packet1.ymag , packet1.zmag ); - mavlink_msg_small_mag_decode(&msg, &packet2); + mavlink_msg_small_mag_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_small_mag_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_SMALL_BARO >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_small_baro_t packet_in = { - 17.0,45.0,73.0 + mavlink_small_baro_t packet_in = { + 17.0,45.0,73.0 }; - mavlink_small_baro_t packet1, packet2; + mavlink_small_baro_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.altitude = packet_in.altitude; - packet1.pressure = packet_in.pressure; - packet1.temperature = packet_in.temperature; + packet1.altitude = packet_in.altitude; + packet1.pressure = packet_in.pressure; + 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_SMALL_BARO_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_SMALL_BARO_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_small_baro_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_small_baro_decode(&msg, &packet2); + mavlink_msg_small_baro_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_small_baro_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_small_baro_pack(system_id, component_id, &msg , packet1.altitude , packet1.pressure , packet1.temperature ); - mavlink_msg_small_baro_decode(&msg, &packet2); + mavlink_msg_small_baro_pack(system_id, component_id, &msg , packet1.altitude , packet1.pressure , packet1.temperature ); + mavlink_msg_small_baro_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_small_baro_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.altitude , packet1.pressure , packet1.temperature ); - mavlink_msg_small_baro_decode(&msg, &packet2); + mavlink_msg_small_baro_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.altitude , packet1.pressure , packet1.temperature ); + mavlink_msg_small_baro_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_DIFF_PRESSURE >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_diff_pressure_t packet_in = { - 17.0,45.0,73.0 + mavlink_diff_pressure_t packet_in = { + 17.0,45.0,73.0 }; - mavlink_diff_pressure_t packet1, packet2; + mavlink_diff_pressure_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.velocity = packet_in.velocity; - packet1.diff_pressure = packet_in.diff_pressure; - packet1.temperature = packet_in.temperature; + packet1.velocity = packet_in.velocity; + packet1.diff_pressure = packet_in.diff_pressure; + 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_DIFF_PRESSURE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_DIFF_PRESSURE_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_diff_pressure_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_diff_pressure_decode(&msg, &packet2); + mavlink_msg_diff_pressure_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_diff_pressure_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_diff_pressure_pack(system_id, component_id, &msg , packet1.velocity , packet1.diff_pressure , packet1.temperature ); - mavlink_msg_diff_pressure_decode(&msg, &packet2); + mavlink_msg_diff_pressure_pack(system_id, component_id, &msg , packet1.velocity , packet1.diff_pressure , packet1.temperature ); + mavlink_msg_diff_pressure_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_diff_pressure_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.velocity , packet1.diff_pressure , packet1.temperature ); - mavlink_msg_diff_pressure_decode(&msg, &packet2); + mavlink_msg_diff_pressure_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.velocity , packet1.diff_pressure , packet1.temperature ); + mavlink_msg_diff_pressure_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_SMALL_RANGE >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_small_range_t packet_in = { - 17.0,45.0,73.0,41 + mavlink_small_range_t packet_in = { + 17.0,45.0,73.0,41 }; - mavlink_small_range_t packet1, packet2; + mavlink_small_range_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.range = packet_in.range; - packet1.max_range = packet_in.max_range; - packet1.min_range = packet_in.min_range; - packet1.type = packet_in.type; + packet1.range = packet_in.range; + packet1.max_range = packet_in.max_range; + packet1.min_range = packet_in.min_range; + packet1.type = packet_in.type; - +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_SMALL_RANGE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_SMALL_RANGE_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_small_range_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_small_range_decode(&msg, &packet2); + mavlink_msg_small_range_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_small_range_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_small_range_pack(system_id, component_id, &msg , packet1.type , packet1.range , packet1.max_range , packet1.min_range ); - mavlink_msg_small_range_decode(&msg, &packet2); + mavlink_msg_small_range_pack(system_id, component_id, &msg , packet1.type , packet1.range , packet1.max_range , packet1.min_range ); + mavlink_msg_small_range_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_small_range_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.range , packet1.max_range , packet1.min_range ); - mavlink_msg_small_range_decode(&msg, &packet2); + mavlink_msg_small_range_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.range , packet1.max_range , packet1.min_range ); + mavlink_msg_small_range_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_ROSFLIGHT_CMD >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_rosflight_cmd_t packet_in = { - 5 + mavlink_rosflight_cmd_t packet_in = { + 5 }; - mavlink_rosflight_cmd_t packet1, packet2; + mavlink_rosflight_cmd_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.command = packet_in.command; + packet1.command = packet_in.command; - +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ROSFLIGHT_CMD_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ROSFLIGHT_CMD_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_cmd_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_rosflight_cmd_decode(&msg, &packet2); + mavlink_msg_rosflight_cmd_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_rosflight_cmd_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_cmd_pack(system_id, component_id, &msg , packet1.command ); - mavlink_msg_rosflight_cmd_decode(&msg, &packet2); + mavlink_msg_rosflight_cmd_pack(system_id, component_id, &msg , packet1.command ); + mavlink_msg_rosflight_cmd_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_cmd_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.command ); - mavlink_msg_rosflight_cmd_decode(&msg, &packet2); + mavlink_msg_rosflight_cmd_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.command ); + mavlink_msg_rosflight_cmd_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_ROSFLIGHT_CMD_ACK >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_rosflight_cmd_ack_t packet_in = { - 5,72 + mavlink_rosflight_cmd_ack_t packet_in = { + 5,72 }; - mavlink_rosflight_cmd_ack_t packet1, packet2; + mavlink_rosflight_cmd_ack_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.command = packet_in.command; - packet1.success = packet_in.success; + packet1.command = packet_in.command; + packet1.success = packet_in.success; - +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ROSFLIGHT_CMD_ACK_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_cmd_ack_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_rosflight_cmd_ack_decode(&msg, &packet2); + mavlink_msg_rosflight_cmd_ack_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_rosflight_cmd_ack_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_cmd_ack_pack(system_id, component_id, &msg , packet1.command , packet1.success ); - mavlink_msg_rosflight_cmd_ack_decode(&msg, &packet2); + mavlink_msg_rosflight_cmd_ack_pack(system_id, component_id, &msg , packet1.command , packet1.success ); + mavlink_msg_rosflight_cmd_ack_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_cmd_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.command , packet1.success ); - mavlink_msg_rosflight_cmd_ack_decode(&msg, &packet2); + mavlink_msg_rosflight_cmd_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.command , packet1.success ); + mavlink_msg_rosflight_cmd_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_ROSFLIGHT_OUTPUT_RAW >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_rosflight_output_raw_t packet_in = { - 93372036854775807ULL,{ 73.0, 74.0, 75.0, 76.0, 77.0, 78.0, 79.0, 80.0, 81.0, 82.0, 83.0, 84.0, 85.0, 86.0 } + mavlink_rosflight_output_raw_t packet_in = { + 93372036854775807ULL,{ 73.0, 74.0, 75.0, 76.0, 77.0, 78.0, 79.0, 80.0, 81.0, 82.0, 83.0, 84.0, 85.0, 86.0 } }; - mavlink_rosflight_output_raw_t packet1, packet2; + mavlink_rosflight_output_raw_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.stamp = packet_in.stamp; + packet1.stamp = packet_in.stamp; - mav_array_memcpy(packet1.values, packet_in.values, sizeof(float)*14); + mav_array_memcpy(packet1.values, packet_in.values, sizeof(float)*14); - +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ROSFLIGHT_OUTPUT_RAW_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_output_raw_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_rosflight_output_raw_decode(&msg, &packet2); + mavlink_msg_rosflight_output_raw_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_rosflight_output_raw_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_output_raw_pack(system_id, component_id, &msg , packet1.stamp , packet1.values ); - mavlink_msg_rosflight_output_raw_decode(&msg, &packet2); + mavlink_msg_rosflight_output_raw_pack(system_id, component_id, &msg , packet1.stamp , packet1.values ); + mavlink_msg_rosflight_output_raw_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_output_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.stamp , packet1.values ); - mavlink_msg_rosflight_output_raw_decode(&msg, &packet2); + mavlink_msg_rosflight_output_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.stamp , packet1.values ); + mavlink_msg_rosflight_output_raw_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_ROSFLIGHT_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_rosflight_status_t packet_in = { - 17235,17339,17,84,151,218,29,96 + mavlink_rosflight_status_t packet_in = { + 17235,17339,17,84,151,218,29,96 }; - mavlink_rosflight_status_t packet1, packet2; + mavlink_rosflight_status_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.num_errors = packet_in.num_errors; - packet1.loop_time_us = packet_in.loop_time_us; - packet1.armed = packet_in.armed; - packet1.failsafe = packet_in.failsafe; - packet1.rc_override = packet_in.rc_override; - packet1.offboard = packet_in.offboard; - packet1.error_code = packet_in.error_code; - packet1.control_mode = packet_in.control_mode; - - - + packet1.num_errors = packet_in.num_errors; + packet1.loop_time_us = packet_in.loop_time_us; + packet1.armed = packet_in.armed; + packet1.failsafe = packet_in.failsafe; + packet1.rc_override = packet_in.rc_override; + packet1.offboard = packet_in.offboard; + packet1.error_code = packet_in.error_code; + packet1.control_mode = packet_in.control_mode; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ROSFLIGHT_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ROSFLIGHT_STATUS_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_status_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_rosflight_status_decode(&msg, &packet2); + mavlink_msg_rosflight_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_rosflight_status_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_status_pack(system_id, component_id, &msg , packet1.armed , packet1.failsafe , packet1.rc_override , packet1.offboard , packet1.error_code , packet1.control_mode , packet1.num_errors , packet1.loop_time_us ); - mavlink_msg_rosflight_status_decode(&msg, &packet2); + mavlink_msg_rosflight_status_pack(system_id, component_id, &msg , packet1.armed , packet1.failsafe , packet1.rc_override , packet1.offboard , packet1.error_code , packet1.control_mode , packet1.num_errors , packet1.loop_time_us ); + mavlink_msg_rosflight_status_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.armed , packet1.failsafe , packet1.rc_override , packet1.offboard , packet1.error_code , packet1.control_mode , packet1.num_errors , packet1.loop_time_us ); - mavlink_msg_rosflight_status_decode(&msg, &packet2); + mavlink_msg_rosflight_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.armed , packet1.failsafe , packet1.rc_override , packet1.offboard , packet1.error_code , packet1.control_mode , packet1.num_errors , packet1.loop_time_us ); + mavlink_msg_rosflight_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_ROSFLIGHT_VERSION >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_rosflight_version_t packet_in = { - "ABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVW" + mavlink_rosflight_version_t packet_in = { + "ABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVW" }; - mavlink_rosflight_version_t packet1, packet2; + mavlink_rosflight_version_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - mav_array_memcpy(packet1.version, packet_in.version, sizeof(char)*50); + mav_array_memcpy(packet1.version, packet_in.version, sizeof(char)*50); - +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ROSFLIGHT_VERSION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ROSFLIGHT_VERSION_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_version_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_rosflight_version_decode(&msg, &packet2); + mavlink_msg_rosflight_version_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_rosflight_version_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_version_pack(system_id, component_id, &msg , packet1.version ); - mavlink_msg_rosflight_version_decode(&msg, &packet2); + mavlink_msg_rosflight_version_pack(system_id, component_id, &msg , packet1.version ); + mavlink_msg_rosflight_version_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_version_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.version ); - mavlink_msg_rosflight_version_decode(&msg, &packet2); + mavlink_msg_rosflight_version_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.version ); + mavlink_msg_rosflight_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; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_rosflight_aux_cmd_t packet_in = { - { 17.0, 18.0, 19.0, 20.0, 21.0, 22.0, 23.0, 24.0, 25.0, 26.0, 27.0, 28.0, 29.0, 30.0 },{ 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186 } + mavlink_rosflight_aux_cmd_t packet_in = { + { 17.0, 18.0, 19.0, 20.0, 21.0, 22.0, 23.0, 24.0, 25.0, 26.0, 27.0, 28.0, 29.0, 30.0 },{ 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186 } }; - mavlink_rosflight_aux_cmd_t packet1, packet2; + mavlink_rosflight_aux_cmd_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - mav_array_memcpy(packet1.aux_cmd_array, packet_in.aux_cmd_array, sizeof(float)*14); - mav_array_memcpy(packet1.type_array, packet_in.type_array, sizeof(uint8_t)*14); + mav_array_memcpy(packet1.aux_cmd_array, packet_in.aux_cmd_array, sizeof(float)*14); + mav_array_memcpy(packet1.type_array, packet_in.type_array, sizeof(uint8_t)*14); - +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_aux_cmd_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_rosflight_aux_cmd_decode(&msg, &packet2); + mavlink_msg_rosflight_aux_cmd_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_rosflight_aux_cmd_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_aux_cmd_pack(system_id, component_id, &msg , packet1.type_array , packet1.aux_cmd_array ); - mavlink_msg_rosflight_aux_cmd_decode(&msg, &packet2); + mavlink_msg_rosflight_aux_cmd_pack(system_id, component_id, &msg , packet1.type_array , packet1.aux_cmd_array ); + mavlink_msg_rosflight_aux_cmd_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_aux_cmd_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type_array , packet1.aux_cmd_array ); - mavlink_msg_rosflight_aux_cmd_decode(&msg, &packet2); + mavlink_msg_rosflight_aux_cmd_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type_array , packet1.aux_cmd_array ); + mavlink_msg_rosflight_aux_cmd_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_EXTERNAL_ATTITUDE >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_external_attitude_t packet_in = { - 17.0,45.0,73.0,101.0 + mavlink_external_attitude_t packet_in = { + 17.0,45.0,73.0,101.0 }; - mavlink_external_attitude_t packet1, packet2; + mavlink_external_attitude_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.qw = packet_in.qw; - packet1.qx = packet_in.qx; - packet1.qy = packet_in.qy; - packet1.qz = packet_in.qz; + packet1.qw = packet_in.qw; + packet1.qx = packet_in.qx; + packet1.qy = packet_in.qy; + packet1.qz = packet_in.qz; - +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_EXTERNAL_ATTITUDE_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_external_attitude_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_external_attitude_decode(&msg, &packet2); + mavlink_msg_external_attitude_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_external_attitude_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_external_attitude_pack(system_id, component_id, &msg , packet1.qw , packet1.qx , packet1.qy , packet1.qz ); - mavlink_msg_external_attitude_decode(&msg, &packet2); + mavlink_msg_external_attitude_pack(system_id, component_id, &msg , packet1.qw , packet1.qx , packet1.qy , packet1.qz ); + mavlink_msg_external_attitude_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_external_attitude_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.qw , packet1.qx , packet1.qy , packet1.qz ); - mavlink_msg_external_attitude_decode(&msg, &packet2); + mavlink_msg_external_attitude_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.qw , packet1.qx , packet1.qy , packet1.qz ); + mavlink_msg_external_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_ROSFLIGHT_HARD_ERROR >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_rosflight_hard_error_t packet_in = { - 963497464,963497672,963497880,963498088 + mavlink_rosflight_hard_error_t packet_in = { + 963497464,963497672,963497880,963498088 }; - mavlink_rosflight_hard_error_t packet1, packet2; + mavlink_rosflight_hard_error_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.error_code = packet_in.error_code; - packet1.pc = packet_in.pc; - packet1.reset_count = packet_in.reset_count; - packet1.doRearm = packet_in.doRearm; + packet1.error_code = packet_in.error_code; + packet1.pc = packet_in.pc; + packet1.reset_count = packet_in.reset_count; + packet1.doRearm = packet_in.doRearm; - +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ROSFLIGHT_HARD_ERROR_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_hard_error_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_rosflight_hard_error_decode(&msg, &packet2); + mavlink_msg_rosflight_hard_error_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_rosflight_hard_error_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_hard_error_pack(system_id, component_id, &msg , packet1.error_code , packet1.pc , packet1.reset_count , packet1.doRearm ); - mavlink_msg_rosflight_hard_error_decode(&msg, &packet2); + mavlink_msg_rosflight_hard_error_pack(system_id, component_id, &msg , packet1.error_code , packet1.pc , packet1.reset_count , packet1.doRearm ); + mavlink_msg_rosflight_hard_error_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_hard_error_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.error_code , packet1.pc , packet1.reset_count , packet1.doRearm ); - mavlink_msg_rosflight_hard_error_decode(&msg, &packet2); + mavlink_msg_rosflight_hard_error_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.error_code , packet1.pc , packet1.reset_count , packet1.doRearm ); + mavlink_msg_rosflight_hard_error_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_ROSFLIGHT_GNSS >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_rosflight_gnss_t packet_in = { - 93372036854775807LL,179.0,235.0,93372036854777319ULL,963499128,269.0,297.0,325.0,353.0,381.0,409.0,437.0,197,8 + mavlink_rosflight_gnss_t packet_in = { + 93372036854775807LL,179.0,235.0,93372036854777319ULL,963499128,269.0,297.0,325.0,353.0,381.0,409.0,437.0,197,8 }; - mavlink_rosflight_gnss_t packet1, packet2; + mavlink_rosflight_gnss_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.seconds = packet_in.seconds; - packet1.lat = packet_in.lat; - packet1.lon = packet_in.lon; - packet1.rosflight_timestamp = packet_in.rosflight_timestamp; - packet1.nanos = packet_in.nanos; - packet1.height = packet_in.height; - packet1.vel_n = packet_in.vel_n; - packet1.vel_e = packet_in.vel_e; - packet1.vel_d = packet_in.vel_d; - packet1.h_acc = packet_in.h_acc; - packet1.v_acc = packet_in.v_acc; - packet1.s_acc = packet_in.s_acc; - packet1.fix_type = packet_in.fix_type; - packet1.num_sat = packet_in.num_sat; - - - + packet1.seconds = packet_in.seconds; + packet1.lat = packet_in.lat; + packet1.lon = packet_in.lon; + packet1.rosflight_timestamp = packet_in.rosflight_timestamp; + packet1.nanos = packet_in.nanos; + packet1.height = packet_in.height; + packet1.vel_n = packet_in.vel_n; + packet1.vel_e = packet_in.vel_e; + packet1.vel_d = packet_in.vel_d; + packet1.h_acc = packet_in.h_acc; + packet1.v_acc = packet_in.v_acc; + packet1.s_acc = packet_in.s_acc; + packet1.fix_type = packet_in.fix_type; + packet1.num_sat = packet_in.num_sat; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ROSFLIGHT_GNSS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ROSFLIGHT_GNSS_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_gnss_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_rosflight_gnss_decode(&msg, &packet2); + mavlink_msg_rosflight_gnss_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_rosflight_gnss_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_gnss_pack(system_id, component_id, &msg , packet1.seconds , packet1.nanos , packet1.fix_type , packet1.num_sat , packet1.lat , packet1.lon , packet1.height , packet1.vel_n , packet1.vel_e , packet1.vel_d , packet1.h_acc , packet1.v_acc , packet1.s_acc , packet1.rosflight_timestamp ); - mavlink_msg_rosflight_gnss_decode(&msg, &packet2); + mavlink_msg_rosflight_gnss_pack(system_id, component_id, &msg , packet1.seconds , packet1.nanos , packet1.fix_type , packet1.num_sat , packet1.lat , packet1.lon , packet1.height , packet1.vel_n , packet1.vel_e , packet1.vel_d , packet1.h_acc , packet1.v_acc , packet1.s_acc , packet1.rosflight_timestamp ); + mavlink_msg_rosflight_gnss_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_gnss_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.seconds , packet1.nanos , packet1.fix_type , packet1.num_sat , packet1.lat , packet1.lon , packet1.height , packet1.vel_n , packet1.vel_e , packet1.vel_d , packet1.h_acc , packet1.v_acc , packet1.s_acc , packet1.rosflight_timestamp ); - mavlink_msg_rosflight_gnss_decode(&msg, &packet2); + mavlink_msg_rosflight_gnss_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.seconds , packet1.nanos , packet1.fix_type , packet1.num_sat , packet1.lat , packet1.lon , packet1.height , packet1.vel_n , packet1.vel_e , packet1.vel_d , packet1.h_acc , packet1.v_acc , packet1.s_acc , packet1.rosflight_timestamp ); + mavlink_msg_rosflight_gnss_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_ROSFLIGHT_BATTERY_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_rosflight_battery_status_t packet_in = { - 17.0,45.0 + mavlink_rosflight_battery_status_t packet_in = { + 17.0,45.0 }; - mavlink_rosflight_battery_status_t packet1, packet2; + mavlink_rosflight_battery_status_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.battery_voltage = packet_in.battery_voltage; - packet1.battery_current = packet_in.battery_current; + packet1.battery_voltage = packet_in.battery_voltage; + packet1.battery_current = packet_in.battery_current; - +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ROSFLIGHT_BATTERY_STATUS_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_battery_status_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_rosflight_battery_status_decode(&msg, &packet2); + mavlink_msg_rosflight_battery_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_rosflight_battery_status_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_battery_status_pack(system_id, component_id, &msg , packet1.battery_voltage , packet1.battery_current ); - mavlink_msg_rosflight_battery_status_decode(&msg, &packet2); + mavlink_msg_rosflight_battery_status_pack(system_id, component_id, &msg , packet1.battery_voltage , packet1.battery_current ); + mavlink_msg_rosflight_battery_status_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rosflight_battery_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.battery_voltage , packet1.battery_current ); - mavlink_msg_rosflight_battery_status_decode(&msg, &packet2); + mavlink_msg_rosflight_battery_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.battery_voltage , packet1.battery_current ); + mavlink_msg_rosflight_battery_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_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,2 + mavlink_heartbeat_t packet_in = { + 963497464,17,84,151,218,2 }; - mavlink_heartbeat_t packet1, packet2; + 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; - - - + 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_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_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_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_PARAM_REQUEST_READ >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_param_request_read_t packet_in = { - 17235,139,206,"EFGHIJKLMNOPQRS" + mavlink_param_request_read_t packet_in = { + 17235,139,206,"EFGHIJKLMNOPQRS" }; - mavlink_param_request_read_t packet1, packet2; + mavlink_param_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; + 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); + 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_REQUEST_READ_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PARAM_REQUEST_READ_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_param_request_read_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_param_request_read_decode(&msg, &packet2); + mavlink_msg_param_request_read_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_request_read_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_param_request_read_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_index ); - mavlink_msg_param_request_read_decode(&msg, &packet2); + mavlink_msg_param_request_read_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_index ); + mavlink_msg_param_request_read_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_param_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_request_read_decode(&msg, &packet2); + mavlink_msg_param_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_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_REQUEST_LIST >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_param_request_list_t packet_in = { - 5,72 + mavlink_param_request_list_t packet_in = { + 5,72 }; - mavlink_param_request_list_t packet1, packet2; + mavlink_param_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.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_REQUEST_LIST_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PARAM_REQUEST_LIST_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_param_request_list_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_param_request_list_decode(&msg, &packet2); + mavlink_msg_param_request_list_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_request_list_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_param_request_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component ); - mavlink_msg_param_request_list_decode(&msg, &packet2); + mavlink_msg_param_request_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_param_request_list_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_param_request_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component ); - mavlink_msg_param_request_list_decode(&msg, &packet2); + mavlink_msg_param_request_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_param_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_VALUE >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_param_value_t packet_in = { - 17.0,17443,17547,"IJKLMNOPQRSTUVW",77 + mavlink_param_value_t packet_in = { + 17.0,17443,17547,"IJKLMNOPQRSTUVW",77 }; - mavlink_param_value_t packet1, packet2; + mavlink_param_value_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.param_value = packet_in.param_value; - packet1.param_count = packet_in.param_count; - packet1.param_index = packet_in.param_index; - packet1.param_type = packet_in.param_type; + packet1.param_value = packet_in.param_value; + 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_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_VALUE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PARAM_VALUE_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_param_value_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_param_value_decode(&msg, &packet2); + mavlink_msg_param_value_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_value_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_param_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_value_decode(&msg, &packet2); + mavlink_msg_param_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_value_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_param_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_value_decode(&msg, &packet2); + mavlink_msg_param_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_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_SET >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_param_set_t packet_in = { - 17.0,17,84,"GHIJKLMNOPQRSTU",199 + mavlink_param_set_t packet_in = { + 17.0,17,84,"GHIJKLMNOPQRSTU",199 }; - mavlink_param_set_t packet1, packet2; + mavlink_param_set_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.param_value = packet_in.param_value; - packet1.target_system = packet_in.target_system; - packet1.target_component = packet_in.target_component; - packet1.param_type = packet_in.param_type; + packet1.param_value = packet_in.param_value; + 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_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_SET_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PARAM_SET_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_param_set_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_param_set_decode(&msg, &packet2); + mavlink_msg_param_set_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_set_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_param_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_set_decode(&msg, &packet2); + mavlink_msg_param_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_set_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_param_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_set_decode(&msg, &packet2); + mavlink_msg_param_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_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_ATTITUDE_QUATERNION >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_attitude_quaternion_t packet_in = { - 963497464,45.0,73.0,101.0,129.0,157.0,185.0,213.0 + mavlink_attitude_quaternion_t packet_in = { + 963497464,45.0,73.0,101.0,129.0,157.0,185.0,213.0 }; - mavlink_attitude_quaternion_t packet1, packet2; + mavlink_attitude_quaternion_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.time_boot_ms = packet_in.time_boot_ms; - packet1.q1 = packet_in.q1; - packet1.q2 = packet_in.q2; - packet1.q3 = packet_in.q3; - packet1.q4 = packet_in.q4; - packet1.rollspeed = packet_in.rollspeed; - packet1.pitchspeed = packet_in.pitchspeed; - packet1.yawspeed = packet_in.yawspeed; - - - + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.q1 = packet_in.q1; + packet1.q2 = packet_in.q2; + packet1.q3 = packet_in.q3; + packet1.q4 = packet_in.q4; + packet1.rollspeed = packet_in.rollspeed; + packet1.pitchspeed = packet_in.pitchspeed; + packet1.yawspeed = packet_in.yawspeed; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_attitude_quaternion_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_attitude_quaternion_decode(&msg, &packet2); + mavlink_msg_attitude_quaternion_encode(system_id, component_id, &msg, &packet1); + 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(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_decode(&msg, &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_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_decode(&msg, &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_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_RC_CHANNELS >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_rc_channels_t packet_in = { - 963497464,17443,17547,17651,17755,17859,17963,18067,18171,18275,18379,18483,18587,18691,18795,18899,19003,19107,19211,125,192 + mavlink_rc_channels_t packet_in = { + 963497464,17443,17547,17651,17755,17859,17963,18067,18171,18275,18379,18483,18587,18691,18795,18899,19003,19107,19211,125,192 }; - mavlink_rc_channels_t packet1, packet2; + mavlink_rc_channels_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.time_boot_ms = packet_in.time_boot_ms; - packet1.chan1_raw = packet_in.chan1_raw; - packet1.chan2_raw = packet_in.chan2_raw; - packet1.chan3_raw = packet_in.chan3_raw; - packet1.chan4_raw = packet_in.chan4_raw; - packet1.chan5_raw = packet_in.chan5_raw; - packet1.chan6_raw = packet_in.chan6_raw; - packet1.chan7_raw = packet_in.chan7_raw; - packet1.chan8_raw = packet_in.chan8_raw; - 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; - packet1.chancount = packet_in.chancount; - packet1.rssi = packet_in.rssi; - - - + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.chan1_raw = packet_in.chan1_raw; + packet1.chan2_raw = packet_in.chan2_raw; + packet1.chan3_raw = packet_in.chan3_raw; + packet1.chan4_raw = packet_in.chan4_raw; + packet1.chan5_raw = packet_in.chan5_raw; + packet1.chan6_raw = packet_in.chan6_raw; + packet1.chan7_raw = packet_in.chan7_raw; + packet1.chan8_raw = packet_in.chan8_raw; + 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; + packet1.chancount = packet_in.chancount; + packet1.rssi = packet_in.rssi; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_RC_CHANNELS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_RC_CHANNELS_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rc_channels_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_rc_channels_decode(&msg, &packet2); + mavlink_msg_rc_channels_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_rc_channels_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rc_channels_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.chancount , 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 , packet1.rssi ); - mavlink_msg_rc_channels_decode(&msg, &packet2); + mavlink_msg_rc_channels_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.chancount , 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 , packet1.rssi ); + mavlink_msg_rc_channels_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rc_channels_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.chancount , 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 , packet1.rssi ); - mavlink_msg_rc_channels_decode(&msg, &packet2); + mavlink_msg_rc_channels_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.chancount , 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 , packet1.rssi ); + mavlink_msg_rc_channels_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_TIMESYNC >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_timesync_t packet_in = { - 93372036854775807LL,93372036854776311LL + mavlink_timesync_t packet_in = { + 93372036854775807LL,93372036854776311LL }; - mavlink_timesync_t packet1, packet2; + mavlink_timesync_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.tc1 = packet_in.tc1; - packet1.ts1 = packet_in.ts1; + packet1.tc1 = packet_in.tc1; + packet1.ts1 = packet_in.ts1; - +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_TIMESYNC_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_TIMESYNC_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_timesync_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_timesync_decode(&msg, &packet2); + mavlink_msg_timesync_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_timesync_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_timesync_pack(system_id, component_id, &msg , packet1.tc1 , packet1.ts1 ); - mavlink_msg_timesync_decode(&msg, &packet2); + mavlink_msg_timesync_pack(system_id, component_id, &msg , packet1.tc1 , packet1.ts1 ); + mavlink_msg_timesync_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_timesync_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.tc1 , packet1.ts1 ); - mavlink_msg_timesync_decode(&msg, &packet2); + mavlink_msg_timesync_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.tc1 , packet1.ts1 ); + mavlink_msg_timesync_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_STATUSTEXT >= 256) { + return; + } +#endif + mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_statustext_t packet_in = { - 5,"BCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWX" + mavlink_statustext_t packet_in = { + 5,"BCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWX" }; - mavlink_statustext_t packet1, packet2; + mavlink_statustext_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.severity = packet_in.severity; + packet1.severity = packet_in.severity; - mav_array_memcpy(packet1.text, packet_in.text, sizeof(char)*50); + mav_array_memcpy(packet1.text, packet_in.text, sizeof(char)*50); - +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN); + } +#endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_statustext_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_statustext_decode(&msg, &packet2); + mavlink_msg_statustext_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_statustext_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_statustext_pack(system_id, component_id, &msg , packet1.severity , packet1.text ); - mavlink_msg_statustext_decode(&msg, &packet2); + mavlink_msg_statustext_pack(system_id, component_id, &msg , packet1.severity , packet1.text ); + mavlink_msg_statustext_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_statustext_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.severity , packet1.text ); - mavlink_msg_statustext_decode(&msg, &packet2); + mavlink_msg_statustext_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.severity , packet1.text ); + mavlink_msg_statustext_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=PARAM_PRIMARY_MIXER_OUTPUT_0 )&&(param_id <=PARAM_PRIMARY_MIXER_5_9 )) { + load_primary_mixer_values(); + } else if ((param_id >=PARAM_SECONDARY_MIXER_0_0 )&&(param_id <=PARAM_SECONDARY_MIXER_5_9 )) { + load_secondary_mixer_values(); + } else switch (param_id) { case PARAM_PRIMARY_MIXER: case PARAM_SECONDARY_MIXER: init_mixing(); @@ -208,7 +77,6 @@ void Mixer::param_change_callback(uint16_t param_id) update_parameters(); break; case PARAM_MOTOR_PWM_SEND_RATE: - case PARAM_RC_TYPE: init_PWM(); break; default: @@ -244,8 +112,7 @@ void Mixer::init_mixing() } else if (mixer_choice != FIXEDWING && mixer_choice != INVERTED_VTAIL) { // Invert the selected "canned" matrix - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, - "Inverting selected mixing matrix..."); + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,"Inverting selected mixing matrix..."); primary_mixer_ = invert_mixer(array_of_mixers_[mixer_choice]); // Save the primary mixer values to the params @@ -324,7 +191,7 @@ void Mixer::init_mixing() RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "Inverting selected mixing matrix..."); - // Invert the secondary mixer + // Invert the selected mixer and copy to secondary mixer secondary_mixer_ = invert_mixer(array_of_mixers_[mixer_choice]); save_secondary_mixer_params(); } else { @@ -388,9 +255,8 @@ Mixer::mixer_t Mixer::invert_mixer(const mixer_t* mixer_to_invert) Eigen::Matrix mixer_matrix_pinv = svd.matrixV() * Sig * svd.matrixU().transpose(); - // Fill in the mixing matrix from the inverted matrix above mixer_t inverted_mixer; - + // Fill in the mixing matrix from the inverted matrix above for (int i = 0; i < NUM_MIXER_OUTPUTS; i++) { inverted_mixer.output_type[i] = mixer_to_invert->output_type[i]; inverted_mixer.default_pwm_rate[i] = mixer_to_invert->default_pwm_rate[i]; @@ -401,7 +267,6 @@ Mixer::mixer_t Mixer::invert_mixer(const mixer_t* mixer_to_invert) inverted_mixer.Qy[i] = mixer_matrix_pinv(i, 4); inverted_mixer.Qz[i] = mixer_matrix_pinv(i, 5); } - return inverted_mixer; } diff --git a/test/parameters_test.cpp b/test/parameters_test.cpp index a0f4a4b2..a9097464 100644 --- a/test/parameters_test.cpp +++ b/test/parameters_test.cpp @@ -7,8 +7,8 @@ using namespace rosflight_firmware; -#define EXPECT_PARAM_EQ_INT(id, value) EXPECT_EQ(value, rf.params_.get_param_int(id)) -#define EXPECT_PARAM_EQ_FLOAT(id, value) EXPECT_EQ(value, rf.params_.get_param_float(id)) +#define EXPECT_PARAM_EQ_INT(PARAM_id, value) EXPECT_EQ(value, rf.params_.get_param_int(PARAM_id)) +#define EXPECT_PARAM_EQ_FLOAT(PARAM_id, value) EXPECT_EQ(value, rf.params_.get_param_float(PARAM_id)) TEST(Parameters, DefaultParameters) { @@ -18,61 +18,206 @@ TEST(Parameters, DefaultParameters) rf.init(); - EXPECT_PARAM_EQ_INT(PARAM_BAUD_RATE, 921600); - EXPECT_PARAM_EQ_INT(PARAM_SYSTEM_ID, 1); - EXPECT_PARAM_EQ_FLOAT(PARAM_X_EQ_TORQUE, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_Y_EQ_TORQUE, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_Z_EQ_TORQUE, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_MOTOR_IDLE_THROTTLE, 0.1f); - EXPECT_PARAM_EQ_FLOAT(PARAM_FAILSAFE_THROTTLE, -1.0f); - EXPECT_PARAM_EQ_INT(PARAM_SPIN_MOTORS_WHEN_ARMED, 1); - EXPECT_PARAM_EQ_INT(PARAM_CALIBRATE_GYRO_ON_ARM, 0); - EXPECT_PARAM_EQ_FLOAT(PARAM_GYRO_X_BIAS, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_GYRO_Y_BIAS, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_GYRO_Z_BIAS, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_ACC_X_BIAS, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_ACC_Y_BIAS, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_ACC_Z_BIAS, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_ACC_X_TEMP_COMP, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_ACC_Y_TEMP_COMP, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_ACC_Z_TEMP_COMP, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_MAG_A11_COMP, 1.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_MAG_A12_COMP, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_MAG_A13_COMP, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_MAG_A21_COMP, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_MAG_A22_COMP, 1.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_MAG_A23_COMP, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_MAG_A31_COMP, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_MAG_A32_COMP, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_MAG_A33_COMP, 1.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_MAG_X_BIAS, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_MAG_Y_BIAS, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_MAG_Z_BIAS, 0.0f); - EXPECT_PARAM_EQ_FLOAT(PARAM_BARO_BIAS, 0.0f); - EXPECT_PARAM_EQ_INT(PARAM_RC_TYPE, 0); - EXPECT_PARAM_EQ_INT(PARAM_RC_X_CHANNEL, 0); - EXPECT_PARAM_EQ_INT(PARAM_RC_Y_CHANNEL, 1); - EXPECT_PARAM_EQ_INT(PARAM_RC_Z_CHANNEL, 3); - EXPECT_PARAM_EQ_INT(PARAM_RC_F_CHANNEL, 2); - EXPECT_PARAM_EQ_INT(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, 4); - EXPECT_PARAM_EQ_INT(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, 4); - EXPECT_PARAM_EQ_INT(PARAM_RC_ATT_CONTROL_TYPE_CHANNEL, -1); - EXPECT_PARAM_EQ_INT(PARAM_RC_ARM_CHANNEL, -1); - EXPECT_PARAM_EQ_INT(PARAM_RC_NUM_CHANNELS, 6); - EXPECT_PARAM_EQ_INT(PARAM_RC_SWITCH_5_DIRECTION, 1); - EXPECT_PARAM_EQ_INT(PARAM_RC_SWITCH_6_DIRECTION, 1); - EXPECT_PARAM_EQ_INT(PARAM_RC_SWITCH_7_DIRECTION, 1); - EXPECT_PARAM_EQ_INT(PARAM_RC_SWITCH_8_DIRECTION, 1); - EXPECT_PARAM_EQ_FLOAT(PARAM_RC_OVERRIDE_DEVIATION, 0.1f); - EXPECT_PARAM_EQ_INT(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, 1); - EXPECT_PARAM_EQ_INT(PARAM_RC_ATTITUDE_MODE, 1); - EXPECT_PARAM_EQ_INT(PARAM_PRIMARY_MIXER, Mixer::INVALID_MIXER); - EXPECT_PARAM_EQ_INT(PARAM_FIXED_WING, 0); - EXPECT_PARAM_EQ_INT(PARAM_AILERON_REVERSE, 0); - EXPECT_PARAM_EQ_INT(PARAM_ELEVATOR_REVERSE, 0); - EXPECT_PARAM_EQ_INT(PARAM_RUDDER_REVERSE, 0); - EXPECT_PARAM_EQ_INT(PARAM_FC_ROLL, 0); - EXPECT_PARAM_EQ_INT(PARAM_FC_PITCH, 0); - EXPECT_PARAM_EQ_INT(PARAM_FC_YAW, 0); - EXPECT_PARAM_EQ_FLOAT(PARAM_ARM_THRESHOLD, 0.15f); + EXPECT_PARAM_EQ_INT(PARAM_BAUD_RATE, 921600); // Baud rate of MAVlink communication with companion computer | 9600 | 921600 + EXPECT_PARAM_EQ_INT(PARAM_SERIAL_DEVICE, 0); // Serial Port (for supported devices) | 0 | 3 + EXPECT_PARAM_EQ_INT(PARAM_NUM_MOTORS, 4); // Number of vertical-facing motors on the vehicle | 1 | 8 + EXPECT_PARAM_EQ_FLOAT(PARAM_MOTOR_RESISTANCE, 0.042f); // Electrical resistance of the motor windings (ohms) | 0 | 1000.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_MOTOR_KV, 0.01706f); // Back emf constant of the motor in SI units (V/rad/s) | 0 | 1000.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_PROP_DIAMETER, 0.381f); // Diameter of the propeller in meters | 0 | 1.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_PROP_CT, 0.075f); // Thrust coefficient of the propeller | 0 | 100.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_PROP_CQ, 0.0045f); // Torque coefficient of the propeller | 0 | 100.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_VOLT_MAX, 25.0f); // Maximum voltage of the battery (V) | 0 | 100.0 + EXPECT_PARAM_EQ_INT(PARAM_PRIMARY_MIXER_OUTPUT_0, 0); // Output type of mixer output 0. | 0 | 1 | 2 | 3 + EXPECT_PARAM_EQ_INT(PARAM_PRIMARY_MIXER_OUTPUT_1, 0); // Output type of mixer output 1. | 0 | 1 | 2 | 3 + EXPECT_PARAM_EQ_INT(PARAM_PRIMARY_MIXER_OUTPUT_2, 0); // Output type of mixer output 2. | 0 | 1 | 2 | 3 + EXPECT_PARAM_EQ_INT(PARAM_PRIMARY_MIXER_OUTPUT_3, 0); // Output type of mixer output 3. | 0 | 1 | 2 | 3 + EXPECT_PARAM_EQ_INT(PARAM_PRIMARY_MIXER_OUTPUT_4, 0); // Output type of mixer output 4. | 0 | 1 | 2 | 3 + EXPECT_PARAM_EQ_INT(PARAM_PRIMARY_MIXER_OUTPUT_5, 0); // Output type of mixer output 5. | 0 | 1 | 2 | 3 + EXPECT_PARAM_EQ_INT(PARAM_PRIMARY_MIXER_OUTPUT_6, 0); // Output type of mixer output 6. | 0 | 1 | 2 | 3 + EXPECT_PARAM_EQ_INT(PARAM_PRIMARY_MIXER_OUTPUT_7, 0); // Output type of mixer output 7. | 0 | 1 | 2 | 3 + EXPECT_PARAM_EQ_INT(PARAM_PRIMARY_MIXER_OUTPUT_8, 0); // Output type of mixer output 8. | 0 | 1 | 2 | 3 + EXPECT_PARAM_EQ_INT(PARAM_PRIMARY_MIXER_OUTPUT_9, 0); // Output type of mixer output 9. | 0 | 1 | 2 | 3 + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_0_0, 0.0f); // Value of the custom mixer at element [0,0] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_1_0, 0.0f); // Value of the custom mixer at element [1,0] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_2_0, 0.0f); // Value of the custom mixer at element [2,0] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_3_0, 0.0f); // Value of the custom mixer at element [3,0] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_4_0, 0.0f); // Value of the custom mixer at element [4,0] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_5_0, 0.0f); // Value of the custom mixer at element [5,0] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_0_1, 0.0f); // Value of the custom mixer at element [0,1] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_1_1, 0.0f); // Value of the custom mixer at element [1,1] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_2_1, 0.0f); // Value of the custom mixer at element [2,1] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_3_1, 0.0f); // Value of the custom mixer at element [3,1] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_4_1, 0.0f); // Value of the custom mixer at element [4,1] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_5_1, 0.0f); // Value of the custom mixer at element [5,1] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_0_2, 0.0f); // Value of the custom mixer at element [0,2] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_1_2, 0.0f); // Value of the custom mixer at element [1,2] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_2_2, 0.0f); // Value of the custom mixer at element [2,2] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_3_2, 0.0f); // Value of the custom mixer at element [3,2] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_4_2, 0.0f); // Value of the custom mixer at element [4,2] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_5_2, 0.0f); // Value of the custom mixer at element [5,2] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_0_3, 0.0f); // Value of the custom mixer at element [0,3] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_1_3, 0.0f); // Value of the custom mixer at element [1,3] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_2_3, 0.0f); // Value of the custom mixer at element [2,3] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_3_3, 0.0f); // Value of the custom mixer at element [3,3] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_4_3, 0.0f); // Value of the custom mixer at element [4,3] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_5_3, 0.0f); // Value of the custom mixer at element [5,3] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_0_4, 0.0f); // Value of the custom mixer at element [0,4] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_1_4, 0.0f); // Value of the custom mixer at element [1,4] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_2_4, 0.0f); // Value of the custom mixer at element [2,4] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_3_4, 0.0f); // Value of the custom mixer at element [3,4] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_4_4, 0.0f); // Value of the custom mixer at element [4,4] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_5_4, 0.0f); // Value of the custom mixer at element [5,4] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_0_5, 0.0f); // Value of the custom mixer at element [0,5] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_1_5, 0.0f); // Value of the custom mixer at element [1,5] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_2_5, 0.0f); // Value of the custom mixer at element [2,5] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_3_5, 0.0f); // Value of the custom mixer at element [3,5] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_4_5, 0.0f); // Value of the custom mixer at element [4,5] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_5_5, 0.0f); // Value of the custom mixer at element [5,5] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_0_6, 0.0f); // Value of the custom mixer at element [0,6] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_1_6, 0.0f); // Value of the custom mixer at element [1,6] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_2_6, 0.0f); // Value of the custom mixer at element [2,6] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_3_6, 0.0f); // Value of the custom mixer at element [3,6] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_4_6, 0.0f); // Value of the custom mixer at element [4,6] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_5_6, 0.0f); // Value of the custom mixer at element [5,6] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_0_7, 0.0f); // Value of the custom mixer at element [0,7] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_1_7, 0.0f); // Value of the custom mixer at element [1,7] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_2_7, 0.0f); // Value of the custom mixer at element [2,7] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_3_7, 0.0f); // Value of the custom mixer at element [3,7] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_4_7, 0.0f); // Value of the custom mixer at element [4,7] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_5_7, 0.0f); // Value of the custom mixer at element [5,7] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_0_8, 0.0f); // Value of the custom mixer at element [0,8] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_1_8, 0.0f); // Value of the custom mixer at element [1,8] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_2_8, 0.0f); // Value of the custom mixer at element [2,8] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_3_8, 0.0f); // Value of the custom mixer at element [3,8] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_4_8, 0.0f); // Value of the custom mixer at element [4,8] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_5_8, 0.0f); // Value of the custom mixer at element [5,8] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_0_9, 0.0f); // Value of the custom mixer at element [0,9] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_1_9, 0.0f); // Value of the custom mixer at element [1,9] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_2_9, 0.0f); // Value of the custom mixer at element [2,9] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_3_9, 0.0f); // Value of the custom mixer at element [3,9] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_4_9, 0.0f); // Value of the custom mixer at element [4,9] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_PRIMARY_MIXER_5_9, 0.0f); // Value of the custom mixer at element [5,9] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_0_0, 0.0f); // Value of the custom mixer at element [0,0] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_1_0, 0.0f); // Value of the custom mixer at element [1,0] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_2_0, 0.0f); // Value of the custom mixer at element [2,0] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_3_0, 0.0f); // Value of the custom mixer at element [3,0] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_4_0, 0.0f); // Value of the custom mixer at element [4,0] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_5_0, 0.0f); // Value of the custom mixer at element [5,0] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_0_1, 0.0f); // Value of the custom mixer at element [0,1] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_1_1, 0.0f); // Value of the custom mixer at element [1,1] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_2_1, 0.0f); // Value of the custom mixer at element [2,1] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_3_1, 0.0f); // Value of the custom mixer at element [3,1] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_4_1, 0.0f); // Value of the custom mixer at element [4,1] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_5_1, 0.0f); // Value of the custom mixer at element [5,1] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_0_2, 0.0f); // Value of the custom mixer at element [0,2] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_1_2, 0.0f); // Value of the custom mixer at element [1,2] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_2_2, 0.0f); // Value of the custom mixer at element [2,2] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_3_2, 0.0f); // Value of the custom mixer at element [3,2] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_4_2, 0.0f); // Value of the custom mixer at element [4,2] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_5_2, 0.0f); // Value of the custom mixer at element [5,2] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_0_3, 0.0f); // Value of the custom mixer at element [0,3] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_1_3, 0.0f); // Value of the custom mixer at element [1,3] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_2_3, 0.0f); // Value of the custom mixer at element [2,3] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_3_3, 0.0f); // Value of the custom mixer at element [3,3] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_4_3, 0.0f); // Value of the custom mixer at element [4,3] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_5_3, 0.0f); // Value of the custom mixer at element [5,3] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_0_4, 0.0f); // Value of the custom mixer at element [0,4] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_1_4, 0.0f); // Value of the custom mixer at element [1,4] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_2_4, 0.0f); // Value of the custom mixer at element [2,4] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_3_4, 0.0f); // Value of the custom mixer at element [3,4] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_4_4, 0.0f); // Value of the custom mixer at element [4,4] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_5_4, 0.0f); // Value of the custom mixer at element [5,4] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_0_5, 0.0f); // Value of the custom mixer at element [0,5] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_1_5, 0.0f); // Value of the custom mixer at element [1,5] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_2_5, 0.0f); // Value of the custom mixer at element [2,5] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_3_5, 0.0f); // Value of the custom mixer at element [3,5] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_4_5, 0.0f); // Value of the custom mixer at element [4,5] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_5_5, 0.0f); // Value of the custom mixer at element [5,5] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_0_6, 0.0f); // Value of the custom mixer at element [0,6] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_1_6, 0.0f); // Value of the custom mixer at element [1,6] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_2_6, 0.0f); // Value of the custom mixer at element [2,6] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_3_6, 0.0f); // Value of the custom mixer at element [3,6] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_4_6, 0.0f); // Value of the custom mixer at element [4,6] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_5_6, 0.0f); // Value of the custom mixer at element [5,6] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_0_7, 0.0f); // Value of the custom mixer at element [0,7] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_1_7, 0.0f); // Value of the custom mixer at element [1,7] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_2_7, 0.0f); // Value of the custom mixer at element [2,7] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_3_7, 0.0f); // Value of the custom mixer at element [3,7] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_4_7, 0.0f); // Value of the custom mixer at element [4,7] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_5_7, 0.0f); // Value of the custom mixer at element [5,7] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_0_8, 0.0f); // Value of the custom mixer at element [0,8] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_1_8, 0.0f); // Value of the custom mixer at element [1,8] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_2_8, 0.0f); // Value of the custom mixer at element [2,8] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_3_8, 0.0f); // Value of the custom mixer at element [3,8] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_4_8, 0.0f); // Value of the custom mixer at element [4,8] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_5_8, 0.0f); // Value of the custom mixer at element [5,8] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_0_9, 0.0f); // Value of the custom mixer at element [0,9] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_1_9, 0.0f); // Value of the custom mixer at element [1,9] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_2_9, 0.0f); // Value of the custom mixer at element [2,9] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_3_9, 0.0f); // Value of the custom mixer at element [3,9] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_4_9, 0.0f); // Value of the custom mixer at element [4,9] | -inf | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_SECONDARY_MIXER_5_9, 0.0f); // Value of the custom mixer at element [5,9] | -inf | inf + EXPECT_PARAM_EQ_INT(PARAM_SYSTEM_ID, 1); // Mavlink System ID | 1 | 255 + EXPECT_PARAM_EQ_FLOAT(PARAM_PID_ROLL_RATE_P, 0.070f); // Roll Rate Proportional Gain | 0.0 | 1000.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_PID_ROLL_RATE_I, 0.000f); // Roll Rate Integral Gain | 0.0 | 1000.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_PID_ROLL_RATE_D, 0.000f); // Roll Rate Derivative Gain | 0.0 | 1000.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_PID_PITCH_RATE_I, 0.0000f); // Pitch Rate Integral Gain | 0.0 | 1000.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_PID_PITCH_RATE_D, 0.0000f); // Pitch Rate Derivative Gain | 0.0 | 1000.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_PID_PITCH_ANGLE_D, 0.05f); // Pitch Angle Derivative Gain | 0.0 | 1000.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_X_EQ_TORQUE, 0.0f); // Equilibrium torque added to output of controller on x axis | -1.0 | 1.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_Y_EQ_TORQUE, 0.0f); // Equilibrium torque added to output of controller on y axis | -1.0 | 1.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_Z_EQ_TORQUE, 0.0f); // Equilibrium torque added to output of controller on z axis | -1.0 | 1.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_PID_TAU, 0.05f); // Dirty Derivative time constant - See controller documentation | 0.0 | 1.0 + EXPECT_PARAM_EQ_INT(PARAM_MOTOR_PWM_SEND_RATE, 0); // Overrides default PWM rate specified by mixer if non-zero - Requires reboot to take effect | 0 | 490 + EXPECT_PARAM_EQ_INT(PARAM_SPIN_MOTORS_WHEN_ARMED, true); // Enforce MOTOR_IDLE_THR | 0 | 1 + EXPECT_PARAM_EQ_INT(PARAM_INIT_TIME, 3000); // Time in ms to initialize estimator | 0 | 100000 + EXPECT_PARAM_EQ_FLOAT(PARAM_FILTER_KP_ACC, 0.5f); // estimator proportional gain on accel-based error - See estimator documentation | 0 | 10.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_FILTER_KI, 0.01f); // estimator integral gain - See estimator documentation | 0 | 1.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_FILTER_KP_EXT, 1.5f); // estimator proportional gain on external attitude-based error - See estimator documentation | 0 | 10.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_FILTER_ACCEL_MARGIN, 0.1f); // allowable accel norm margin around 1g to determine if accel is usable | 0 | 1.0 + EXPECT_PARAM_EQ_INT(PARAM_FILTER_USE_QUAD_INT, 1); // Perform a quadratic averaging of LPF gyro data prior to integration (adds ~20 us to estimation loop on F1 processors) | 0 | 1 + EXPECT_PARAM_EQ_INT(PARAM_FILTER_USE_MAT_EXP, 1); // 1 - Use matrix exponential to improve gyro integration (adds ~90 us to estimation loop in F1 processors) 0 - use euler integration | 0 | 1 + EXPECT_PARAM_EQ_INT(PARAM_CALIBRATE_GYRO_ON_ARM, false); // True if desired to calibrate gyros on arm | 0 | 1 + EXPECT_PARAM_EQ_FLOAT(PARAM_GYRO_XY_ALPHA, 0.3f); // Low-pass filter constant on gyro X and Y axes - See estimator documentation | 0 | 1.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_GYRO_Z_ALPHA, 0.3f); // Low-pass filter constant on gyro Z axis - See estimator documentation | 0 | 1.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_ACC_ALPHA, 0.5f); // Low-pass filter constant on all accel axes - See estimator documentation | 0 | 1.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_GYRO_X_BIAS, 0.0f); // Constant x-bias of gyroscope readings | -1.0 | 1.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_GYRO_Y_BIAS, 0.0f); // Constant y-bias of gyroscope readings | -1.0 | 1.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_GYRO_Z_BIAS, 0.0f); // Constant z-bias of gyroscope readings | -1.0 | 1.0 + EXPECT_PARAM_EQ_FLOAT(PARAM_BARO_BIAS, 0.0f); // Barometer measurement bias (Pa) | 0 | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_GROUND_LEVEL, 1387.0f); // Altitude of ground level (m) | -1000 | 10000 + EXPECT_PARAM_EQ_FLOAT(PARAM_DIFF_PRESS_BIAS, 0.0f); // Differential Pressure Bias (Pa) | -10 | 10 + EXPECT_PARAM_EQ_INT(PARAM_RC_TYPE, 0); // Type of RC input 0 - PPM, 1 - SBUS | 0 | 1 + EXPECT_PARAM_EQ_INT(PARAM_RC_X_CHANNEL, 0); // RC input channel mapped to x-axis commands [0 - indexed] | 0 | 3 + EXPECT_PARAM_EQ_INT(PARAM_RC_Y_CHANNEL, 1); // RC input channel mapped to y-axis commands [0 - indexed] | 0 | 3 + EXPECT_PARAM_EQ_INT(PARAM_RC_Z_CHANNEL, 3); // RC input channel mapped to z-axis commands [0 - indexed] | 0 | 3 + EXPECT_PARAM_EQ_INT(PARAM_RC_F_CHANNEL, 2); // RC input channel mapped to F-axis commands [0 - indexed] | 0 | 3 + EXPECT_PARAM_EQ_INT(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, 4); // RC switch mapped to attitude override [0 indexed, -1 to disable] | 4 | 7 + EXPECT_PARAM_EQ_INT(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, 4); // RC switch channel mapped to throttle override [0 indexed, -1 to disable] | 4 | 7 + EXPECT_PARAM_EQ_INT(PARAM_RC_NUM_CHANNELS, 6); // number of RC input channels | 1 | 8 + EXPECT_PARAM_EQ_INT(PARAM_RC_SWITCH_5_DIRECTION, 1); // RC switch 5 toggle direction | -1 | 1 + EXPECT_PARAM_EQ_INT(PARAM_RC_SWITCH_6_DIRECTION, 1); // RC switch 6 toggle direction | -1 | 1 + EXPECT_PARAM_EQ_INT(PARAM_RC_SWITCH_7_DIRECTION, 1); // RC switch 7 toggle direction | -1 | 1 + EXPECT_PARAM_EQ_INT(PARAM_RC_SWITCH_8_DIRECTION, 1); // RC switch 8 toggle direction | -1 | 1 + EXPECT_PARAM_EQ_INT(PARAM_OVERRIDE_LAG_TIME, 1000); // RC stick deviation lag time before returning control (ms) | 0 | 100000 + EXPECT_PARAM_EQ_INT(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true); // Take minimum throttle between RC and computer at all times | 0 | 1 + EXPECT_PARAM_EQ_FLOAT(PARAM_RC_MAX_THROTTLE, 0.7f); // Maximum throttle command sent by full deflection of RC sticks, to maintain controllability during aggressive maneuvers | 0.0 | 1.0 + EXPECT_PARAM_EQ_INT(PARAM_RC_ATTITUDE_MODE, 1); // Attitude mode for RC sticks (0: rate, 1: angle). Overridden if RC_ATT_CTRL_CHN is set. | 0 | 1 + EXPECT_PARAM_EQ_FLOAT(PARAM_RC_MAX_ROLL, 0.786f); // Maximum roll angle command sent by full deflection of RC sticks | 0.0 | 3.14159 + EXPECT_PARAM_EQ_FLOAT(PARAM_RC_MAX_PITCH, 0.786f); // Maximum pitch angle command sent by full stick deflection of RC sticks | 0.0 | 3.14159 + EXPECT_PARAM_EQ_FLOAT(PARAM_RC_MAX_ROLLRATE, 3.14159f); // Maximum roll rate command sent by full stick deflection of RC sticks | 0.0 | 9.42477796077 + EXPECT_PARAM_EQ_FLOAT(PARAM_RC_MAX_PITCHRATE, 3.14159f); // Maximum pitch command sent by full stick deflection of RC sticks | 0.0 | 3.14159 + EXPECT_PARAM_EQ_FLOAT(PARAM_RC_MAX_YAWRATE, 1.507f); // Maximum pitch command sent by full stick deflection of RC sticks | 0.0 | 3.14159 + EXPECT_PARAM_EQ_INT(PARAM_PRIMARY_MIXER, Mixer::INVALID_MIXER); // Which mixer to choose for primary mixer - See Mixer documentation | 0 | 11 + EXPECT_PARAM_EQ_INT(PARAM_SECONDARY_MIXER, Mixer::INVALID_MIXER); // Which mixer to choose for secondary mixer - See Mixer documentation | 0 | 11 + EXPECT_PARAM_EQ_INT(PARAM_FIXED_WING, false); // switches on pass-through commands for fixed-wing operation | 0 | 1 + EXPECT_PARAM_EQ_INT(PARAM_ELEVATOR_REVERSE, 0); // reverses elevator servo output | 0 | 1 + EXPECT_PARAM_EQ_INT(PARAM_AILERON_REVERSE, 0); // reverses aileron servo output | 0 | 1 + EXPECT_PARAM_EQ_INT(PARAM_RUDDER_REVERSE, 0); // reverses rudder servo output | 0 | 1 + EXPECT_PARAM_EQ_FLOAT(PARAM_FC_ROLL, 0.0f); // roll angle (deg) of flight controller wrt aircraft body | 0 | 360 + EXPECT_PARAM_EQ_FLOAT(PARAM_FC_PITCH, 0.0f); // pitch angle (deg) of flight controller wrt aircraft body | 0 | 360 + EXPECT_PARAM_EQ_FLOAT(PARAM_FC_YAW, 0.0f); // yaw angle (deg) of flight controller wrt aircraft body | 0 | 360 + EXPECT_PARAM_EQ_FLOAT(PARAM_BATTERY_VOLTAGE_MULTIPLIER, 0.0f); // Multiplier for the voltage sensor | 0 | inf + EXPECT_PARAM_EQ_FLOAT(PARAM_BATTERY_CURRENT_MULTIPLIER, 0.0f); // Multiplier for the current sensor | 0 | inf + EXPECT_PARAM_EQ_INT(PARAM_OFFBOARD_TIMEOUT, 100); // Timeout in milliseconds for offboard commands, after which RC override is activated | 0 | 100000 } diff --git a/unit_test.sh b/unit_test.sh new file mode 100644 index 00000000..062473c0 --- /dev/null +++ b/unit_test.sh @@ -0,0 +1,67 @@ +#!/bin/bash + +# Building the projects: +# mkdir varmint_build && cd varmint_build && cmake .. -DBOARD_TO_BUILD=varmint && make -j +# mkdir pixracer_pro_build && cd pixracer_pro_build && cmake .. -DBOARD_TO_BUILD=pixracer_pro && make -j +# +# alternate for make -j above is cmake --build . -j (note only one .) +# make -j OR cmake --build . -j +# to clean: +# make clean -j OR make --build . --target clean -j +# +# unit test: +# build: +# mkdir test_build && cd test_build && cmake .. -DBOARD_TO_BUILD=test -DCMAKE_BUILD_TYPE=Release && make -j +# test: +# ./test/unit_tests + +#!/bin/bash +#function echo_red { echo -e "\033[1;31m$@\033[0m"; } +#function echo_green { echo -e "\033[1;32m$@\033[0m"; } +#function echo_blue { echo -e "\033[1;34m$@\033[0m"; } + +#EXIT_CODE=0 + +#function print_result() { +# if [ $1 -eq 0 ]; then +# echo_green "[Passed]" +# else +# echo_red "[Failed]" +# EXIT_CODE=1 +# fi +# echo "" +#} + +rm CMakeCache.txt + +# Save the current directory +START_DIR=$(pwd) + +# Create the build directory if it doesn't exist +mkdir -p build + +# Empty build/test directory if it exists, otherwise create it +if [ -d "build/test" ]; then + sudo rm -rf build/test/* +else + mkdir -p build/test +fi + +echo_blue "Test 1: Build test suite" +# Navigate to build/test directory +cd build/test + +# Run cmake with specified parameters and build project in Release mode +cmake ../.. -DBOARD_TO_BUILD=test -DCMAKE_BUILD_TYPE=Release && make -j +#print_result $? + +echo_blue "Test 2: Run test suite" +# Run unit tests +./test/unit_tests +#print_result $? + +# Return to starting directory +cd "$START_DIR" + + +